Use teh torso orientation velocity to generate a desired angular momentum

Author: GiulioRomualdi

src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini

@@ -21,7 +21,7 @@ saturationFactors       (0.7, 0.7)
 
 ##Bounds
 #Step length
-maxStepLength           0.34
+maxStepLength           0.31
 minStepLength           0.01
 maxLengthBackwardFactor 1.0
 #Width
@@ -32,7 +32,7 @@ maxAngleVariation       18.0
 minAngleVariation       5.0
 #Timings
 maxStepDuration         1.31
-minStepDuration         0.5
+minStepDuration         0.7
 
 ##Nominal Values
 #Width

src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h

@@ -184,6 +184,7 @@ namespace WalkingControllers
         bool solveBLFIK(const iDynTree::Position& desiredCoMPosition,
                         const iDynTree::Vector3& desiredCoMVelocity,
                         const iDynTree::Rotation& desiredNeckOrientation,
+                        const iDynTree::SpatialMomentum& desiredCentroidalMomentum,
                         iDynTree::VectorDynSize &output);
 
         /**

src/WalkingModule/src/Module.cpp

@@ -371,6 +371,9 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf)
             return false;
         }
 
+        m_vectorsCollectionServer.populateMetadata("angular_momentum::measured", {"x", "y", "z"});
+        m_vectorsCollectionServer.populateMetadata("angular_momentum::desired", {"x", "y", "z"});
+
         m_vectorsCollectionServer.populateMetadata("dcm::position::measured", {"x", "y"});
         m_vectorsCollectionServer.populateMetadata("dcm::position::desired", {"x", "y"});
         m_vectorsCollectionServer.populateMetadata("dcm::velocity::desired", {"x", "y"});
@@ -502,6 +505,7 @@ bool WalkingModule::close()
 bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition,
                                const iDynTree::Vector3 &desiredCoMVelocity,
                                const iDynTree::Rotation &desiredNeckOrientation,
+                               const iDynTree::SpatialMomentum &centroidalMomentumDesired,
                                iDynTree::VectorDynSize &output)
 {
     const std::string phase = m_isStancePhase.front() ? "stance" : "walking";
@@ -516,6 +520,8 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition,
     ok = ok && m_BLFIKSolver->setRetargetingJointSetPoint(m_retargetingClient->jointPositions(),
                                                           m_retargetingClient->jointVelocities());
 
+    ok = ok && m_BLFIKSolver->setAngularMomentumSetPoint(centroidalMomentumDesired.getAngularVec3());
+
     if (m_useRootLinkForHeight)
     {
         ok = ok && m_BLFIKSolver->setRootSetPoint(desiredCoMPosition, desiredCoMVelocity);
@@ -902,6 +908,10 @@ bool WalkingModule::updateModule()
         yawRotation = yawRotation.inverse();
         modifiedInertial = yawRotation * m_inertial_R_worldFrame;
 
+        // compute the desired torso velocity
+        auto torsoVelocity = m_BLFIKSolver->getDesiredTorsoVelocity();
+        auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * torsoVelocity;
+
         if (m_useQPIK)
         {
             // integrate dq because velocity control mode seems not available
@@ -919,6 +929,7 @@ bool WalkingModule::updateModule()
             if (!solveBLFIK(desiredCoMPosition,
                             desiredCoMVelocity,
                             yawRotation,
+                            centroidalMomentumDesired,
                             m_dqDesired))
             {
                 yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with "
@@ -1023,6 +1034,8 @@ bool WalkingModule::updateModule()
             CoMVelocityDesired[1] = m_stableDCMModel->getCoMVelocity().data()[1];
             CoMVelocityDesired[2] = m_retargetingClient->comHeightVelocity();
 
+            m_vectorsCollectionServer.prepareData();
+            m_vectorsCollectionServer.clearData();
             m_vectorsCollectionServer.populateData("com::velocity::desired", CoMVelocityDesired);
 
             // Left foot position
@@ -1083,6 +1096,9 @@ bool WalkingModule::updateModule()
             const double isLeftFootFixed = m_isLeftFixedFrame.front() ? 1.0 : 0.0;
             m_vectorsCollectionServer.populateData("stance_foot::is_left", std::array<double, 1>{isLeftFootFixed});
 
+            m_vectorsCollectionServer.populateData("angular_momentum::measured", m_FKSolver->getKinDyn()->getCentroidalTotalMomentum().getAngularVec3());
+            m_vectorsCollectionServer.populateData("angular_momentum::desired", centroidalMomentumDesired.getAngularVec3());            
+
             m_vectorsCollectionServer.sendData();
         }
 

src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h

@@ -49,8 +49,11 @@ class BLFIK
     bool setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity);
     bool setRootSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity);
     bool setTorsoSetPoint(const iDynTree::Rotation& rotation);
+    bool setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum);
     const iDynTree::VectorDynSize& getDesiredJointVelocity() const;
 
+    iDynTree::Twist getDesiredTorsoVelocity() const;
+
 private:
     std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::MultiStateWeightProvider>
         m_torsoWeight;

src/WholeBodyControllers/src/BLFIK.cpp

@@ -142,9 +142,6 @@ bool BLFIK::initialize(
 
     ok = ok && m_qpIK.finalize(m_variableHandler);
 
-    m_angularMomentumTask->setSetPoint(Eigen::Vector3d::Zero());
-
-
     BipedalLocomotion::log()->info("[BLFIK::initialize] {}", m_qpIK.toString());
 
     m_jointVelocity.resize(kinDyn->getNrOfDegreesOfFreedom());
@@ -221,6 +218,11 @@ bool BLFIK::setRegularizationJointSetPoint(const iDynTree::VectorDynSize& jointP
     return m_jointRegularizationTask->setSetPoint(iDynTree::toEigen(jointPosition));
 }
 
+bool BLFIK::setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum)
+{
+    return m_angularMomentumTask->setSetPoint(iDynTree::toEigen(angularMomentum));
+}
+
 bool BLFIK::setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity)
 {
     return m_comTask->setSetPoint(iDynTree::toEigen(position), iDynTree::toEigen(velocity));
@@ -244,3 +246,11 @@ const iDynTree::VectorDynSize& BLFIK::getDesiredJointVelocity() const
 {
     return m_jointVelocity;
 }
+
+iDynTree::Twist BLFIK::getDesiredTorsoVelocity() const
+{
+    iDynTree::Twist tmp;
+    tmp.zero();
+    iDynTree::toEigen(tmp.getAngularVec3()) = m_torsoTask->getB();
+    return tmp;
+}