Merge branch 'Armar6RT' into GamepadDriver

source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h

@@ -34,6 +34,7 @@
 #include <RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h>
 #include <RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h>
 #include <RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h>
 
 namespace armarx
 {
@@ -134,9 +135,38 @@ namespace armarx
         }
     };
 
+    template <>
+    struct PassThroughControllerTargetTypeTraits<PlatformWheelVelocityTarget>
+    {
+        static float* getTargetDatamember(PlatformWheelVelocityTarget& t)
+        {
+            return  &t.velocity;
+        }
+        static const float* getFrontRightWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getFrontRightWheelVelocity();
+        }
+        static const float* getFrontLeftWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getFrontLeftWheelVelocity();
+        }
+        static const float* getRearRightWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getRearRightWheelVelocity();
+        }
+        static const float* getRearLeftWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getRearLeftWheelVelocity();
+        }
+        static  std::string getControlMode()
+        {
+            return ControlModes::VelocityMode;
+        }
+    };
+
     LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
     LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
-    LVL1ControllerRegistration<PassThroughController<JointTorqueTarget  >> registrationSomeControllerJointPassThroughController("JointPassThroughController");
+    LVL1ControllerRegistration<PassThroughController<JointTorqueTarget  >> registrationSomeControllerJointPassThroughController("TorquePassThroughController");
 
     template <typename TargetType>
     std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
@@ -194,6 +224,8 @@ namespace armarx
         for (std::size_t i = 0; i < jointStates.size(); ++i)
         {
             iceTargets.at(i).val.store(*jointStates.at(i));
+            //set it to zero
+            iceTargets.at(i).val.store(0.0);
         }
     }
 

source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h

@@ -302,6 +302,12 @@ namespace armarx
             controlDataTripleBuffer {initialCommands}
         {
         }
+
+        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+        {
+            rtUpdateControlStruct();
+            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+        }
     protected:
         const ControlDataStruct& rtGetControlStruct() const
         {
@@ -311,11 +317,6 @@ namespace armarx
         {
             return controlDataTripleBuffer.updateReadBuffer();
         }
-        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
-        {
-            rtUpdateControlStruct();
-            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
-        }
 
         void writeControlStruct()
         {
@@ -336,5 +337,26 @@ namespace armarx
     };
     template <typename ControlDataStruct>
     using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
+
+
+    struct PlatformCartesianVelocity
+    {
+        float vx;
+        float vy;
+        float vAngle;
+        PlatformCartesianVelocity() : vx(0), vy(0), vAngle(0)
+        { }
+    };
+
+
+    class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity> {
+
+    public:
+        virtual void setTarget(float vx, float vy, float vAngle) = 0;
+    };
+
+    typedef boost::shared_ptr <AbstractLvl1PlatformVelocityController> AbstractLvl1PlatformVelocityControllerPtr;
+
+
 }
 #endif