adds possibility to show trajectory only as a preview to TrajectoryControllerSubUnit

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp

@@ -9,22 +9,18 @@ namespace armarx
     NJointTrajectoryController::NJointTrajectoryController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
     {
         cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
-        //        robot->getRobotNode("")->isLimitless();
         ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointTrajectoryControllerConfigPtr");
-        //        trajectory = TrajectoryPtr::dynamicCast(cfg->trajectory);
 
         for (size_t i = 0; i < cfg->jointNames.size(); i++)
         {
             auto jointName = cfg->jointNames.at(i);
             ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
             const SensorValueBase* sv = prov->getSensorValue(jointName);
-            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-            sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
-            PIDs.push_back(PIDControllerPtr(new PIDController(cfg->PID_p,
-                                            cfg->PID_i, cfg->PID_d,
-                                            cfg->maxVelocity)));
+            targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
+            sensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
+            //            targets[jointName] = ct->asA<ControlTarget1DoFActuatorVelocity>();
+            //            sensors[jointName] = sv->asA<SensorValue1DoFActuatorPosition>();
         }
-        currentPos.resize(cfg->jointNames.size());
     }
 
     std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
@@ -38,18 +34,27 @@ namespace armarx
         {
             TIMING_START(TrajectoryControl);
             TrajectoryController& contr = *rtGetControlStruct().trajectory;
-            int i = 0;
-            for (auto& sv : sensors)
+
+            auto dimNames = contr.getTraj()->getDimensionNames();
+            for (size_t i = 0; i < dimNames.size(); i++)
             {
-                currentPos(i) = sv->position;
-                i++;
+                const auto& jointName = dimNames.at(i);
+                currentPos(i) = (sensors.count(jointName) == 1) ? sensors[jointName]->position : 0.0f;
             }
+
             auto newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
             bool finished = (contr.getCurrentTimestamp() >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
                             || (contr.getCurrentTimestamp() <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
-            for (size_t i = 0; i < targets.size(); ++i)
+            if (!cfg->isPreview)
             {
-                targets.at(i)->velocity = finished ? 0.0f : newVelocities(i);
+                for (size_t i = 0; i < dimNames.size(); ++i)
+                {
+                    const auto& jointName = dimNames.at(i);
+                    if (targets.count(jointName) == 1)
+                    {
+                        targets[jointName]->velocity = finished ? 0.0f : newVelocities(i);
+                    }
+                }
             }
 
             if (finished && looping)
@@ -130,6 +135,7 @@ namespace armarx
         }
 
         trajEndTime = *trajectory->getTimestamps().rbegin() - *trajectory->getTimestamps().begin();
+        currentPos.resize(trajectory->getDimensionNames().size());
 
         LockGuardType guard {controlDataMutex};
         getWriterControlStruct().trajectory.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
@@ -162,6 +168,20 @@ namespace armarx
         return trajEndTime;
     }
 
+    TrajectoryPtr NJointTrajectoryController::getTrajectoryCopy() const
+    {
+        LockGuardType guard {controlDataMutex};
+        TrajectoryControllerPtr contr = rtGetControlStruct().trajectory;
+        if (contr)
+        {
+            return TrajectoryPtr::dynamicCast(contr->getTraj()->clone());
+        }
+        else
+        {
+            return new Trajectory();
+        }
+    }
+
     void NJointTrajectoryController::rtPreActivateController()
     {
         startTime = IceUtil::Time();
@@ -169,9 +189,9 @@ namespace armarx
 
     void NJointTrajectoryController::rtPostDeactivateController()
     {
-        for (size_t i = 0; i < targets.size(); ++i)
+        for (auto& target : targets)
         {
-            targets.at(i)->velocity = 0.0f;
+            target.second->velocity = 0.0f;
         }
     }
 

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h

@@ -4,7 +4,6 @@
 #include "NJointController.h"
 #include <VirtualRobot/Robot.h>
 #include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
 #include <RobotAPI/libraries/core/TrajectoryController.h>
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
@@ -41,12 +40,12 @@ namespace armarx
         double getCurrentTrajTime() const;
         void setLooping(bool looping);
         double getTrajEndTime() const;
+        TrajectoryPtr getTrajectoryCopy() const;
     private:
         //        TrajectoryPtr trajectory;
         IceUtil::Time startTime;
-        std::vector<PIDControllerPtr> PIDs;
-        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-        std::vector<const SensorValue1DoFActuatorPosition*> sensors;
+        std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
+        std::map<std::string, const SensorValue1DoFActuatorPosition*> sensors;
         Eigen::VectorXf currentPos;
         bool looping = false;
         float direction = 1.0;

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

@@ -35,11 +35,14 @@ void TrajectoryControllerSubUnit::onInitComponent()
     ki = getProperty<float>("Ki").getValue();
     kd = getProperty<float>("Kd").getValue();
     maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 * M_PI; // config expects value in rad/sec
+
+    offeringTopic("DebugDrawerUpdates");
 }
 
 void TrajectoryControllerSubUnit::onConnectComponent()
 {
     kinematicUnit = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+    debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 }
 
 bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
@@ -48,7 +51,19 @@ bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
     assureTrajectoryController();
 
     jointTrajController->setTrajectory(this->jointTraj, c);
+    jointTraj = jointTrajController->getTrajectoryCopy();
     jointTrajController->activateController();
+
+    if (isPreview)
+    {
+        ARMARX_INFO << "robot file name : " << kinematicUnit->getRobotFilename();
+        debugDrawer->setRobotVisu("Preview", "previewRobot", kinematicUnit->getRobotFilename(), boost::join(kinematicUnit->getArmarXPackages(), ","), armarx::CollisionModel);
+        debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor {0, 1, 0, 0.5});
+
+        previewTask = new PeriodicTask<TrajectoryControllerSubUnit>(this, &TrajectoryControllerSubUnit::previewRun, 20, false, "TrajectoryControllerSubUnitPreviewTask", false);
+        previewTask->start();
+    }
+
     return true;
 }
 
@@ -72,6 +87,12 @@ bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
 {
     if (controllerExists())
     {
+        if (isPreview)
+        {
+            previewTask->stop();
+            debugDrawer->clearLayer("Preview");
+        }
+
         jointTrajController->deactivateController();
         while (jointTrajController->isControllerActive()) {}
         jointTrajController->deleteController();
@@ -225,7 +246,18 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
 
 void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
 {
-    ARMARX_WARNING << "NYI : TrajectoryControllerSubUnit::setIsPreview()";
+    if (controllerExists() && jointTrajController->isControllerActive())
+    {
+        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting wheter the controller is only for preview.";
+        return;
+    }
+    this->isPreview = isPreview;
+
+    if (jointTraj)
+    {
+        jointTrajController = createTrajectoryController(usedJoints, false);
+        jointTrajController->setTrajectory(jointTraj, c);
+    }
 }
 
 bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, bool isInUse, const Ice::Current& c)
@@ -278,7 +310,7 @@ void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::
 {
     if (controllerExists() && jointTrajController->isControllerActive())
     {
-        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting joints in use.";
+        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether constraints should be considered.";
         return;
     }
     considerConstraintsForTrajectoryOptimization = consider;
@@ -316,6 +348,13 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
         config->maxVelocity = maxVelocity;
         config->jointNames = jointNames;
         config->considerConstraints = considerConstraintsForTrajectoryOptimization;
+        config->isPreview = isPreview;
+
+        ARMARX_IMPORTANT << config->PID_p << "\n"
+                         << config->PID_i << "\n" << config->PID_i << "\n"
+                         << config->maxVelocity << "\n" << config->jointNames << "\n"
+                         << config->considerConstraints << "\n" << config->isPreview << "\n";
+
         trajController = NJointTrajectoryControllerPtr::dynamicCast(
                              robotUnit->createNJointController(
                                  "NJointTrajectoryController", controllerName,
@@ -355,6 +394,33 @@ bool TrajectoryControllerSubUnit::controllerExists()
     return false;
 }
 
+void TrajectoryControllerSubUnit::previewRun()
+{
+    if (controllerExists())
+    {
+        if (jointTrajController->isControllerActive())
+        {
+            ARMARX_DEBUG << "Preview ... ";
+
+            std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
+            NameValueMap targetPositionValues;
+
+            auto dimNames = jointTraj->getDimensionNames();
+
+            for (size_t i = 0; i < dimNames.size(); i++)
+            {
+                const auto& jointName = dimNames.at(i);
+                if (std::find(usedJoints.begin(), usedJoints.end(), jointName) != usedJoints.end())
+                {
+                    targetPositionValues[jointName] = states[i][0];
+                }
+            }
+
+            debugDrawer->updateRobotConfig("Preview", "previewRobot", targetPositionValues);
+        }
+    }
+}
+
 void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
 {
     ARMARX_CHECK_EXPRESSION(!robotUnit);

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

@@ -129,6 +129,7 @@ namespace armarx
         NJointTrajectoryControllerPtr createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
         void assureTrajectoryController();
         bool controllerExists();
+        void previewRun();
 
         RobotUnit* robotUnit = NULL;
         NJointTrajectoryControllerPtr jointTrajController;
@@ -155,6 +156,10 @@ namespace armarx
         float maxVelocity;
         bool considerConstraintsForTrajectoryOptimization = false;
 
+        bool isPreview = false;
+        DebugDrawerInterfacePrx debugDrawer;
+        PeriodicTask<TrajectoryControllerSubUnit>::pointer_type previewTask;
+
         Mutex dataMutex;
     };
 }

source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice

@@ -39,6 +39,7 @@ module armarx
         float PID_i = 0;
         float PID_d = 0;
         bool considerConstraints = true;
+        bool isPreview = false;
     };
 
     interface NJointTrajectoryControllerInterface extends NJointControllerInterface