diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini index d051cabad..f3633cb06 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini +++ b/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.25 +maxStepLength 0.31 minStepLength 0.01 maxLengthBackwardFactor 1.0 #Width @@ -45,11 +45,11 @@ comHeightDelta 0.01 #Timings nominalDuration 1.3 lastStepSwitchTime 0.8 -switchOverSwingRatio 0.3 +switchOverSwingRatio 0.1 #ZMP Delta -leftZMPDelta (0.005 -0.0) -rightZMPDelta (0.005 0.0) +leftZMPDelta (0.0 -0.0) +rightZMPDelta (0.0 0.0) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini index be0243b00..4f74c63cd 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini @@ -26,8 +26,8 @@ max-cpu-time 20 #DEGREES jointRegularization (7, 0.12, -0.01, - -3.7, 20.0, -13.0, 40.769, - -3.7, 20.0, -13.0, 40.769, + 0.0, 0.0, -0.0, 0.0, + 0.0, 0.0, -0.0, 0.0, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 70c48fa81..f843d25da 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -24,5 +24,10 @@ frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 +[ANGULAR_MOMENTUM_TASK] +robot_velocity_variable_name "robot_velocity" +mask (false, false, true) +weight (2.0) + [include TORSO_TASK "./tasks/torso.ini"] [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..73efbf070 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,9 +1,9 @@ robot_velocity_variable_name "robot_velocity" -kp (5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) +kp (5.0, 5.0, 1.0, + 0.5, 5.0, 5.0, 0.5, + 0.5, 5.0, 5.0, 0.5, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) states ("stance", "walking") sampling_time 0.002 @@ -11,16 +11,16 @@ settling_time 0.5 [stance] name "stance" -weight (1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, +weight (1.0, 1.0, 0.5, + 1.0, 2.0, 2.0, 1.0, + 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) [walking] name "walking" -weight (1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, +weight (1.0, 1.0, 0.5, + 1.0, 2.0, 2.0, 1.0, + 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..59047e651 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini @@ -1,6 +1,6 @@ robot_velocity_variable_name "robot_velocity" frame_name "chest" -kp_angular 5.0 +kp_angular 50.0 states ("stance", "walking") @@ -9,8 +9,8 @@ settling_time 0.5 [stance] name "stance" -weight (5.0, 5.0, 5.0) +weight (15.0, 15.0, 1.0) [walking] name "walking" -weight (5.0, 5.0, 5.0) +weight (15.0, 15.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini index b6fe27b7b..07e521441 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini @@ -17,7 +17,7 @@ skip_dcm_controller 1 goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.02 +planner_advance_time_in_s 0.08 # How much time (in seconds) before failing due to missing feedback max_feedback_delay_in_s 1.0 @@ -29,7 +29,7 @@ remove_zmp_offset 0 [GENERAL] name walking-coordinator # height of the com -com_height 0.7 +com_height 0.715 # sampling time sampling_time 0.01 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini index ef948ede3..3f55907cd 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini @@ -32,8 +32,8 @@ max-cpu-time 20 # 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) jointRegularization (7, 0.12, -0.01, - 12.0, 7.0, -12.0, 40.769, - 12.0, 7.0, -12.0, 40.769, + 0.0, 0.0, 0.0, 0.0 + 0.0, 0.0, 0.0, 0.0, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 82acb692e..f874bfed6 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -24,5 +24,10 @@ frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 +[ANGULAR_MOMENTUM_TASK] +robot_velocity_variable_name "robot_velocity" +mask (false, false, true) +weight (5.0) + [include TORSO_TASK "./tasks/torso.ini"] [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..05aa894ca 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,7 +1,7 @@ robot_velocity_variable_name "robot_velocity" -kp (5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, +kp (5.0, 5.0, 1.0, + 0.5, 5.0, 5.0, 0.5, + 0.5, 5.0, 5.0, 0.5, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) @@ -11,16 +11,16 @@ settling_time 0.5 [stance] name "stance" -weight (1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, +weight (1.0, 1.0, 0.5, + 1.0, 2.0, 2.0, 1.0, + 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) [walking] name "walking" -weight (1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, +weight (1.0, 1.0, 0.5, + 1.0, 2.0, 2.0, 1.0, + 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..040298f3c 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini @@ -1,6 +1,6 @@ robot_velocity_variable_name "robot_velocity" frame_name "chest" -kp_angular 5.0 +kp_angular 10.0 states ("stance", "walking") @@ -9,8 +9,8 @@ settling_time 0.5 [stance] name "stance" -weight (5.0, 5.0, 5.0) +weight (10.0, 10.0, 10.0) [walking] name "walking" -weight (5.0, 5.0, 5.0) +weight (10.0, 10.0, 10.0) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 16d4ea0e8..19fa78af1 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/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); /** @@ -199,6 +200,8 @@ namespace WalkingControllers */ iDynTree::Rotation computeAverageYawRotationFromPlannedFeet() const; + iDynTree::Twist computeAverageTwistFromPlannedFeet() const; + /** * Generate the first trajectory. * This method has to be called before updateTrajectories() method. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 929424fea..8ba27a950 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/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 ¢roidalMomentumDesired, 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 + const iDynTree::Twist desiredTorsoVelocity = this->computeAverageTwistFromPlannedFeet(); + auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * desiredTorsoVelocity; + 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{isLeftFootFixed}); + m_vectorsCollectionServer.populateData("angular_momentum::measured", m_FKSolver->getKinDyn()->getCentroidalTotalMomentum().getAngularVec3()); + m_vectorsCollectionServer.populateData("angular_momentum::desired", centroidalMomentumDesired.getAngularVec3()); + m_vectorsCollectionServer.sendData(); } @@ -1124,6 +1140,21 @@ iDynTree::Rotation WalkingModule::computeAverageYawRotationFromPlannedFeet() con return iDynTree::Rotation::RotZ(meanYaw); } +iDynTree::Twist WalkingModule::computeAverageTwistFromPlannedFeet() const +{ + iDynTree::Twist twist; + iDynTree::Vector3 meanLinearVelocity, meanAngularVelocity; + iDynTree::toEigen(meanLinearVelocity) = (iDynTree::toEigen(m_leftTwistTrajectory.front().getLinearVec3()) + + iDynTree::toEigen(m_rightTwistTrajectory.front().getLinearVec3())) / 2.0; + iDynTree::toEigen(meanAngularVelocity) = (iDynTree::toEigen(m_leftTwistTrajectory.front().getAngularVec3()) + + iDynTree::toEigen(m_rightTwistTrajectory.front().getAngularVec3())) / 2.0; + + twist.setLinearVec3(meanLinearVelocity); + twist.setAngularVec3(meanAngularVelocity); + + return twist; +} + bool WalkingModule::prepareRobot(bool onTheFly) { if (m_robotState != WalkingFSM::Configured && m_robotState != WalkingFSM::Stopped) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index 1d19809ba..605e75cae 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -48,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 m_torsoWeight; @@ -68,6 +72,7 @@ class BLFIK std::shared_ptr m_rootTask; std::shared_ptr m_jointRetargetingTask; std::shared_ptr m_jointRegularizationTask; + std::shared_ptr m_angularMomentumTask; iDynTree::VectorDynSize m_jointVelocity; bool m_usejointRetargeting{false}; diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index 93dfcaf65..9577d8fc7 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -102,6 +102,24 @@ bool BLFIK::initialize( lowPriority, m_jointRegularizationWeight); + m_angularMomentumTask = std::make_shared(); + ok = ok && m_angularMomentumTask->setKinDyn(kinDyn); + ok = ok && m_angularMomentumTask->initialize(ptr->getGroup("ANGULAR_MOMENTUM_TASK")); + + Eigen::VectorXd angularMomentumWeight; + ok = ok && ptr->getGroup("ANGULAR_MOMENTUM_TASK").lock()->getParameter("weight", angularMomentumWeight); + + ok = ok + && m_qpIK.addTask(m_angularMomentumTask, + "angular_momentum_task", + lowPriority, + angularMomentumWeight); + if (!ok) + { + BipedalLocomotion::log()->error("{} Unable to initialize the angular momentum task.", prefix); + return false; + } + if (m_usejointRetargeting) { m_jointRetargetingTask = std::make_shared(); @@ -200,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)); @@ -223,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; +} \ No newline at end of file