Merge pull request #41 from robotology/feature/joint_retargeting

Implement joint retargeting

src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h

@@ -69,6 +69,8 @@ namespace WalkingControllers
 
         bool m_firstStep; /**< True only during the first step. */
 
+        iDynTree::VectorDynSize m_jointPositions; /**< joint positions in radians. */
+
         /**
          * Set the model of the robot.
          * @param model iDynTree model.
@@ -258,6 +260,12 @@ namespace WalkingControllers
          * @return true/false in case of success/failure.
          */
         bool getCoMJacobian(iDynTree::MatrixDynSize &jacobian);
+
+        /**
+         * Get the joint position
+         * @return the joint position expressed in radians
+         */
+        const iDynTree::VectorDynSize& getJointPos();
     };
 };
 #endif

src/KinDynWrapper/src/Wrapper.cpp

@@ -262,6 +262,10 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config,
 
     m_useFilters = config.check("use_filters", yarp::os::Value(false)).asBool();
     m_firstStep = true;
+
+
+    // resize the joint positions
+    m_jointPositions.resize(model.getNrOfDOFs());
     return true;
 }
 
@@ -517,3 +521,13 @@ bool WalkingFK::getCoMJacobian(iDynTree::MatrixDynSize &jacobian)
 {
     return m_kinDyn.getCenterOfMassJacobian(jacobian);
 }
+
+const iDynTree::VectorDynSize& WalkingFK::getJointPos()
+{
+
+    bool ok = m_kinDyn.getJointPos(m_jointPositions);
+
+    assert(ok);
+
+    return m_jointPositions;
+}

src/RetargetingHelper/CMakeLists.txt

@@ -24,6 +24,7 @@ if(WALKING_CONTROLLERS_COMPILE_RetargetingHelper)
 
   target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC
     WalkingControllers::YarpUtilities
+    WalkingControllers::KinDynWrapper
     ${iDynTree_LIBRARIES}
     ctrlLib)
 

src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h

@@ -11,10 +11,12 @@
 
 // std
 #include <memory>
+#include <vector>
 
 // iDyntree
 #include <iDynTree/Core/Transform.h>
 #include <iDynTree/Core/Rotation.h>
+#include <iDynTree/Core/VectorDynSize.h>
 
 // yarp
 #include <yarp/os/BufferedPort.h>
@@ -24,6 +26,8 @@
 // iCub-ctrl
 #include <iCub/ctrl/minJerkCtrl.h>
 
+#include <WalkingControllers/KinDynWrapper/Wrapper.h>
+
 namespace WalkingControllers
 {
 
@@ -32,28 +36,65 @@ namespace WalkingControllers
  */
     class RetargetingClient
     {
+    public:
+      enum class Phase
+      {
+          /** In this phase the smoothing time of the minimum jerk trajectory is
+              increased. This will guarantee a smoother transition between the
+              initial joint configuration and the desired joint configuration. */
+          approacing,
+          stance,
+          walking
+      };
+
     private:
 
+        template <class Data>
+        struct RetargetingElement
+        {
+            yarp::sig::Vector yarpReadBuffer;
+            std::unique_ptr<iCub::ctrl::minJerkTrajGen> smoother;
+            yarp::os::BufferedPort<yarp::sig::Vector> port;
+            double smoothingTimeInApproaching;
+            double smoothingTimeInWalking;
+            Data data;
+        };
+
         bool m_useHandRetargeting; /**< True if the hand retargeting is used */
         bool m_useVirtualizer; /**< True if the virtualizer is used */
+        bool m_useJointRetargeting; /**< True if the joint retargeting is used */
+        bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */
 
-        iDynTree::Transform m_leftHandTransform; /**< Left hand transform head_T_leftHand */
-        iDynTree::Transform m_rightHandTransform; /**< Right hand transform head_T_rightHand*/
+        RetargetingElement<iDynTree::Transform> m_leftHand; /**< Left hand retargeting element */
+        RetargetingElement<iDynTree::Transform> m_rightHand; /**< Right hand retargeting element */
 
-        yarp::sig::Vector m_leftHandTransformYarp; /**< Left hand position + rpy head_T_leftHand */
-        yarp::sig::Vector m_rightHandTransformYarp; /**< Right hand position + rpy head_T_rightHand*/
+        /** Offset of the CoM Height coming from the user. It is required given the different size
+         *  between the human and the robot */
+        double m_comHeightInputOffset;
 
-        yarp::os::BufferedPort<yarp::sig::Vector> m_leftHandTransformPort; /**< Right hand position + rpy head_T_rightHand*/
-        yarp::os::BufferedPort<yarp::sig::Vector> m_rightHandTransformPort; /**< Right hand position + rpy head_T_rightHand*/
+        /** Desired value of the CoM height used during walking. The simplified model used for
+         * the locomotion is based on the assumption of a constant CoM height */
+        double m_comConstantHeight;
 
-        std::unique_ptr<iCub::ctrl::minJerkTrajGen> m_leftHandSmoother; /**< Minimum jerk trajectory
-                                                                           for the left hand. */
+        /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */
+        double m_comHeightScalingFactor;
 
-        std::unique_ptr<iCub::ctrl::minJerkTrajGen> m_rightHandSmoother; /**< Minimum jerk trajectory
-                                                                            for the right hand. */
+        struct KinematicState
+        {
+            double position;
+            double velocity;
+        };
+        RetargetingElement<KinematicState> m_comHeight;
+
+        std::vector<int> m_retargetJointsIndex; /**< Vector containing the indices of the retargeted joints. */
+        RetargetingElement<iDynTree::VectorDynSize> m_jointRetargeting; /**< Joint retargeting element */
 
         yarp::os::BufferedPort<yarp::sig::Vector> m_robotOrientationPort; /**< Average orientation of the robot.*/
 
+        Phase m_phase{Phase::approacing};
+        double m_startingApproachingPhaseTime; /**< Initial time of the approaching phase (seconds) */
+        double m_approachPhaseDuration; /**< Duration of the approaching phase (seconds) */
+
         /**
          * Convert a yarp vector containing position + rpy into an iDynTree homogeneous transform
          * @param vector a 6d yarp vector
@@ -62,26 +103,33 @@ namespace WalkingControllers
         void convertYarpVectorPoseIntoTransform(const yarp::sig::Vector& vector,
                                                 iDynTree::Transform& transform);
 
+        /**
+         * Terminate the approaching phase
+         */
+        void stopApproachingPhase();
+
     public:
 
         /**
          * Initialize the client
          * @param config configuration parameters
          * @param name name of the module
          * @param period period of the module
+         * @param controlledJointsName name of the controlled joints
          * @return true/false in case of success/failure
          */
         bool initialize(const yarp::os::Searchable &config,
                         const std::string &name,
-                        const double &period);
+                        const double &period,
+                        const std::vector<std::string>& controlledJointNames);
 
         /**
          * Reset the client
-         * @param leftHandTransform head_T_leftHand transform
-         * @param rightHandTransform head_T_rightHand transform
+         * @param kinDynWrapper a wrapper of KinDynComputations useful to get some
+         * informations of the current status of the robot
+         * @return true/false in case of success/failure
          */
-        void reset(const iDynTree::Transform& leftHandTransform,
-                   const iDynTree::Transform& rightHandTransform);
+        bool reset(WalkingFK& kinDynWrapper);
 
         /**
          * Close the client
@@ -103,10 +151,41 @@ namespace WalkingControllers
          */
         const iDynTree::Transform& rightHandTransform() const;
 
+        /**
+         * Get the value of the retarget joints
+         */
+        const iDynTree::VectorDynSize& jointValues() const;
+
+        /**
+         * Get the CoM height
+         * @return height of the CoM
+         */
+        double comHeight() const;
+
+        /**
+         * Get the CoM height velocity
+         * @return height velocity of the CoM
+         */
+        double comHeightVelocity() const;
+
         /**
          * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand
          */
         void setRobotBaseOrientation(const iDynTree::Rotation& rotation);
+
+
+        void setPhase(Phase phase);
+
+        /**
+         * Start the approaching phase
+         */
+        void startApproachingPhase();
+
+        /**
+         * Check if the approaching phase is running
+         * @return true if the approaching phase is running
+         */
+        bool isApproachingPhase() const;
     };
 };
 #endif