trajectory controller is recreated when joints are changed

trajectory controller is recreated when PID config is changed

source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp

@@ -176,6 +176,18 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
 
     auto startTime = this->jointTraj->begin()->getTimestamp();
     this->jointTraj->shiftTime(-startTime);
+    bool differentJointSet = usedJoints.size() != this->jointTraj->getDimensionNames().size();
+    if (!differentJointSet)
+    {
+        for (size_t i = 0; i < usedJoints.size(); i++)
+        {
+            if (usedJoints.at(i) != this->jointTraj->getDimensionNames().at(i))
+            {
+                differentJointSet = true;
+                break;
+            }
+        }
+    }
     usedJoints = this->jointTraj->getDimensionNames();
     ARMARX_INFO << VAROUT(usedJoints);
 
@@ -185,7 +197,10 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
         return;
     }
 
-    jointTrajController = createTrajectoryController(usedJoints, true);
+    if (!jointTrajController || differentJointSet || recreateController)
+    {
+        jointTrajController = createTrajectoryController(usedJoints, true);
+    }
     jointTrajController->setTrajectory(this->jointTraj, c);
 
     endTime = jointTrajController->getTrajEndTime();
@@ -459,24 +474,24 @@ void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
 }
 
 
-void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string> &changedProperties)
+void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
 {
     ARMARX_INFO << "Changning properties";
-    if(changedProperties.count("Kp"))
+    if (changedProperties.count("Kp"))
     {
         ARMARX_INFO << "Changning property Kp";
         kp = getProperty<float>("Kp").getValue();
         recreateController = true;
     }
 
-    if(changedProperties.count("Kd"))
+    if (changedProperties.count("Kd"))
     {
         ARMARX_INFO << "Changning property Kd";
         kd = getProperty<float>("Kd").getValue();
         recreateController = true;
     }
 
-    if(changedProperties.count("Ki"))
+    if (changedProperties.count("Ki"))
     {
         ARMARX_INFO << "Changning property Ki";
         ki = getProperty<float>("Ki").getValue();