Merge branch 'RTRobotUnitV2' of https://gitlab.com/ArmarX/RobotAPI into RTRobotUnitV2

source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.cpp

@@ -44,23 +44,34 @@ namespace armarx
             startTime = sensorValuesTimestamp;
         }
         auto timeSinceStart = sensorValuesTimestamp - startTime;
-        auto positions = trajectory.getStates(timeSinceStart.toSecondsDouble(), 0);
-        auto velocities = trajectory.getStates(timeSinceStart.toSecondsDouble(), 1);
-        //        size_t dim = trajectory->dim();
-        for (size_t d = 0; d < targets.size(); ++d)
+        if (timeSinceStart.toSecondsDouble() > duration)
         {
-            ARMARX_CHECK_EXPRESSION(d < targets.size());
-            ARMARX_CHECK_EXPRESSION(d < PIDs.size());
-            ARMARX_CHECK_EXPRESSION(d < sensors.size());
-            ARMARX_CHECK_EXPRESSION(d < positions.size());
-            ARMARX_CHECK_EXPRESSION(d < velocities.size());
-            PIDControllerPtr& pid = PIDs.at(d);
-            pid->update(timeSinceLastIteration.toSecondsDouble(),
-                        sensors.at(d)->position,
-                        positions.at(d));
-            double newVel = pid->getControlValue();
-            newVel += velocities.at(1);
-            targets.at(d)->velocity = newVel;
+            for (size_t d = 0; d < targets.size(); ++d)
+            {
+                targets.at(d)->velocity = 0;
+            }
+        }
+        else
+        {
+            auto positions = trajectory.getStates(timeSinceStart.toSecondsDouble(), 0);
+            auto velocities = trajectory.getStates(timeSinceStart.toSecondsDouble(), 1);
+            //        size_t dim = trajectory->dim();
+
+            for (size_t d = 0; d < targets.size(); ++d)
+            {
+                ARMARX_CHECK_EXPRESSION(d < targets.size());
+                ARMARX_CHECK_EXPRESSION(d < PIDs.size());
+                ARMARX_CHECK_EXPRESSION(d < sensors.size());
+                ARMARX_CHECK_EXPRESSION(d < positions.size());
+                ARMARX_CHECK_EXPRESSION(d < velocities.size());
+                PIDControllerPtr& pid = PIDs.at(d);
+                pid->update(timeSinceLastIteration.toSecondsDouble(),
+                            sensors.at(d)->position,
+                            positions.at(d));
+                double newVel = pid->getControlValue();
+                newVel += velocities.at(1);
+                targets.at(d)->velocity = newVel;
+            }
         }
         TIMING_END(TrajectoryControl);
 
@@ -95,7 +106,7 @@ namespace armarx
         TrajectoryPtr newTraj = new Trajectory();
 
         Ice::DoubleSeq newTimestamps;
-        double duration = timeOptimalTraj.getDuration();
+        duration = timeOptimalTraj.getDuration();
         for (double t = 0.0; t < duration; t += timeStep)
         {
             newTimestamps.push_back(t);
@@ -127,6 +138,7 @@ namespace armarx
         }
         newTraj->setDimensionNames(trajectory->getDimensionNames());
         TIMING_END(TrajectoryOptimization);
+        ARMARX_INFO << "Trajectory duration: " << newTraj->getTimeLength();
         LockGuardType guard {controlDataMutex};
         getWriterControlStruct().trajectory = newTraj;
         writeControlStruct();

source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.h

@@ -38,6 +38,7 @@ namespace armarx
     private:
         //        TrajectoryPtr trajectory;
         IceUtil::Time startTime;
+        double duration = 0;
         std::vector<PIDControllerPtr> PIDs;
         std::vector<ControlTarget1DoFActuatorVelocity*> targets;
         std::vector<const SensorValue1DoFActuatorPosition*> sensors;