uUpdate color

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);
+            }
         }
     }
 }

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();

src/RobotAssemblerBody.cpp

@@ -17,6 +17,8 @@
 #include <cnoid/SpotLight>
 #include <cnoid/DeviceList>
 
+#include "ValueTreeUtil.h"
+
 #include <cnoid/UTF8>
 #include <cnoid/stdx/filesystem>
 
@@ -312,9 +314,9 @@ bool RoboasmBodyCreator::createLinks(const RoboasmPartsPtr _pt, bool _is_root, s
         //
         map_link_cnoid_roboasm.insert(std::pair<std::string, std::string>(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)

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);
             }

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);