From 3007ce3979aa03ad72fd0bb9f697332cad717e57 Mon Sep 17 00:00:00 2001 From: Yohei Kakiuchi Date: Thu, 16 Jul 2026 19:39:50 +0900 Subject: [PATCH 1/2] update color of connecting points --- src/AssemblerManager.cpp | 6 +++++- src/AssemblerManager.h | 2 +- src/RobotAssemblerHelper.cpp | 35 ++++++++++++++++++++++++++--------- src/RobotAssemblerHelper.h | 5 ++++- 4 files changed, 36 insertions(+), 12 deletions(-) diff --git a/src/AssemblerManager.cpp b/src/AssemblerManager.cpp index 822da5d..5e2fa31 100644 --- a/src/AssemblerManager.cpp +++ b/src/AssemblerManager.cpp @@ -419,7 +419,11 @@ void AssemblerManager::clearAllPoints() for(auto it = srobot_set.begin(); it != srobot_set.end(); it++) { auto pit_end = (*it)->spoint_set.end(); for(auto pit = (*it)->spoint_set.begin(); pit != pit_end; pit++) { - (*pit)->changeState(ra::RASceneConnectingPoint::DEFAULT); + if ( (*pit)->point()->isConnected() ) { + (*pit)->changeState(ra::RASceneConnectingPoint::USED); + } else { + (*pit)->changeState(ra::RASceneConnectingPoint::DEFAULT); + } } } } diff --git a/src/AssemblerManager.h b/src/AssemblerManager.h index 74d967c..266a5a2 100644 --- a/src/AssemblerManager.h +++ b/src/AssemblerManager.h @@ -52,7 +52,7 @@ class CNOID_EXPORT AssemblerManager : public SceneWidgetEventHandler void updateConnectingPoints(); void updateMatchedPoints(ra::RASceneConnectingPoint *_pt, bool clearSelf = true, ra::RASceneConnectingPoint::Clicked clearState = ra::RASceneConnectingPoint::DEFAULT, - ra::RASceneConnectingPoint::Clicked matchedState = ra::RASceneConnectingPoint::CAN_CONNECT1); + ra::RASceneConnectingPoint::Clicked matchedState = ra::RASceneConnectingPoint::CAN_CONNECT0); void searchMatchedPoints(double threshold); void clearAllPoints(); void updateRobots(); diff --git a/src/RobotAssemblerHelper.cpp b/src/RobotAssemblerHelper.cpp index 05adc50..e5b298b 100644 --- a/src/RobotAssemblerHelper.cpp +++ b/src/RobotAssemblerHelper.cpp @@ -15,16 +15,19 @@ using namespace cnoid::robot_assembler; namespace { -static const Vector3f color_default(0.3f, 0.3f, 0.6f); -static const Vector3f color_good0(0.33f, 1.0f, 0.0f); +static const Vector3f color_default(0.3f, 0.3f, 0.6f); // dark purple +static const Vector3f color_used(0.15f, 0.15f, 0.30f); // dark +static const Vector3f color_good0(0.33f, 1.0f, 0.0f); // light green //static const Vector3f color_good1(0.0f, 1.0f, 0.33f);// -static const Vector3f color_good1(0.0f, 0.5f, 0.166f); -static const Vector3f color_bad0(1.0f, 0.0f, 0.0f); +static const Vector3f color_good1(0.0f, 0.5f, 0.166f); // dark green +static const Vector3f color_bad0(1.0f, 0.0f, 0.0f); // red //static const Vector3f color_bad1(1.0f, 0.0f, 0.0f); -static const Vector3f color_bad1(0.5f, 0.0f, 0.166f); -static const Vector3f color_can_connect0(0.0f, 1.0f, 1.0f); -static const Vector3f color_can_connect1(0.0f, 1.0f, 1.0f); +static const Vector3f color_bad1(0.5f, 0.0f, 0.166f); // dark red +static const Vector3f color_can_connect0(0.0f, 1.0f, 1.0f); // cyan +static const Vector3f color_can_connect1(0.15f, 0.65f, 0.60f); // dark cyan static const Vector3f color_selected(0.5f, 0.0f, 0.5f); +static const Vector3f color_pair0a(1.1f, 1.0f, 0.0f); // light yellow +static const Vector3f color_pair0b(0.65f, 0.65f, 0.15f); // dark yellow } #define SCP_LENGTH_LONG 0.015 @@ -175,6 +178,11 @@ void RASceneConnectingPoint::changeState(RASceneConnectingPoint::Clicked _clk) material->setDiffuseColor(color_default); } break; + case USED: + { + material->setDiffuseColor(color_used); + } + break; case SELECT_GOOD0: { material->setDiffuseColor(color_good0); @@ -205,6 +213,16 @@ void RASceneConnectingPoint::changeState(RASceneConnectingPoint::Clicked _clk) material->setDiffuseColor(color_can_connect1); } break; + case SELECT_PAIR0A: + { + material->setDiffuseColor(color_pair0a); + } + break; + case SELECT_PAIR0B: + { + material->setDiffuseColor(color_pair0b); + } + break; } } // @@ -250,8 +268,7 @@ RASceneParts::RASceneParts(RoboasmPartsPtr _p, const std::string &_proj_dir) _p->worldcoords().transformation(newtrans, ptr->worldcoords()); newtrans.toPosition(cp->position()); // add color - Vector3f col(1.0f, 1.0f, 0); - cp->material->setDiffuseColor(col); + cp->material->setDiffuseColor(color_used); partsScene->addChild(cp); spoint_list.push_back(cp); } diff --git a/src/RobotAssemblerHelper.h b/src/RobotAssemblerHelper.h index e28a5da..ed6b7a8 100644 --- a/src/RobotAssemblerHelper.h +++ b/src/RobotAssemblerHelper.h @@ -26,7 +26,9 @@ class CNOID_EXPORT RASceneConnectingPoint : public RASceneBase { public: enum Clicked { + NOT_CONNECT = 0, DEFAULT, + USED, SELECT_GOOD0, SELECT_GOOD1, SELECT_BAD0, @@ -35,7 +37,8 @@ class CNOID_EXPORT RASceneConnectingPoint : public RASceneBase CAN_CONNECT1, CAN_CONNECT, SELECTED, - NOT_CONNECT + SELECT_PAIR0A, + SELECT_PAIR0B, }; RASceneConnectingPoint() = delete; RASceneConnectingPoint(RoboasmConnectingPointPtr _c); From b611e5d5efb36eae229d5a0ef6c0b3bb71801cec Mon Sep 17 00:00:00 2001 From: Yohei Kakiuchi Date: Thu, 16 Jul 2026 19:31:13 +0900 Subject: [PATCH 2/2] [RobotAssemblerBody] export some information to .body --- src/RobotAssemblerBody.cpp | 34 ++++++++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/src/RobotAssemblerBody.cpp b/src/RobotAssemblerBody.cpp index 4356794..16da1e2 100644 --- a/src/RobotAssemblerBody.cpp +++ b/src/RobotAssemblerBody.cpp @@ -17,6 +17,8 @@ #include #include +#include "ValueTreeUtil.h" + #include #include @@ -312,9 +314,9 @@ bool RoboasmBodyCreator::createLinks(const RoboasmPartsPtr _pt, bool _is_root, s // map_link_cnoid_roboasm.insert(std::pair(nm_, _pt->name())); } else { // usual (has parent) link - RoboasmPartsPtr p_pt_; - RoboasmConnectingPointPtr p_cp_; - RoboasmConnectingPointPtr s_cp_; + RoboasmPartsPtr p_pt_; // parent parts + RoboasmConnectingPointPtr p_cp_; // parent point + RoboasmConnectingPointPtr s_cp_; // self point if(!_pt->parentParts(p_pt_, p_cp_, s_cp_)) { ERROR_STREAM(" miss fetch : " << _pt->name()); delete lk; @@ -658,6 +660,10 @@ bool RoboasmBodyCreator::createLinks(const RoboasmPartsPtr _pt, bool _is_root, s } } } + // info + { + addCoordsToMapping(lk->info(), "link_origin_to_self", link_origin_to_self_); + } res.push_back(lk); return true; } @@ -887,7 +893,27 @@ bool cnoid::robot_assembler::mergeLink(Link *plink, Link *clink, BodyPtr _bd) } } } - + //merge-info: + // clink->name(): + // merge-info: clink->info->mergeinfo + // offset_from_parent: + { + Mapping *mi_ = plink->info()->findMapping("merge-info"); + if ( !mi_->isValid() ) { + mi_ = new Mapping(); + plink->info()->insert("merge-info", mi_); + } + MappingPtr cinfo = new Mapping(); + Mapping *cmi_ = clink->info()->findMapping("merge-info"); + if ( cmi_->isValid() ) { + cinfo->insert("merge-info", cmi_); + } + // add offset to cinfo + coordinates cds_Tb(clink->Tb()); + addCoordsToMapping(cinfo, "offset_from_parent", cds_Tb); + // + mi_->insert(clink->name(), cinfo); + } return true; } bool cnoid::robot_assembler::mergeFixedJoint(BodyPtr _bd)