Support robot controller axis manufacturer code

docs/source/changelog.rst

@@ -6,6 +6,8 @@ ChangeLog
 Version 0.142.0
 ===============
 
+- Add robotControllerAxisManufacturerCode so that servo drives from different manufacturer connected to daisy chain can be handled.
+- Fix unbounded growth of _vmimic
 * Add ViewerBase::SetUserText to customize HUD text size
 
 Version 0.141.2

include/openrave/kinbody.h

@@ -157,7 +157,8 @@ class OPENRAVE_API JointControlInfo_RobotController : public InfoBase
     boost::array<int16_t, 3> robotControllerAxisIndex = {-1, -1, -1}; ///< indicates which DOF in the robot controller controls which joint axis (up to the DOF of the joint). -1 if not specified/not valid.
     boost::array<dReal, 3> robotControllerAxisMult = {1.0, 1.0, 1.0}; ///< indicates the multiplier to convert the joint values from the environment units to the robot controller units, per joint axis. (valueInRobotControllerUnits - robotControllerAxisOffset) / robotControllerAxisMult = valueInEnvUnits
     boost::array<dReal, 3> robotControllerAxisOffset = {0.0, 0.0, 0.0}; ///< indicates the offset, in the robot controller units, that should be applied to the joint values, per joint axis.
-    boost::array<std::string, 3> robotControllerAxisProductCode = {"", "", ""}; ///< indicates the product codes of the servo devices per joint axis, in order for the robot controller to validate communication with the servo devices. it is different from ElectricMotorActuatorInfo::model_type, which is the name of the motor type attached to the servo devices.
+    boost::array<std::string, 3> robotControllerAxisManufacturerCode = {"", "", ""}; ///< indicates the manufacturer (vendor) codes of the servo devices per joint axis, in order for the robot controller to validate communication with the servo devices.
+    boost::array<std::string, 3> robotControllerAxisProductCode = {"", "", ""}; ///< indicates the product codes of the servo devices per joint axis, in order for the robot controller to validate communication with the servo devices. This is usually determined by each manufacturer. Manufacturer code and product code together makes a globally unique identification in the protocol. This is different from ElectricMotorActuatorInfo::model_type, which is the name of the motor type attached to the servo devices.
 };
 typedef boost::shared_ptr<JointControlInfo_RobotController> JointControlInfo_RobotControllerPtr;
 

python/bindings/include/openravepy/openravepy_jointinfo.h

@@ -186,6 +186,7 @@ class PyJointControlInfo_RobotController
     object robotControllerAxisIndex;
     object robotControllerAxisMult;
     object robotControllerAxisOffset;
+    py::list robotControllerAxisManufacturerCode;
     py::list robotControllerAxisProductCode;
 };
 typedef OPENRAVE_SHARED_PTR<PyJointControlInfo_RobotController> PyJointControlInfo_RobotControllerPtr;

python/bindings/openravepy_kinbody.cpp

@@ -827,6 +827,7 @@ PyJointControlInfo_RobotController::PyJointControlInfo_RobotController()
     robotControllerAxisIndex = py::cast(std::array<int16_t, 3>({-1, -1, -1}));
     robotControllerAxisMult = toPyVector3(Vector(1.0, 1.0, 1.0));
     robotControllerAxisOffset = toPyVector3(Vector(0.0, 0.0, 0.0));
+    robotControllerAxisManufacturerCode = py::cast(std::array<std::string, 3>({"", "", ""}));
     robotControllerAxisProductCode = py::cast(std::array<std::string, 3>({"", "", ""}));
 }
 
@@ -836,6 +837,7 @@ PyJointControlInfo_RobotController::PyJointControlInfo_RobotController(const Joi
     robotControllerAxisIndex = py::cast(std::array<int16_t, 3>({jci.robotControllerAxisIndex[0], jci.robotControllerAxisIndex[1], jci.robotControllerAxisIndex[2]}));
     robotControllerAxisMult = toPyVector3(Vector(jci.robotControllerAxisMult[0], jci.robotControllerAxisMult[1], jci.robotControllerAxisMult[2]));
     robotControllerAxisOffset = toPyVector3(Vector(jci.robotControllerAxisOffset[0], jci.robotControllerAxisOffset[1], jci.robotControllerAxisOffset[2]));
+    robotControllerAxisManufacturerCode = py::cast(std::array<std::string, 3>({jci.robotControllerAxisManufacturerCode[0], jci.robotControllerAxisManufacturerCode[1], jci.robotControllerAxisManufacturerCode[2]}));
     robotControllerAxisProductCode = py::cast(std::array<std::string, 3>({jci.robotControllerAxisProductCode[0], jci.robotControllerAxisProductCode[1], jci.robotControllerAxisProductCode[2]}));
 }
 
@@ -865,6 +867,13 @@ JointControlInfo_RobotControllerPtr PyJointControlInfo_RobotController::GetJoint
             info.robotControllerAxisOffset[i] = py::extract<dReal>(robotControllerAxisOffset[py::to_object(i)]);
         }
     }
+    if( !IS_PYTHONOBJECT_NONE(robotControllerAxisManufacturerCode) ) {
+        size_t num = len(robotControllerAxisManufacturerCode);
+        OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisManufacturerCode.size(), ORE_InvalidState);
+        for( size_t i = 0; i < num; ++i ) {
+            info.robotControllerAxisManufacturerCode[i] = py::extract<std::string>(robotControllerAxisManufacturerCode[i]);
+        }
+    }
     if( !IS_PYTHONOBJECT_NONE(robotControllerAxisProductCode) ) {
         size_t num = len(robotControllerAxisProductCode);
         OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisProductCode.size(), ORE_InvalidState);
@@ -1100,17 +1109,21 @@ void PyJointInfo::_Update(const KinBody::JointInfo& info) {
     _vupperlimit = toPyArray<dReal,3>(info._vupperlimit);
     // TODO
     // _trajfollow = py::to_object(toPyTrajectory(info._trajfollow, ?env?));
-    FOREACHC(itmimic, info._vmimic) {
-        if( !*itmimic ) {
-            _vmimic.append(py::none_());
-        }
-        else {
-            py::list oequations;
-            FOREACHC(itequation, (*itmimic)->_equations) {
-                oequations.append(*itequation);
+    {
+        py::list vmimic;
+        FOREACHC(itmimic, info._vmimic) {
+            if( !*itmimic ) {
+                vmimic.append(py::none_());
+            }
+            else {
+                py::list oequations;
+                FOREACHC(itequation, (*itmimic)->_equations) {
+                    oequations.append(*itequation);
+                }
+                vmimic.append(oequations);
             }
-            _vmimic.append(oequations);
         }
+        _vmimic = vmimic;
     }
     FOREACHC(it, info._mapFloatParameters) {
         _mapFloatParameters[it->first.c_str()] = toPyArray(it->second);
@@ -4693,14 +4706,15 @@ class JointControlInfo_RobotController_pickle_suite
 public:
     static py::tuple getstate(const PyJointControlInfo_RobotController& r)
     {
-        return py::make_tuple(r.controllerType, r.robotControllerAxisIndex, r.robotControllerAxisMult, r.robotControllerAxisOffset, r.robotControllerAxisProductCode);
+        return py::make_tuple(r.controllerType, r.robotControllerAxisIndex, r.robotControllerAxisMult, r.robotControllerAxisOffset, r.robotControllerAxisManufacturerCode, r.robotControllerAxisProductCode);
     }
     static void setstate(PyJointControlInfo_RobotController& r, py::tuple state) {
         r.controllerType = py::extract<int>(state[0]);
         r.robotControllerAxisIndex = state[1];
         r.robotControllerAxisMult = state[2];
         r.robotControllerAxisOffset = state[3];
-        r.robotControllerAxisProductCode = state[4];
+        r.robotControllerAxisManufacturerCode = state[4];
+        r.robotControllerAxisProductCode = state[5];
     }
 };
 
@@ -5293,6 +5307,7 @@ void init_openravepy_kinbody()
         .def_readwrite("robotControllerAxisIndex", &PyJointControlInfo_RobotController::robotControllerAxisIndex)
         .def_readwrite("robotControllerAxisMult", &PyJointControlInfo_RobotController::robotControllerAxisMult)
         .def_readwrite("robotControllerAxisOffset", &PyJointControlInfo_RobotController::robotControllerAxisOffset)
+        .def_readwrite("robotControllerAxisManufacturerCode", &PyJointControlInfo_RobotController::robotControllerAxisManufacturerCode)
         .def_readwrite("robotControllerAxisProductCode", &PyJointControlInfo_RobotController::robotControllerAxisProductCode)
 #ifdef USE_PYBIND11_PYTHON_BINDINGS
         .def(py::pickle(

src/libopenrave-core/colladaparser/colladareader.cpp

@@ -1772,6 +1772,9 @@ class ColladaReader : public daeErrorHandler
                                             else if( pchild->getElementName() == std::string("robotControllerAxisOffset") ) {
                                                 jci.robotControllerAxisOffset.at(ijointaxis) = boost::lexical_cast<dReal>(pchild->getCharData());
                                             }
+                                            else if( pchild->getElementName() == std::string("robotControllerAxisManufacturerCode") ) {
+                                                jci.robotControllerAxisManufacturerCode.at(ijointaxis) = std::string(pchild->getCharData());
+                                            }
                                             else if( pchild->getElementName() == std::string("robotControllerAxisProductCode") ) {
                                                 jci.robotControllerAxisProductCode.at(ijointaxis) = std::string(pchild->getCharData());
                                             }

src/libopenrave-core/colladaparser/colladawriter.cpp

@@ -1720,6 +1720,9 @@ class ColladaWriter : public daeErrorHandler
                             daeElementRef param_robotControllerAxisOffset = param_jointcontrolinfo_robotcontroller->add("robotControllerAxisOffset");
                             param_robotControllerAxisOffset->setAttribute("axis", boost::lexical_cast<std::string>(iaxis).c_str());
                             param_robotControllerAxisOffset->setCharData(boost::lexical_cast<std::string>(pjoint->_info._jci_robotcontroller->robotControllerAxisOffset[iaxis]).c_str());
+                            daeElementRef param_robotControllerAxisManufacturerCode = param_jointcontrolinfo_robotcontroller->add("robotControllerAxisManufacturerCode");
+                            param_robotControllerAxisManufacturerCode->setAttribute("axis", boost::lexical_cast<std::string>(iaxis).c_str());
+                            param_robotControllerAxisManufacturerCode->setCharData(pjoint->_info._jci_robotcontroller->robotControllerAxisManufacturerCode[iaxis].c_str());
                             daeElementRef param_robotControllerAxisProductCode = param_jointcontrolinfo_robotcontroller->add("robotControllerAxisProductCode");
                             param_robotControllerAxisProductCode->setAttribute("axis", boost::lexical_cast<std::string>(iaxis).c_str());
                             param_robotControllerAxisProductCode->setCharData(pjoint->_info._jci_robotcontroller->robotControllerAxisProductCode[iaxis].c_str());

src/libopenrave/kinbodyjoint.cpp

@@ -108,6 +108,7 @@ void JointControlInfo_RobotController::Reset()
     robotControllerAxisIndex[0] = robotControllerAxisIndex[1] = robotControllerAxisIndex[2] = -1;
     robotControllerAxisMult[0] = robotControllerAxisMult[1] = robotControllerAxisMult[2] = 1.0;
     robotControllerAxisOffset[0] = robotControllerAxisOffset[1] = robotControllerAxisOffset[2] = 0.0;
+    robotControllerAxisManufacturerCode[0] = robotControllerAxisManufacturerCode[1] = robotControllerAxisManufacturerCode[2] = "";
     robotControllerAxisProductCode[0] = robotControllerAxisProductCode[1] = robotControllerAxisProductCode[2] = "";
 }
 
@@ -117,6 +118,7 @@ void JointControlInfo_RobotController::SerializeJSON(rapidjson::Value& value, ra
     orjson::SetJsonValueByKey(value, "robotControllerAxisIndex", robotControllerAxisIndex, allocator);
     orjson::SetJsonValueByKey(value, "robotControllerAxisMult", robotControllerAxisMult, allocator);
     orjson::SetJsonValueByKey(value, "robotControllerAxisOffset", robotControllerAxisOffset, allocator);
+    orjson::SetJsonValueByKey(value, "robotControllerAxisManufacturerCode", robotControllerAxisManufacturerCode, allocator);
     orjson::SetJsonValueByKey(value, "robotControllerAxisProductCode", robotControllerAxisProductCode, allocator);
 }
 
@@ -126,6 +128,7 @@ void JointControlInfo_RobotController::DeserializeJSON(const rapidjson::Value& v
     orjson::LoadJsonValueByKey(value, "robotControllerAxisIndex", robotControllerAxisIndex);
     orjson::LoadJsonValueByKey(value, "robotControllerAxisMult", robotControllerAxisMult);
     orjson::LoadJsonValueByKey(value, "robotControllerAxisOffset", robotControllerAxisOffset);
+    orjson::LoadJsonValueByKey(value, "robotControllerAxisManufacturerCode", robotControllerAxisManufacturerCode);
     orjson::LoadJsonValueByKey(value, "robotControllerAxisProductCode", robotControllerAxisProductCode);
 }