From 92e19bd14a59a111c558450188836df1e9356fba Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Mon, 24 Feb 2025 17:06:44 +0100 Subject: [PATCH 01/26] first skeleton of tsid --- .../TaskSpaceInverseDynamicsBlf.ini | 161 +++++++++++++++ .../tsid_tasks/angular_momentum_task.ini | 15 ++ .../tsid_tasks/base_dynamics_task.ini | 14 ++ .../tsid_tasks/joint_dynamics_task.ini | 15 ++ src/WholeBodyControllers/CMakeLists.txt | 4 +- .../WholeBodyControllers/BLFTSID.h | 123 ++++++++++++ src/WholeBodyControllers/src/BLFTSID.cpp | 188 ++++++++++++++++++ 7 files changed, 519 insertions(+), 1 deletion(-) create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini create mode 100644 src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h create mode 100644 src/WholeBodyControllers/src/BLFTSID.cpp diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini new file mode 100644 index 000000000..421dc3ed2 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -0,0 +1,161 @@ +tasks ("COM_TRACKING", "ROOT_TRACKING", "TORSO_TRACKING", "ANGULAR_MOMENTUM_TRACKING", + "LEFT_FOOT_TRACKING", "RIGHT_FOOT_TRACKING", + "LEFT_FOOT_FEASIBLE_WRENCH", "RIGHT_FOOT_FEASIBLE_WRENCH", + "JOINT_TRACKING", "JOINT_DYNAMICS", "BASE_DYNAMICS", + "LEFT_FOOT_WRENCH_REGULARIZATION", "RIGHT_FOOT_WRENCH_REGULARIZATION", "TORQUE_REGULARIZATION") + +[TSID] +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +contact_wrench_variables_name ("lf_wrench", "rf_wrench") +verbosity false +automatic_scaling false + + +; [VARIABLES] +; variables_name ("robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench") +; variables_size (28, 22, 6, 6) + + +[LEFT_FOOT_TRACKING] +type SE3Task +priority 0 +kp_linear 10.0 +kp_angular 10.0 +kd_linear 1.0 +kd_angular 1.0 +frame_name "l_sole" +use_orientation_exogenous_feedback false +use_position_exogenous_feedback false +weight (1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0) +mask (true, true, true) +robot_acceleration_variable_name "robot_acceleration" + + +[RIGHT_FOOT_TRACKING] +type SE3Task +priority 0 +kp_linear 10.0 +kp_angular 10.0 +kd_linear 1.0 +kd_angular 1.0 +frame_name "r_sole" +use_orientation_exogenous_feedback false +use_position_exogenous_feedback false +weight (1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0) +mask (true, true, true) +robot_acceleration_variable_name "robot_acceleration" + + +[COM_TRACKING] +type CoMTask +kp_linear 10.0 +kd_linear 1.0 +priority 0 +mask (true, true, false) +use_exogenous_feedback false +weight (1000.0, 1000.0) +robot_acceleration_variable_name "robot_acceleration" + + +[TORSO_TRACKING] +type SO3Task +kp_angular 5.0 +kd_angular 5.0 +frame_name "chest" +priority 1 +weight (100.0, 100.0, 100.0) +robot_acceleration_variable_name "robot_acceleration" + + +[ROOT_TRACKING] +type R3Task +frame_name "root_link" +kp_linear 10.0 +kd_linear 1.0 +mask (false, false, true) +priority 0 +weight (1000.0) +robot_acceleration_variable_name "robot_acceleration" + + +[JOINT_TRACKING] + +# joints_list "torso_pitch", "torso_roll", "torso_yaw", +# "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", +# "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", +# "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", +# "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll" + +type JointTrackingTask +priority 1 +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) + +weight (10.0, 10.0, + 10.0, 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 + 10.0, 10.0, 10.0, 10.0, 10.0, 10.0) +robot_acceleration_variable_name "robot_acceleration" + + +[LEFT_FOOT_FEASIBLE_WRENCH] +name "lf_feasibility_wrench" +type "FeasibleContactWrenchTask" +priority 0 + +variable_name "lf_wrench" +frame_name "l_sole" +number_of_slices 2 +static_friction_coefficient 0.5 +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) + + +[RIGHT_FOOT_FEASIBLE_WRENCH] +name "rf_feasibility_wrench" +type "FeasibleContactWrenchTask" +priority 0 + +variable_name "rf_wrench" +frame_name "r_sole" +number_of_slices 2 +static_friction_coefficient 0.5 +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) + +[LEFT_FOOT_WRENCH_REGULARIZATION] +type "VariableRegularizationTask" +variable_name "lf_wrench" + +variable_size 6 +priority 1 +weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[RIGHT_FOOT_WRENCH_REGULARIZATION] +type "VariableRegularizationTask" +variable_name "rf_wrench" + +variable_size 6 +priority 1 +weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[TORQUE_REGULARIZATION] +type "VariableRegularizationTask" +variable_name "joint_torques" + +variable_size 22 +priority 1 +weight (0.01, 0.01, + 0.01, 0.01, 0.01, 0.01, + 0.01, 0.01, 0.01, 0.01, + 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 + 0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[include JOINT_DYNAMICS "./tsid_tasks/joint_dynamics_task.ini"] +[include BASE_DYNAMICS "./tsid_tasks/base_dynamics_task.ini"] +[include ANGULAR_MOMENTUM_TRACKING "./tsid_tasks/angular_momentum_task.ini"] \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini new file mode 100644 index 000000000..ff09bc0d4 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini @@ -0,0 +1,15 @@ +name "angular_momentum_task" +type "AngularMomentumTask" +kp 30.0 +priority 1 +use_exogenous_feedback false +weight (500.0, 500.0, 500.0) +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini new file mode 100644 index 000000000..0f3094bbf --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini @@ -0,0 +1,14 @@ +name "base_dynamics_task" +type "BaseDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini new file mode 100644 index 000000000..014bdf8f5 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini @@ -0,0 +1,15 @@ +name "joint_dynamics_task" +type "JointDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" \ No newline at end of file diff --git a/src/WholeBodyControllers/CMakeLists.txt b/src/WholeBodyControllers/CMakeLists.txt index 6ced55e00..7ed46107a 100644 --- a/src/WholeBodyControllers/CMakeLists.txt +++ b/src/WholeBodyControllers/CMakeLists.txt @@ -4,10 +4,12 @@ add_walking_controllers_library( NAME WholeBodyControllers - SOURCES src/InverseKinematics.cpp src/BLFIK.cpp + SOURCES src/InverseKinematics.cpp src/BLFIK.cpp src/BLFTSID.cpp PUBLIC_HEADERS include/WalkingControllers/WholeBodyControllers/InverseKinematics.h include/WalkingControllers/WholeBodyControllers/BLFIK.h + include/WalkingControllers/WholeBodyControllers/BLFTSID.h PUBLIC_LINK_LIBRARIES BipedalLocomotion::IK + BipedalLocomotion::TSID BipedalLocomotion::ContinuousDynamicalSystem WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h new file mode 100644 index 000000000..f4f9729b2 --- /dev/null +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h @@ -0,0 +1,123 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_BLF_TSID +#define WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_BLF_TSID + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace WalkingControllers { + +class BLFTSID { + public: + bool initialize( + std::weak_ptr< + const BipedalLocomotion::ParametersHandler::IParametersHandler> + handler, + std::shared_ptr kinDyn); + + bool solve(); + + bool + setLeftFootTrackingSetPoint(const iDynTree::Transform &desiredTransform, + const iDynTree::Twist &desiredVelocity, + const iDynTree::Twist &desiredAcceleration); + + bool + setRightFootTrackingSetPoint(const iDynTree::Transform &desiredTransform, + const iDynTree::Twist &desiredVelocity, + const iDynTree::Twist &desiredAcceleration); + bool + setJointTrackingSetPoint(const iDynTree::VectorDynSize &jointPosition, + const iDynTree::VectorDynSize &jointVelocity, + const iDynTree::VectorDynSize &jointAcceleration); + + bool setAngularMomentumTrackingSetPoint( + const iDynTree::Vector3 &angularMomentum, + const iDynTree::Vector3 &angularMomentumRate); + + bool setCoMTrackingSetPoint(const iDynTree::Position &position, + const iDynTree::Vector3 &velocity, + const iDynTree::Vector3 &acceleration); + + bool setRootTrackingSetPoint(const iDynTree::Position &position, + const iDynTree::Vector3 &linearVelocity, + const iDynTree::Vector3 &linearAcceleration); + + bool setTorsoTrackingSetPoint(const iDynTree::Rotation &rotation, + const iDynTree::Vector3 &angularVelocity, + const iDynTree::Vector3 &angularAcceleration); + + bool setLeftContactWrenchSetPoint(const iDynTree::Wrench &contactWrench); + + bool setRightContactWrenchSetPoint(const iDynTree::Wrench &contactWrench); + + bool setTorqueRegularizationSetPoint(const iDynTree::VectorDynSize &torque); + + void setLeftContactActive(bool isActive); + + void setRightContactActive(bool isActive); + + const iDynTree::VectorDynSize &getDesiredJointAcceleration() const; + + private: + struct TSIDProblemAndTasks { + BipedalLocomotion::TSID::TaskSpaceInverseDynamicsProblem problem; + + struct TSIDtasks { + std::shared_ptr comTracking; + std::shared_ptr torsoTracking; + std::shared_ptr leftFootTracking; + std::shared_ptr rightFootTracking; + std::shared_ptr rootTracking; + std::shared_ptr + angularMomentumTracking; + std::shared_ptr + jointTracking; + std::shared_ptr + leftFeasibleContactWrench; + std::shared_ptr + rightFeasibleContactWrench; + std::shared_ptr + leftContactWrenchRegularization; + std::shared_ptr + rightContactWrenchRegularization; + std::shared_ptr + torqueRegularization; + std::shared_ptr + jointDynamics; + std::shared_ptr + baseDynamics; + }; + + TSIDtasks tasks; + }; + + TSIDProblemAndTasks m_qpTSID; + + iDynTree::VectorDynSize m_jointAccelerations; + iDynTree::VectorDynSize m_jointTorques; + + bool m_useRootLinkForHeight{false}; +}; + +} // namespace WalkingControllers + +#endif // WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_BLF_TSID diff --git a/src/WholeBodyControllers/src/BLFTSID.cpp b/src/WholeBodyControllers/src/BLFTSID.cpp new file mode 100644 index 000000000..983062375 --- /dev/null +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -0,0 +1,188 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace WalkingControllers; + +bool BLFTSID::initialize( + std::weak_ptr< + const BipedalLocomotion::ParametersHandler::IParametersHandler> + handler, + std::shared_ptr kinDyn) { + constexpr auto logPrefix = "[WholeBodyQPBlock::instantiateTSID]"; + + auto getTask = [this, logPrefix](const std::string &taskName, + auto &task) -> bool { + auto ptr = m_qpTSID.problem.tsid->getTask(taskName).lock(); + if (ptr == nullptr) { + BipedalLocomotion::log()->error( + "{} Unable to get the task named {}.", logPrefix, taskName); + return false; + } + + // cast the task + task = std::dynamic_pointer_cast< + typename std::remove_reference::type::element_type>( + ptr); + if (task == nullptr) { + BipedalLocomotion::log()->error( + "{} Unable to cast the task named {} to the expected " + "type.", + logPrefix, taskName); + return false; + } + return true; + }; + m_qpTSID.problem = BipedalLocomotion::TSID::QPTSID::build(handler, kinDyn); + if (!m_qpTSID.problem.isValid()) { + BipedalLocomotion::log()->error("{} Unable to initialize the TSID.", + logPrefix); + return false; + } + + BipedalLocomotion::log()->info("{}", m_qpTSID.problem.tsid->toString()); + + return getTask("LEFT_FOOT_TRACKING", m_qpTSID.tasks.leftFootTracking) && + getTask("RIGHT_FOOT_TRACKING", m_qpTSID.tasks.rightFootTracking) && + getTask("COM_TRACKING", m_qpTSID.tasks.comTracking) // + && getTask("CHEST_TRACKING", m_qpTSID.tasks.torsoTracking) && + getTask("JOINT_TRACKING", m_qpTSID.tasks.jointTracking) && + getTask("ROOT_TRACKING", m_qpTSID.tasks.rootTracking) && + getTask("BASE_DYNAMICS", m_qpTSID.tasks.baseDynamics) && + getTask("JOINT_DYNAMICS", m_qpTSID.tasks.jointDynamics) && + getTask("LEFT_FOOT_FEASIBLE_WRENCH", + m_qpTSID.tasks.leftFeasibleContactWrench) && + getTask("RIGHT_FOOT_FEASIBLE_WRENCH", + m_qpTSID.tasks.rightFeasibleContactWrench) && + getTask("LEFT_FOOT_WRENCH_REGULARIZATION", + m_qpTSID.tasks.leftContactWrenchRegularization) && + getTask("RIGHT_FOOT_WRENCH_REGULARIZATION", + m_qpTSID.tasks.rightContactWrenchRegularization) && + getTask("TORQUE_REGULARIZATION", + m_qpTSID.tasks.torqueRegularization); + ; +} + +bool BLFTSID::solve() { + bool ok = m_qpTSID.problem.tsid->advance(); + ok = ok && m_qpTSID.problem.tsid->isOutputValid(); + + if (ok) { + iDynTree::toEigen(m_jointAccelerations) = + m_qpTSID.problem.tsid->getOutput().jointAccelerations; + iDynTree::toEigen(m_jointTorques) = + m_qpTSID.problem.tsid->getOutput().jointTorques; + } + + return ok; +} + +bool BLFTSID::setLeftFootTrackingSetPoint( + const iDynTree::Transform &desiredTransform, + const iDynTree::Twist &desiredVelocity, + const iDynTree::Twist &desiredAcceleration) { + return m_qpTSID.tasks.leftFootTracking->setSetPoint( + BipedalLocomotion::Conversions::toManifPose(desiredTransform), + BipedalLocomotion::Conversions::toManifTwist(desiredVelocity), + BipedalLocomotion::Conversions::toManifTwist(desiredAcceleration)); +} + +bool BLFTSID::setRightFootTrackingSetPoint( + const iDynTree::Transform &desiredTransform, + const iDynTree::Twist &desiredVelocity, + const iDynTree::Twist &desiredAcceleration) { + return m_qpTSID.tasks.rightFootTracking->setSetPoint( + BipedalLocomotion::Conversions::toManifPose(desiredTransform), + BipedalLocomotion::Conversions::toManifTwist(desiredVelocity), + BipedalLocomotion::Conversions::toManifTwist(desiredAcceleration)); +} + +bool BLFTSID::setJointTrackingSetPoint( + const iDynTree::VectorDynSize &jointPosition, + const iDynTree::VectorDynSize &jointVelocity, + const iDynTree::VectorDynSize &jointAcceleration) { + return m_qpTSID.tasks.jointTracking->setSetPoint( + iDynTree::toEigen(jointPosition), iDynTree::toEigen(jointVelocity), + iDynTree::toEigen(jointAcceleration)); +} + +bool BLFTSID::setCoMTrackingSetPoint(const iDynTree::Position &position, + const iDynTree::Vector3 &velocity, + const iDynTree::Vector3 &acceleration) { + return m_qpTSID.tasks.comTracking->setSetPoint( + iDynTree::toEigen(position), iDynTree::toEigen(velocity), + iDynTree::toEigen(acceleration)); +} + +bool BLFTSID::setRootTrackingSetPoint( + const iDynTree::Position &position, const iDynTree::Vector3 &linearVelocity, + const iDynTree::Vector3 &linearAcceleration) { + if (m_useRootLinkForHeight) { + return m_qpTSID.tasks.rootTracking->setSetPoint( + iDynTree::toEigen(position), iDynTree::toEigen(linearVelocity), + iDynTree::toEigen(linearAcceleration)); + } + return true; +} + +bool BLFTSID::setTorsoTrackingSetPoint( + const iDynTree::Rotation &rotation, + const iDynTree::Vector3 &angularVelocity, + const iDynTree::Vector3 &angularAcceleration) { + return m_qpTSID.tasks.torsoTracking->setSetPoint( + BipedalLocomotion::Conversions::toManifRot(rotation), + iDynTree::toEigen(angularVelocity), + iDynTree::toEigen(angularAcceleration)); +} + +bool BLFTSID::setAngularMomentumTrackingSetPoint( + const iDynTree::Vector3 &angularMomentum, + const iDynTree::Vector3 &angularMomentumRate) { + return m_qpTSID.tasks.angularMomentumTracking->setSetPoint( + iDynTree::toEigen(angularMomentum), + iDynTree::toEigen(angularMomentumRate)); +} + +bool BLFTSID::setLeftContactWrenchSetPoint( + const iDynTree::Wrench &contactWrench) { + return m_qpTSID.tasks.leftContactWrenchRegularization->setSetPoint( + iDynTree::toEigen(contactWrench)); +} + +bool BLFTSID::setRightContactWrenchSetPoint( + const iDynTree::Wrench &contactWrench) { + return m_qpTSID.tasks.rightContactWrenchRegularization->setSetPoint( + iDynTree::toEigen(contactWrench)); +} + +bool BLFTSID::setTorqueRegularizationSetPoint( + const iDynTree::VectorDynSize &torque) { + return m_qpTSID.tasks.torqueRegularization->setSetPoint( + iDynTree::toEigen(torque)); +} + +void BLFTSID::setLeftContactActive(bool isActive) { + m_qpTSID.tasks.leftFeasibleContactWrench->setContactActive(isActive); +} + +void BLFTSID::setRightContactActive(bool isActive) { + m_qpTSID.tasks.rightFeasibleContactWrench->setContactActive(isActive); +} + +const iDynTree::VectorDynSize &BLFTSID::getDesiredJointAcceleration() const { + return m_jointAccelerations; +} From 8badb810db6b7f1a116740c9234e6031e133316c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 25 Feb 2025 10:06:47 +0100 Subject: [PATCH 02/26] Add torque control interface and implementation for setting torque references --- .../RobotInterface/Helper.h | 12 +++++- src/RobotInterface/src/Helper.cpp | 41 +++++++++++++++++++ 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index e2637b6fd..bed3ee234 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -44,7 +45,8 @@ namespace WalkingControllers yarp::dev::IEncodersTimed *m_encodersInterface{nullptr}; /**< Encorders interface. */ yarp::dev::IPositionDirect *m_positionDirectInterface{nullptr}; /**< Direct position control interface. */ yarp::dev::IPositionControl *m_positionInterface{nullptr}; /**< Position control interface. */ - yarp::dev::IVelocityControl *m_velocityInterface{nullptr}; /**< Position control interface. */ + yarp::dev::IVelocityControl *m_velocityInterface{nullptr}; /**< velocity control interface. */ + yarp::dev::ITorqueControl *m_torqueInterface{nullptr}; /**< Torque control interface. */ yarp::dev::IControlMode *m_controlModeInterface{nullptr}; /**< Control mode interface. */ yarp::dev::IControlLimits *m_limitsInterface{nullptr}; /**< Encorders interface. */ yarp::dev::IInteractionMode *m_interactionInterface{nullptr}; /**< Stiff/compliant mode interface. */ @@ -181,6 +183,14 @@ namespace WalkingControllers */ bool setVelocityReferences(const iDynTree::VectorDynSize& desiredVelocityRad); + /** + * Set the desired torque reference. + * (The position will be sent using TorqueControl mode) + * @param desiredTorqueNm desired joints torque; + * @return true in case of success and false otherwise. + */ + bool setTorqueReferences(const iDynTree::VectorDynSize& desiredTorqueNm); + /** * Reset filters. * @return true in case of success and false otherwise. diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index cbf264222..14e77b6b6 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -345,6 +345,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) return false; } + if(!m_robotDevice.view(m_torqueInterface) || !m_torqueInterface) + { + yError() << "[configureRobot] Cannot obtain ITorqueControl interface"; + return false; + } + if(!m_robotDevice.view(m_controlModeInterface) || !m_controlModeInterface) { yError() << "[configureRobot] Cannot obtain IControlMode interface"; @@ -872,6 +878,41 @@ bool RobotInterface::setInteractionMode(std::vectorsetRefTorques(desiredTorqueNm.data())) + { + yError() << "[RobotInterface::setTorqueReferences] Error while setting the desired torques."; + return false; + } + + return true; +} + bool RobotInterface::setPositionReferences(const iDynTree::VectorDynSize& desiredJointPositionsRad, const double& positioningTimeSec) { From 595c2e37a131adc2572952b35979680cd2f3451a Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 25 Feb 2025 14:31:34 +0100 Subject: [PATCH 03/26] add Admittance controller and double integrator, and update Walking Module --- src/CMakeLists.txt | 2 + src/Integrators/CMakeLists.txt | 11 ++ .../Integrators/jointIntegrators.h | 79 ++++++++ src/Integrators/src/jointIntegrators.cpp | 169 +++++++++++++++++ src/JointControllers/CMakeLists.txt | 11 ++ .../JointControllers/AdmittanceController.h | 80 ++++++++ .../src/AdmittanceController.cpp | 139 ++++++++++++++ src/WalkingModule/CMakeLists.txt | 2 + .../common/joint_admittance_controller.ini | 13 ++ .../TaskSpaceInverseDynamicsBlf.ini | 6 +- .../dcm_walking_with_joypad.ini | 9 + .../WalkingControllers/WalkingModule/Module.h | 62 ++++++- src/WalkingModule/src/Module.cpp | 175 +++++++++++++++--- .../WholeBodyControllers/BLFTSID.h | 1 + src/WholeBodyControllers/src/BLFTSID.cpp | 4 + 15 files changed, 735 insertions(+), 28 deletions(-) create mode 100644 src/Integrators/CMakeLists.txt create mode 100644 src/Integrators/include/WalkingControllers/Integrators/jointIntegrators.h create mode 100644 src/Integrators/src/jointIntegrators.cpp create mode 100644 src/JointControllers/CMakeLists.txt create mode 100644 src/JointControllers/include/WalkingControllers/JointControllers/AdmittanceController.h create mode 100644 src/JointControllers/src/AdmittanceController.cpp create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e98705095..15103220d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,3 +13,5 @@ add_subdirectory(KinDynWrapper) add_subdirectory(RetargetingHelper) add_subdirectory(WalkingModule) add_subdirectory(JoypadModule) +add_subdirectory(JointControllers) +add_subdirectory(Integrators) diff --git a/src/Integrators/CMakeLists.txt b/src/Integrators/CMakeLists.txt new file mode 100644 index 000000000..369cafd36 --- /dev/null +++ b/src/Integrators/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (C) 2025 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Lorenzo Moretti + +add_walking_controllers_library( + NAME Integrators + SOURCES src/jointIntegrators.cpp + PUBLIC_HEADERS include/WalkingControllers/Integrators/jointIntegrators.h + PUBLIC_LINK_LIBRARIES BipedalLocomotion::ContinuousDynamicalSystem + BipedalLocomotion::TextLogging + Eigen3::Eigen) \ No newline at end of file diff --git a/src/Integrators/include/WalkingControllers/Integrators/jointIntegrators.h b/src/Integrators/include/WalkingControllers/Integrators/jointIntegrators.h new file mode 100644 index 000000000..990654d5e --- /dev/null +++ b/src/Integrators/include/WalkingControllers/Integrators/jointIntegrators.h @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include +#include + +#ifndef WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_JOINT_INTEGRATORS +#define WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_JOINT_INTEGRATORS + +namespace WalkingControllers { + +class JointAccelerationIntegrator { + + private: + std::shared_ptr< + BipedalLocomotion::ContinuousDynamicalSystem::LinearTimeInvariantSystem> + dynamics; + std::shared_ptr> + integrator; + + bool m_isInitialized{ + false}; /**< true if the integrator is successfully initialized */ + + int m_numberOfJoints{0}; /**< number of joints */ + + std::chrono::nanoseconds m_dT{0}; /**< time step */ + + Eigen::VectorXd m_jointPosition; /**< joint position */ + Eigen::VectorXd m_jointVelocity; /**< joint velocity */ + + public: + /** + * @brief Initialize the integrator. + * @param numberOfJoints number of joints. + * @param dt time step in seconds. + * @return true if successful. + */ + bool initialize(int numberOfJoints, double dt); + + /** + * @brief Set the input of the integrator. + * @param jointAcceleration joint acceleration. + * @return true if successful. + */ + bool setInput(const Eigen::Ref &jointAcceleration); + + /** + * @brief Set the state of the integrator. + * @param jointposition joint position. + * @param jointVelocity joint velocity. + * @return true if successful. + */ + bool setState(const Eigen::Ref &jointposition, + const Eigen::Ref &jointVelocity); + + /** + * @brief Perform one step integration. + * @return true if successful. + */ + bool oneStepIntegration(); + + /** + * @brief Get the joint position. + * @return joint position. + */ + const Eigen::VectorXd &getJointPosition() const; + + /** + * @brief Get the joint velocity. + * @return joint velocity. + */ + const Eigen::VectorXd &getJointVelocity() const; +}; + +} // namespace WalkingControllers + +#endif // WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_JOINT_INTEGRATORS \ No newline at end of file diff --git a/src/Integrators/src/jointIntegrators.cpp b/src/Integrators/src/jointIntegrators.cpp new file mode 100644 index 000000000..b6c1f7f7e --- /dev/null +++ b/src/Integrators/src/jointIntegrators.cpp @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include + +#include +#include + +using namespace WalkingControllers; + +bool JointAccelerationIntegrator::initialize(int numberOfJoints, double dt) { + + constexpr auto logPrefix = "[JointAccelerationIntegrator::initialize]"; + + dynamics = std::make_shared(); + + m_numberOfJoints = numberOfJoints; + + // Set joints state space system matrices. + // + // [s_dot; s_ddot] = [0, I; 0, 0] * [s; s_dot] + [0; I] * sddot + // + // therefore + // + // A = [0, I; 0, 0] + // b = [0; 1] + Eigen::MatrixXd A = + Eigen::MatrixXd::Zero(2 * m_numberOfJoints, 2 * m_numberOfJoints); + Eigen::MatrixXd B = + Eigen::MatrixXd::Zero(2 * m_numberOfJoints, m_numberOfJoints); + A.block(0, m_numberOfJoints, m_numberOfJoints, m_numberOfJoints) + .setIdentity(); + B.block(m_numberOfJoints, 0, m_numberOfJoints, m_numberOfJoints) + .setIdentity(); + + if (!dynamics->setSystemMatrices(A, B)) { + BipedalLocomotion::log()->error( + "{} Failed to set system matrices of the dynamical system", + logPrefix); + return false; + }; + + // set dynamical system of the integrator + integrator = + std::make_shared>(); + if (!integrator->setDynamicalSystem(dynamics)) { + BipedalLocomotion::log()->error( + "{} Failed to set dynamical system of the integrator", logPrefix); + return false; + }; + + // set integration step + m_dT = std::chrono::milliseconds(static_cast(dt * 1000)); + if (!integrator->setIntegrationStep(m_dT)) { + BipedalLocomotion::log()->error( + "{} Failed to set integration step of the integrator", logPrefix); + return false; + }; + + m_jointPosition.resize(m_numberOfJoints); + m_jointPosition.setZero(); + m_jointVelocity.resize(m_numberOfJoints); + m_jointVelocity.setZero(); + + m_isInitialized = true; + return true; +} + +bool JointAccelerationIntegrator::setInput( + const Eigen::Ref &jointAcceleration) { + + constexpr auto logPrefix = "[JointAccelerationIntegrator::setInput]"; + + if (!m_isInitialized) { + BipedalLocomotion::log()->error("{} Integrator is not initialized", + logPrefix); + return false; + } + + if (jointAcceleration.size() != m_numberOfJoints) { + BipedalLocomotion::log()->error("{} Size of joint acceleration is not " + "equal to the number of joints", + logPrefix); + return false; + } + + if (!dynamics->setControlInput({jointAcceleration})) { + BipedalLocomotion::log()->error( + "{} Failed to set input of the dynamical system", logPrefix); + return false; + } + + return true; +} + +bool JointAccelerationIntegrator::setState( + const Eigen::Ref &jointposition, + const Eigen::Ref &jointVelocity) { + + constexpr auto logPrefix = "[JointAccelerationIntegrator::setState]"; + + if (!m_isInitialized) { + BipedalLocomotion::log()->error("{} Integrator is not initialized", + logPrefix); + return false; + } + + if (jointposition.size() != m_numberOfJoints) { + BipedalLocomotion::log()->error( + "{} Size of joint position is not equal to the number of joints", + logPrefix); + return false; + } + + if (jointVelocity.size() != m_numberOfJoints) { + BipedalLocomotion::log()->error( + "{} Size of joint velocity is not equal to the number of joints", + logPrefix); + return false; + } + + Eigen::VectorXd state(2 * m_numberOfJoints); + state << jointposition, jointVelocity; + + if (!dynamics->setState({state})) { + BipedalLocomotion::log()->error( + "[JointAccelerationIntegrator::setState] Failed to set state of " + "the dynamical system"); + return false; + } + + return true; +} + +bool JointAccelerationIntegrator::oneStepIntegration() { + + constexpr auto logPrefix = + "[JointAccelerationIntegrator::oneStepIntegration]"; + + if (!m_isInitialized) { + BipedalLocomotion::log()->error("{} Integrator is not initialized", + logPrefix); + return false; + } + + if (!integrator->oneStepIntegration(std::chrono::nanoseconds(0), m_dT)) { + BipedalLocomotion::log()->error("{} Failed to advance the integrator", + logPrefix); + return false; + } + + const auto &[stateX] = integrator->getSolution(); + m_jointPosition = stateX.head(m_numberOfJoints); + m_jointVelocity = stateX.tail(m_numberOfJoints); + + return true; +} + +const Eigen::VectorXd &JointAccelerationIntegrator::getJointPosition() const { + const auto &[stateX] = integrator->getSolution(); + return m_jointPosition; +} + +const Eigen::VectorXd &JointAccelerationIntegrator::getJointVelocity() const { + return m_jointVelocity; +} \ No newline at end of file diff --git a/src/JointControllers/CMakeLists.txt b/src/JointControllers/CMakeLists.txt new file mode 100644 index 000000000..f8ca4ee86 --- /dev/null +++ b/src/JointControllers/CMakeLists.txt @@ -0,0 +1,11 @@ +# Copyright (C) 2025 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Lorenzo Moretti + +add_walking_controllers_library( + NAME JointControllers + SOURCES src/AdmittanceController.cpp + PUBLIC_HEADERS include/WalkingControllers/JointControllers/AdmittanceController.h + PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler + BipedalLocomotion::TextLogging + Eigen3::Eigen) diff --git a/src/JointControllers/include/WalkingControllers/JointControllers/AdmittanceController.h b/src/JointControllers/include/WalkingControllers/JointControllers/AdmittanceController.h new file mode 100644 index 000000000..cf47820be --- /dev/null +++ b/src/JointControllers/include/WalkingControllers/JointControllers/AdmittanceController.h @@ -0,0 +1,80 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_ADMITTANCE_CONTROLLER +#define WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_ADMITTANCE_CONTROLLER + +#include +#include + +#include + +namespace WalkingControllers { + +class AdmittanceController { + + struct Input { + Eigen::VectorXd jointTorqueFeedforward; /**< Joint position desired */ + Eigen::VectorXd jointPositionDesired; /**< Joint position desired */ + Eigen::VectorXd jointVelocityDesired; /**< Joint velocity desired*/ + Eigen::VectorXd jointPosition; /**< Joint position */ + Eigen::VectorXd jointVelocity; /**< Joint velocity */ + }; + Input m_input; /**< Input of the admittance controller */ + + struct Output { + Eigen::VectorXd jointTorque; /**< Joint Torque */ + }; + Output m_output; /**< Output of the admittance controller */ + + bool m_isInitialized{false}; /**< true if the admittance controller is + successfully initialized */ + struct Gains { + Eigen::VectorXd kp; /**< Proportional gains */ + Eigen::VectorXd kd; /**< Derivative gains */ + }; + Gains m_gains; /**< gains of the admittance controller */ + + int m_numberOfJoints; /**< Number of controlled joints */ + + public: + AdmittanceController() = default; + + ~AdmittanceController() = default; + + /** + * @brief Initialize the admittance controller. + * @param parametersHandler parameters handler. + * @return true if successful. + */ + bool + initialize(std::shared_ptr< + const BipedalLocomotion::ParametersHandler::IParametersHandler> + parametersHandler); + /** + * @brief Advance the admittance controller. + * @return true if successful. + */ + bool advance(); + + /** + * @brief Set the input to the admittance controller. + * @return output of the admittance controller. + */ + bool + setInput(const Eigen::Ref &jointTorqueFeedforward, + const Eigen::Ref &jointPositionDesired, + const Eigen::Ref &jointVelocityDesired, + const Eigen::Ref &jointPosition, + const Eigen::Ref &jointVelocity); + + /** + * @brief Get the output of the admittance controller (i.e., joint torques). + * @return output of the admittance controller. + */ + const Eigen::VectorXd &getOutput() const; +}; + +} // namespace WalkingControllers + +#endif // WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_ADMITTANCE_CONTROLLER \ No newline at end of file diff --git a/src/JointControllers/src/AdmittanceController.cpp b/src/JointControllers/src/AdmittanceController.cpp new file mode 100644 index 000000000..9c6f4a389 --- /dev/null +++ b/src/JointControllers/src/AdmittanceController.cpp @@ -0,0 +1,139 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include + +#include + +using namespace WalkingControllers; + +bool AdmittanceController::initialize( + std::shared_ptr< + const BipedalLocomotion::ParametersHandler::IParametersHandler> + parametersHandler) { + + constexpr auto logPrefix = "[AdmittanceController::initialize]"; + + if (parametersHandler == nullptr) { + BipedalLocomotion::log()->error( + "{} The parameter handler is not valid.", logPrefix); + return false; + } + + if (!parametersHandler->getParameter("number_of_joints", + m_numberOfJoints)) { + BipedalLocomotion::log()->error( + "{} Unable to find the number_of_joints parameter.", logPrefix); + return false; + } + + auto gainsHandler = parametersHandler->getGroup("GAINS").lock(); + if (gainsHandler == nullptr) { + BipedalLocomotion::log()->error("{} Unable to find the gains group.", + logPrefix); + return false; + } + + if (!gainsHandler->getParameter("kp", m_gains.kp)) { + BipedalLocomotion::log()->error("{} Unable to find the kp parameter.", + logPrefix); + return false; + } + + if (!gainsHandler->getParameter("kd", m_gains.kd)) { + BipedalLocomotion::log()->error("{} Unable to find the kd parameter.", + logPrefix); + return false; + } + + if (m_gains.kp.size() != m_numberOfJoints) { + BipedalLocomotion::log()->error( + "{} The size of the proportional gain vector is " + "not equal to the number of joints.", + logPrefix); + return false; + } + + if (m_gains.kd.size() != m_numberOfJoints) { + BipedalLocomotion::log()->error( + "{} The size of the derivative gain vector " + "is not equal to the number of joints.", + logPrefix); + return false; + } + + m_gains.kp.resize(m_numberOfJoints); + m_gains.kd.resize(m_numberOfJoints); + + m_input.jointTorqueFeedforward.resize(m_numberOfJoints); + m_input.jointPositionDesired.resize(m_numberOfJoints); + m_input.jointVelocityDesired.resize(m_numberOfJoints); + m_input.jointPosition.resize(m_numberOfJoints); + m_input.jointVelocity.resize(m_numberOfJoints); + m_output.jointTorque.resize(m_numberOfJoints); + + m_isInitialized = true; + + return true; +}; + +bool AdmittanceController::setInput( + const Eigen::Ref &jointTorqueFeedforward, + const Eigen::Ref &jointPositionDesired, + const Eigen::Ref &jointVelocityDesired, + const Eigen::Ref &jointPosition, + const Eigen::Ref &jointVelocity) { + + if (!m_isInitialized) { + return false; + } + if (jointTorqueFeedforward.size() != m_numberOfJoints) { + return false; + } + if (jointPositionDesired.size() != m_numberOfJoints) { + return false; + } + if (jointVelocityDesired.size() != m_numberOfJoints) { + return false; + } + if (jointPosition.size() != m_numberOfJoints) { + return false; + } + if (jointVelocity.size() != m_numberOfJoints) { + return false; + } + + m_input.jointPositionDesired = jointPositionDesired; + m_input.jointVelocityDesired = jointVelocityDesired; + m_input.jointPosition = jointPosition; + m_input.jointVelocity = jointVelocity; + + return true; +}; + +const Eigen::VectorXd & +AdmittanceController::getOutput() const { + return m_output.jointTorque; +}; + +bool AdmittanceController::advance() { + + constexpr auto logPrefix = "[AdmittanceController::advance]"; + + m_output.jointTorque.setZero(); + + if (!m_isInitialized) { + BipedalLocomotion::log()->error( + "{} The admittance controller is not initialized.", logPrefix); + return false; + } + + m_output.jointTorque = + m_gains.kp.cwiseProduct(m_input.jointPositionDesired - + m_input.jointPosition) + + m_gains.kd.cwiseProduct(m_input.jointVelocityDesired - + m_input.jointVelocity) + + m_input.jointTorqueFeedforward; + + return true; +}; diff --git a/src/WalkingModule/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index a7e2d0d36..1dbca0681 100644 --- a/src/WalkingModule/CMakeLists.txt +++ b/src/WalkingModule/CMakeLists.txt @@ -18,6 +18,8 @@ add_walking_controllers_application( WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers + WalkingControllers::JointControllers + WalkingControllers::Integrators WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection BipedalLocomotion::ParametersHandlerYarpImplementation diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini new file mode 100644 index 000000000..4bf7d4009 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini @@ -0,0 +1,13 @@ +[GAINS] + +kp (300.0, 300.0, 300.0, + 300.0, 300.0, 300.0, 300.0, + 300.0, 300.0, 300.0, 300.0, + 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, + 300.0, 300.0, 300.0, 300.0, 300.0, 300.0) + +kd (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.1, 0.1, 0.1, 0.1, + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index 421dc3ed2..bbb9f9cdf 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -89,13 +89,13 @@ robot_acceleration_variable_name "robot_acceleration" type JointTrackingTask priority 1 -kp (5.0, 5.0, +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) -weight (10.0, 10.0, +weight (10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 @@ -150,7 +150,7 @@ variable_name "joint_torques" variable_size 22 priority 1 -weight (0.01, 0.01, +weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 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..7a7917f90 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 @@ -4,6 +4,9 @@ # Remove this line if you don't want to use the QP-IK use_QP-IK 1 +; # Remove this line if you don't want to use the TSID-Admittance control +; use_TSID-Admittance 1 + # remove this line if you don't want to save data of the experiment dump_data 1 @@ -59,6 +62,12 @@ height_reference_frame root_link # include qp inverse kinematcs parameters [include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematicsBlf.ini"] +# include qp task space inverse dynamics parameters +[include TASK_SPACE_INVERSE_DYNAMICS "./dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini"] + +# include joint admittance controller parameters +[include JOINT_ADMITTANCE_CONTROLLER "./dcm_walking/common/joint_admittance_controller.ini"] + # include inverse kinematcs parameters [include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 781e79375..9c4e0b2b3 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -5,8 +5,9 @@ #define WALKING_MODULE_HPP // std -#include #include +#include + #include #include #include @@ -17,7 +18,8 @@ #include - +#include +#include #include #include #include @@ -39,6 +41,11 @@ #include #include +#include +#include + +#include +#include #include @@ -70,6 +77,7 @@ namespace WalkingControllers bool m_useMPC; /**< True if the MPC controller is used. */ bool m_useQPIK; /**< True if the QP-IK is used. */ + bool m_useTSIDadmittance; /**< True if the TSID admittance controller is used. */ bool m_dumpData; /**< True if data are saved. */ bool m_firstRun; /**< True if it is the first run. */ bool m_skipDCMController; /**< True if the desired ZMP should be used instead of the DCM controller. */ @@ -88,6 +96,9 @@ namespace WalkingControllers std::unique_ptr m_IKSolver; /**< Pointer to the inverse kinematics solver. */ std::unique_ptr m_BLFIKSolver; /**< Pointer to the integration based ik. */ std::unique_ptr m_FKSolver; /**< Pointer to the forward kinematics solver. */ + std::unique_ptr m_BLFTSIDSolver; /**< Pointer to the task space inverse dynamics. */ + std::unique_ptr m_jointAdmittanceController; /**< Pointer to the joint Admittance Controller. */ + std::unique_ptr m_jointAccelerationIntegrator; /**< Pointer to the joint acceleration integrator. */ std::unique_ptr m_stableDCMModel; /**< Pointer to the stable DCM dynamics. */ std::unique_ptr m_PIDHandler; /**< Pointer to the PID handler object. */ std::unique_ptr m_retargetingClient; /**< Pointer to the stable DCM dynamics. */ @@ -95,6 +106,18 @@ namespace WalkingControllers std::unique_ptr m_transformHelper; /**< Transform server/client helper. */ BipedalLocomotion::Contacts::GlobalCoPEvaluator m_globalCoPEvaluator; + struct JointsKinematics { + std::shared_ptr< + BipedalLocomotion::ContinuousDynamicalSystem::LinearTimeInvariantSystem> + dynamics; + std::shared_ptr> + integrator; + }; + + JointsKinematics m_jointsAccelerationIntegrator; /**< Double integrator of joint accelerations */ + std::vector> m_framesToStream; /**< Frames to send to the transform server. */ double m_additionalRotationWeightDesired; /**< Desired additional rotational weight matrix. */ @@ -124,8 +147,14 @@ namespace WalkingControllers iDynTree::ModelLoader m_loader; /**< Model loader class. */ - iDynTree::VectorDynSize m_qDesired; /**< Vector containing the results of the IK algorithm [rad]. */ - iDynTree::VectorDynSize m_dqDesired; /**< Vector containing the results of the IK algorithm [rad]. */ + iDynTree::VectorDynSize m_qDesiredIK; /**< Vector containing the results of the IK algorithm [rad]. */ + iDynTree::VectorDynSize m_dqDesiredIK; /**< Vector containing the results of the IK algorithm [rad]. */ + + iDynTree::VectorDynSize m_qDesiredTSID; /**< Vector containing the desired joint position [rad]. */ + iDynTree::VectorDynSize m_dqDesiredTSID; /**< Vector containing the desired joint velocity [rad/s]. */ + iDynTree::VectorDynSize m_ddqDesiredTSID; /**< Vector containing the desired joint acceleration [rad/s^2]. */ + iDynTree::VectorDynSize m_desiredJointTorquesTSID; /**< Vector containing the desired joint torques for the low-level [Nm]. */ + iDynTree::VectorDynSize m_desiredJointTorquesLL; /**< Vector containing the desired joint torques for the low-level [Nm]. */ iDynTree::Rotation m_inertial_R_worldFrame; /**< Rotation between the inertial and the world frame. */ @@ -189,6 +218,31 @@ namespace WalkingControllers const iDynTree::Rotation& desiredNeckOrientation, iDynTree::VectorDynSize &output); + bool solveBLFTSID(iDynTree::VectorDynSize &output); + + /** + * Get the desired joint acceleration and torque computed by the TSID controller. + * @param jointDesiredAcceleration is the desired joint acceleration; + * @param jointDesiredTorque is the desired joint torque. + */ + void getBLFTSIDOutput(iDynTree::VectorDynSize &jointDesiredAcceleration, + iDynTree::VectorDynSize &jointDesiredTorque); + + /** + * Set the Admittance controller references, advance it, and return the output. + * @param jointTorqueFeedforward joint torque feedforward term. + * @param jointDesiredPosition joint desired position. + * @param jointDesiredVelocity joint desired velocity. + * @param jointMeasuredPosition joint measured position. + * @param jointMeasuredVelocity joint measured velocity. + * @param jointDesiredTorque joint desired torque, computed by the Admittance controller. + * @return true if successful. + */ + bool advanceJointAdmittanceController(const iDynTree::VectorDynSize & jointTorqueFeedforward, + const iDynTree::VectorDynSize & jointDesiredPosition, + const iDynTree::VectorDynSize & jointDesiredVelocity, + iDynTree::VectorDynSize & jointDesiredTorque); + /** * Compute Global CoP. * @param globalCoP is the global CoP. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 7d1ea4ffd..4bdaca61d 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -32,7 +32,6 @@ #include #include #include -#include using namespace WalkingControllers; @@ -133,6 +132,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) // module name (used as prefix for opened ports) m_useMPC = rf.check("use_mpc", yarp::os::Value(false)).asBool(); m_useQPIK = rf.check("use_QP-IK", yarp::os::Value(false)).asBool(); + m_useTSIDadmittance = rf.check("use_TSID-Admittance", yarp::os::Value(false)).asBool(); m_dumpData = rf.check("dump_data", yarp::os::Value(false)).asBool(); m_maxInitialCoMVelocity = rf.check("max_initial_com_vel", yarp::os::Value(1.0)).asFloat64(); std::string goalSuffix = rf.check("goal_port_suffix", yarp::os::Value("/goal:i")).asString(); @@ -314,6 +314,37 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) } } + // initialize the TSID admittance controller + if (m_useTSIDadmittance) + { + yarp::os::Bottle &TSIDOptions = rf.findGroup("TASK_SPACE_INVERSE_DYNAMICS"); + m_BLFTSIDSolver = std::make_unique(); + auto paramHandler = std::make_shared(); + paramHandler->set(TSIDOptions); + paramHandler->setParameter("use_root_link_for_height", m_useRootLinkForHeight); + if (!m_BLFTSIDSolver->initialize(paramHandler, m_FKSolver->getKinDyn())) + { + yError() << "[WalkingModule::configure] Failed to configure the TSID controller"; + return false; + } + + yarp::os::Bottle &AdmittanceControllerOptions = rf.findGroup("JOINT_ADMITTANCE_CONTROLLER"); + m_jointAdmittanceController = std::make_unique(); + paramHandler->set(AdmittanceControllerOptions); + paramHandler->setParameter("number_of_joints", static_cast(m_robotControlHelper->getActuatedDoFs())); + if (!m_jointAdmittanceController->initialize(paramHandler)) + { + yError() << "[WalkingModule::configure] Failed to configure the Joint Admittance controller"; + return false; + } + + m_jointAccelerationIntegrator = std::make_unique(); + m_jointAccelerationIntegrator->initialize(m_robotControlHelper->getActuatedDoFs(), m_dT); + m_jointAccelerationIntegrator->setState(iDynTree::toEigen(m_robotControlHelper->getJointPosition()), + iDynTree::toEigen(m_robotControlHelper->getJointVelocity())); + } + + // initialize the linear inverted pendulum model m_stableDCMModel = std::make_unique(); if (!m_stableDCMModel->initialize(generalOptions)) @@ -456,6 +487,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_profiler->addTimer("MPC"); m_profiler->addTimer("IK"); + m_profiler->addTimer("TSID"); m_profiler->addTimer("Total"); m_profiler->addTimer("Loop"); m_profiler->addTimer("Feedback"); @@ -468,8 +500,11 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_inertial_R_worldFrame = iDynTree::Rotation::Identity(); // resize variables - m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); - m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); + m_qDesiredIK.resize(m_robotControlHelper->getActuatedDoFs()); + m_dqDesiredIK.resize(m_robotControlHelper->getActuatedDoFs()); + m_qDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); + m_dqDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); + m_ddqDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -555,6 +590,45 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition, return ok; } +bool WalkingModule::solveBLFTSID(iDynTree::VectorDynSize &output) +{ + + bool ok = m_BLFTSIDSolver->solve(); + + if (ok) + { + output = m_BLFTSIDSolver->getDesiredJointAcceleration(); + } + + return ok; +} + +void WalkingModule::getBLFTSIDOutput(iDynTree::VectorDynSize &jointDesiredAcceleration, + iDynTree::VectorDynSize &jointDesiredTorque) +{ + jointDesiredAcceleration = m_BLFTSIDSolver->getDesiredJointAcceleration(); + jointDesiredTorque = m_BLFTSIDSolver->getDesiredJointTorque(); +} + +bool WalkingModule::advanceJointAdmittanceController(const iDynTree::VectorDynSize & jointTorqueFeedforward, + const iDynTree::VectorDynSize & jointDesiredPosition, + const iDynTree::VectorDynSize & jointDesiredVelocity, + iDynTree::VectorDynSize & jointDesiredTorque) +{ + bool ok = m_jointAdmittanceController->setInput(iDynTree::toEigen(jointTorqueFeedforward), + iDynTree::toEigen(jointDesiredPosition), + iDynTree::toEigen(jointDesiredVelocity), + iDynTree::toEigen(m_robotControlHelper->getJointPosition()), + iDynTree::toEigen(m_robotControlHelper->getJointVelocity())); + + ok = ok && m_jointAdmittanceController->advance(); + if (ok) + { + iDynTree::toEigen(jointDesiredTorque) = m_jointAdmittanceController->getOutput(); + } + return ok; +} + bool WalkingModule::computeGlobalCoP(Eigen::Ref globalCoP) { BipedalLocomotion::Contacts::ContactWrench leftFootContact, rightFootContact; @@ -605,7 +679,7 @@ bool WalkingModule::updateModule() if (motionDone) { // send the reference again in order to reduce error - if (!m_robotControlHelper->setDirectPositionReferences(m_qDesired)) + if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredIK)) { yError() << "[prepareRobot] Error while setting the initial position using " << "POSITION DIRECT mode."; @@ -615,8 +689,8 @@ bool WalkingModule::updateModule() return true; } - yarp::sig::Vector buffer(m_qDesired.size()); - iDynTree::toYarp(m_qDesired, buffer); + yarp::sig::Vector buffer(m_qDesiredIK.size()); + iDynTree::toYarp(m_qDesiredIK, buffer); // instantiate Integrator object yarp::sig::Matrix jointLimits(m_robotControlHelper->getActuatedDoFs(), 2); @@ -645,10 +719,10 @@ bool WalkingModule::updateModule() } // reset the retargeting client with the desired robot data - iDynTree::VectorDynSize zero(m_qDesired.size()); + iDynTree::VectorDynSize zero(m_qDesiredIK.size()); zero.zero(); // reset the internal robot state of the kindyn object - if (!m_FKSolver->setInternalRobotState(m_qDesired, zero)) + if (!m_FKSolver->setInternalRobotState(m_qDesiredIK, zero)) { yError() << "[WalkingModule::updateModule] Unable to set the robot state before resetting the retargeting client."; return false; @@ -938,7 +1012,7 @@ bool WalkingModule::updateModule() yarp::sig::Vector bufferVelocity(m_robotControlHelper->getActuatedDoFs()); yarp::sig::Vector bufferPosition(m_robotControlHelper->getActuatedDoFs()); - if (!m_FKSolver->setInternalRobotState(m_qDesired, m_dqDesired)) + if (!m_FKSolver->setInternalRobotState(m_qDesiredIK, m_dqDesiredIK)) { yError() << "[WalkingModule::updateModule] Unable to set the internal robot state."; return false; @@ -949,17 +1023,17 @@ bool WalkingModule::updateModule() if (!solveBLFIK(desiredCoMPosition, desiredCoMVelocity, yawRotation, - m_dqDesired)) + m_dqDesiredIK)) { yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with " "blf ik."; return false; } - iDynTree::toYarp(m_dqDesired, bufferVelocity); + iDynTree::toYarp(m_dqDesiredIK, bufferVelocity); bufferPosition = m_velocityIntegral->integrate(bufferVelocity); - iDynTree::toiDynTree(bufferPosition, m_qDesired); + iDynTree::toiDynTree(bufferPosition, m_qDesiredIK); if (!m_FKSolver->setInternalRobotState(m_robotControlHelper->getJointPosition(), m_robotControlHelper->getJointVelocity())) @@ -985,7 +1059,7 @@ bool WalkingModule::updateModule() } if (!m_IKSolver->computeIK(m_leftTrajectory.front(), m_rightTrajectory.front(), - desiredCoMPosition, m_qDesired)) + desiredCoMPosition, m_qDesiredIK)) { yError() << "[WalkingModule::updateModule] Error during the inverse Kinematics iteration."; return false; @@ -1017,10 +1091,69 @@ bool WalkingModule::updateModule() } } - if (!m_robotControlHelper->setDirectPositionReferences(m_qDesired)) - { - yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; - return false; + // tsid-admittance + m_profiler->setInitTime("TSID"); + if (m_useTSIDadmittance){ + + // set robot state to TSID joint values + if (!m_FKSolver->setInternalRobotState(m_qDesiredTSID, m_dqDesiredTSID)) + { + yError() << "[WalkingModule::updateModule] Unable to set the internal robot state."; + return false; + } + + if(!solveBLFTSID(m_ddqDesiredTSID)) + { + yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; + return false; + } + + getBLFTSIDOutput(m_ddqDesiredTSID, m_desiredJointTorquesTSID); + + // double integration of joint acceleration to get joint position + m_jointAccelerationIntegrator->setInput(iDynTree::toEigen(m_ddqDesiredTSID)); + m_jointAccelerationIntegrator->oneStepIntegration(); + iDynTree::toEigen(m_dqDesiredTSID) = m_jointAccelerationIntegrator->getJointVelocity(); + iDynTree::toEigen(m_qDesiredTSID) = m_jointAccelerationIntegrator->getJointPosition(); + + // admittance controller + advanceJointAdmittanceController(m_desiredJointTorquesTSID, + m_qDesiredTSID, + m_dqDesiredTSID, + m_desiredJointTorquesLL); + + // restore robot state + if (!m_FKSolver->setInternalRobotState(m_robotControlHelper->getJointPosition(), + m_robotControlHelper->getJointVelocity())) + { + yError() << "[WalkingModule::updateModule] Unable to set the internal robot state."; + return false; + } + + + } + m_profiler->setEndTime("TSID"); + + + if (m_useTSIDadmittance){ + + // if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorques)) + // { + // yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + // return false; + // } + if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + { + yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + return false; + } + + } else { + if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredIK)) + { + yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + return false; + } } // send data to the logger @@ -1121,7 +1254,7 @@ bool WalkingModule::updateModule() // Joint m_vectorsCollectionServer.populateData("joints_state::positions::measured", m_robotControlHelper->getJointPosition()); - m_vectorsCollectionServer.populateData("joints_state::positions::desired", m_qDesired); + m_vectorsCollectionServer.populateData("joints_state::positions::desired", (m_useTSIDadmittance)? m_qDesiredTSID : m_qDesiredIK); m_vectorsCollectionServer.populateData("joints_state::positions::retargeting", m_retargetingClient->jointPositions()); m_vectorsCollectionServer.populateData("joints_state::positions::retargeting_raw", m_retargetingClient->rawJointPositions()); m_vectorsCollectionServer.populateData("joints_state::velocities::measured", m_robotControlHelper->getJointVelocity()); @@ -1264,15 +1397,15 @@ bool WalkingModule::prepareRobot(bool onTheFly) } if (!m_IKSolver->computeIK(m_leftTrajectory.front(), m_rightTrajectory.front(), - desiredCoMPosition, m_qDesired)) + desiredCoMPosition, m_qDesiredIK)) { yError() << "[WalkingModule::prepareRobot] Inverse Kinematics failed while computing the initial position."; return false; } - std::cerr << "q desired IK " << Eigen::VectorXd(iDynTree::toEigen(m_qDesired) * 180 / M_PI).transpose() << std::endl; + std::cerr << "q desired IK " << Eigen::VectorXd(iDynTree::toEigen(m_qDesiredIK) * 180 / M_PI).transpose() << std::endl; - if (!m_robotControlHelper->setPositionReferences(m_qDesired, 5.0)) + if (!m_robotControlHelper->setPositionReferences(m_qDesiredIK, 5.0)) { yError() << "[WalkingModule::prepareRobot] Error while setting the initial position."; return false; diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h index f4f9729b2..b2caf6b17 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h @@ -76,6 +76,7 @@ class BLFTSID { void setRightContactActive(bool isActive); const iDynTree::VectorDynSize &getDesiredJointAcceleration() const; + const iDynTree::VectorDynSize &getDesiredJointTorque() const; private: struct TSIDProblemAndTasks { diff --git a/src/WholeBodyControllers/src/BLFTSID.cpp b/src/WholeBodyControllers/src/BLFTSID.cpp index 983062375..929fe020a 100644 --- a/src/WholeBodyControllers/src/BLFTSID.cpp +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -186,3 +186,7 @@ void BLFTSID::setRightContactActive(bool isActive) { const iDynTree::VectorDynSize &BLFTSID::getDesiredJointAcceleration() const { return m_jointAccelerations; } + +const iDynTree::VectorDynSize &BLFTSID::getDesiredJointTorque() const { + return m_jointTorques; +} \ No newline at end of file From c3f790413083922785a555a3a830c087e1cc52f1 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 25 Feb 2025 14:57:58 +0100 Subject: [PATCH 04/26] fix TSID config files and TSID task names --- .../joypad_control/TaskSpaceInverseDynamicsBlf.ini | 8 +++++++- .../joypad_control/tsid_tasks/angular_momentum_task.ini | 2 ++ .../robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini | 4 ++-- src/WholeBodyControllers/src/BLFTSID.cpp | 2 +- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index bbb9f9cdf..cc0663fe7 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -66,6 +66,7 @@ frame_name "chest" priority 1 weight (100.0, 100.0, 100.0) robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" [ROOT_TRACKING] @@ -77,6 +78,7 @@ mask (false, false, true) priority 0 weight (1000.0) robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" [JOINT_TRACKING] @@ -101,6 +103,7 @@ weight (10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 10.0, 10.0, 10.0, 10.0, 10.0, 10.0) robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" [LEFT_FOOT_FEASIBLE_WRENCH] @@ -131,6 +134,7 @@ foot_limits_y (-0.05, 0.05) [LEFT_FOOT_WRENCH_REGULARIZATION] type "VariableRegularizationTask" variable_name "lf_wrench" +weight_provider_type "ConstantWeightProvider" variable_size 6 priority 1 @@ -139,6 +143,7 @@ weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) [RIGHT_FOOT_WRENCH_REGULARIZATION] type "VariableRegularizationTask" variable_name "rf_wrench" +weight_provider_type "ConstantWeightProvider" variable_size 6 priority 1 @@ -147,8 +152,9 @@ weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) [TORQUE_REGULARIZATION] type "VariableRegularizationTask" variable_name "joint_torques" +weight_provider_type "ConstantWeightProvider" -variable_size 22 +variable_size 23 priority 1 weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini index ff09bc0d4..cdd8f7d03 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini @@ -5,6 +5,8 @@ priority 1 use_exogenous_feedback false weight (500.0, 500.0, 500.0) max_number_of_contacts 2 +weight_provider_type "ConstantWeightProvider" + [CONTACT_0] variable_name "lf_wrench" 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 7a7917f90..2916c13af 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 @@ -4,8 +4,8 @@ # Remove this line if you don't want to use the QP-IK use_QP-IK 1 -; # Remove this line if you don't want to use the TSID-Admittance control -; use_TSID-Admittance 1 +# Remove this line if you don't want to use the TSID-Admittance control +use_TSID-Admittance 1 # remove this line if you don't want to save data of the experiment dump_data 1 diff --git a/src/WholeBodyControllers/src/BLFTSID.cpp b/src/WholeBodyControllers/src/BLFTSID.cpp index 929fe020a..9152769a4 100644 --- a/src/WholeBodyControllers/src/BLFTSID.cpp +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -59,7 +59,7 @@ bool BLFTSID::initialize( return getTask("LEFT_FOOT_TRACKING", m_qpTSID.tasks.leftFootTracking) && getTask("RIGHT_FOOT_TRACKING", m_qpTSID.tasks.rightFootTracking) && getTask("COM_TRACKING", m_qpTSID.tasks.comTracking) // - && getTask("CHEST_TRACKING", m_qpTSID.tasks.torsoTracking) && + && getTask("TORSO_TRACKING", m_qpTSID.tasks.torsoTracking) && getTask("JOINT_TRACKING", m_qpTSID.tasks.jointTracking) && getTask("ROOT_TRACKING", m_qpTSID.tasks.rootTracking) && getTask("BASE_DYNAMICS", m_qpTSID.tasks.baseDynamics) && From c6114c1040c28c9e054c674ad3bed494c9378004 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 25 Feb 2025 16:16:55 +0100 Subject: [PATCH 05/26] set setpoints of TSID and properly initialize joint acceleration integrator --- .../TaskSpaceInverseDynamicsBlf.ini | 6 ++ .../WalkingControllers/WalkingModule/Module.h | 5 +- src/WalkingModule/src/Module.cpp | 88 +++++++++++++++++-- 3 files changed, 90 insertions(+), 9 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index cc0663fe7..a5f4c1bf0 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -97,6 +97,12 @@ 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) +kd (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, 1.0, 1.0, 1.0, 1.0 + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + weight (10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 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 9c4e0b2b3..64f491244 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -5,6 +5,7 @@ #define WALKING_MODULE_HPP // std +#include #include #include @@ -218,7 +219,9 @@ namespace WalkingControllers const iDynTree::Rotation& desiredNeckOrientation, iDynTree::VectorDynSize &output); - bool solveBLFTSID(iDynTree::VectorDynSize &output); + bool solveBLFTSID(const iDynTree::Position& desiredCoMPosition, + const iDynTree::Vector3& desiredCoMVelocity, + const iDynTree::Rotation& desiredTorsoRotation); /** * Get the desired joint acceleration and torque computed by the TSID controller. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 4bdaca61d..5ed56d499 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -340,8 +340,6 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_jointAccelerationIntegrator = std::make_unique(); m_jointAccelerationIntegrator->initialize(m_robotControlHelper->getActuatedDoFs(), m_dT); - m_jointAccelerationIntegrator->setState(iDynTree::toEigen(m_robotControlHelper->getJointPosition()), - iDynTree::toEigen(m_robotControlHelper->getJointVelocity())); } @@ -505,6 +503,8 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_qDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); m_ddqDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); + m_desiredJointTorquesTSID.resize(m_robotControlHelper->getActuatedDoFs()); + m_desiredJointTorquesLL.resize(m_robotControlHelper->getActuatedDoFs()); yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -590,16 +590,60 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition, return ok; } -bool WalkingModule::solveBLFTSID(iDynTree::VectorDynSize &output) +bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, + const iDynTree::Vector3& desiredCoMVelocity, + const iDynTree::Rotation& desiredTorsoRotation) { + iDynTree::Vector3 zero3dVector; + zero3dVector.zero(); - bool ok = m_BLFTSIDSolver->solve(); + iDynTree::Twist zeroTwist; + zeroTwist.zero(); - if (ok) + iDynTree::VectorDynSize zeroNdofVector(m_robotControlHelper->getActuatedDoFs()); + zeroNdofVector.zero(); + + bool isLeftInContact = m_leftInContact.front(); + bool isRightInContact = m_rightInContact.front(); + + double robotMass = m_FKSolver->getKinDyn()->getRobotModel().getTotalMass(); + iDynTree::Vector3 robotWeight; + robotWeight.zero(); + robotWeight(2) = -9.81 * robotMass; + + iDynTree::Wrench leftContactWrench; + iDynTree::Wrench rightContactWrench; + leftContactWrench.zero(); + rightContactWrench.zero(); + + if (isLeftInContact) + { + leftContactWrench.setLinearVec3(robotWeight); + } + if (isRightInContact) { - output = m_BLFTSIDSolver->getDesiredJointAcceleration(); + rightContactWrench.setLinearVec3(robotWeight); } + bool ok{true}; + ok = ok && m_BLFTSIDSolver->setCoMTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); + ok = ok && m_BLFTSIDSolver->setLeftFootTrackingSetPoint(m_leftTrajectory.front(), m_leftTwistTrajectory.front(), zeroTwist); + ok = ok && m_BLFTSIDSolver->setRightFootTrackingSetPoint(m_rightTrajectory.front(), m_rightTwistTrajectory.front(), zeroTwist); + ok = ok && m_BLFTSIDSolver->setJointTrackingSetPoint(m_qDesiredIK, m_dqDesiredIK, zeroNdofVector); + ok = ok && m_BLFTSIDSolver->setTorsoTrackingSetPoint(desiredTorsoRotation, zero3dVector, zero3dVector); + m_BLFTSIDSolver->setLeftContactActive(isLeftInContact); + m_BLFTSIDSolver->setRightContactActive(isRightInContact); + ok = ok && m_BLFTSIDSolver->setLeftContactWrenchSetPoint(leftContactWrench); + ok = ok && m_BLFTSIDSolver->setRightContactWrenchSetPoint(rightContactWrench); + ok = ok && m_BLFTSIDSolver->setTorqueRegularizationSetPoint(zeroNdofVector); + + if (m_useRootLinkForHeight) + { + ok = ok && m_BLFTSIDSolver->setRootTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); + } + + ok = ok && m_BLFTSIDSolver->solve(); + return ok; } @@ -691,7 +735,7 @@ bool WalkingModule::updateModule() yarp::sig::Vector buffer(m_qDesiredIK.size()); iDynTree::toYarp(m_qDesiredIK, buffer); - // instantiate Integrator object + // instantiate Integrators object yarp::sig::Matrix jointLimits(m_robotControlHelper->getActuatedDoFs(), 2); for (int i = 0; i < m_robotControlHelper->getActuatedDoFs(); i++) @@ -701,6 +745,13 @@ bool WalkingModule::updateModule() } m_velocityIntegral = std::make_unique(m_dT, buffer, jointLimits); + if (m_useTSIDadmittance){ + m_qDesiredTSID = m_qDesiredIK; + m_dqDesiredTSID = m_dqDesiredIK; + m_jointAccelerationIntegrator->setState(iDynTree::toEigen(m_qDesiredTSID), + iDynTree::toEigen(m_dqDesiredTSID)); + } + // reset the models m_walkingZMPController->reset(m_DCMPositionDesired.front()); m_stableDCMModel->reset(m_DCMPositionDesired.front()); @@ -982,6 +1033,9 @@ bool WalkingModule::updateModule() desiredCoMVelocity(1) = outputZMPCoMControllerVelocity(1); desiredCoMVelocity(2) = m_retargetingClient->comHeightVelocity(); + iDynTree::Position desiredCOMPositionTSID = desiredCoMPosition; + iDynTree::Vector3 desiredCOMVelocityTSID = desiredCoMVelocity; + if (m_firstRun) { double comVelocityNorm = iDynTree::toEigen(desiredCoMVelocity).norm(); @@ -1006,6 +1060,8 @@ bool WalkingModule::updateModule() yawRotation = yawRotation.inverse(); modifiedInertial = yawRotation * m_inertial_R_worldFrame; + iDynTree::Rotation desiredYawRotationTSID = yawRotation; + if (m_useQPIK) { // integrate dq because velocity control mode seems not available @@ -1035,6 +1091,18 @@ bool WalkingModule::updateModule() bufferPosition = m_velocityIntegral->integrate(bufferVelocity); iDynTree::toiDynTree(bufferPosition, m_qDesiredIK); + // update robot state with IK solution + if (!m_FKSolver->setInternalRobotState(m_qDesiredIK, m_dqDesiredIK)) + { + yError() << "[WalkingModule::updateModule] Unable to set the internal robot state."; + return false; + } + desiredCOMPositionTSID = m_FKSolver->getCoMPosition(); + desiredCOMVelocityTSID = m_FKSolver->getCoMVelocity(); + desiredYawRotationTSID = m_FKSolver->getNeckOrientation(); + + // restore robot state + if (!m_FKSolver->setInternalRobotState(m_robotControlHelper->getJointPosition(), m_robotControlHelper->getJointVelocity())) { @@ -1102,7 +1170,11 @@ bool WalkingModule::updateModule() return false; } - if(!solveBLFTSID(m_ddqDesiredTSID)) + if(!solveBLFTSID(desiredCOMPositionTSID, + desiredCOMVelocityTSID, desiredYawRotationTSID)){ + yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; + return false; + } { yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; return false; From 4d79d210395d484a05e74b9dd55624b0f0c56fea Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 25 Feb 2025 16:44:14 +0100 Subject: [PATCH 06/26] fix leftover --- src/WalkingModule/src/Module.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 5ed56d499..f0fbda501 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -642,8 +642,18 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, ok = ok && m_BLFTSIDSolver->setRootTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); } + if (!ok) + { + yError() << "[WalkingModule::solveBLFTSID] Unable to set the TSID set points."; + } + ok = ok && m_BLFTSIDSolver->solve(); + if (!ok) + { + yError() << "[WalkingModule::solveBLFTSID] Unable to solve the optimization problem."; + } + return ok; } @@ -1175,10 +1185,6 @@ bool WalkingModule::updateModule() yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; return false; } - { - yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; - return false; - } getBLFTSIDOutput(m_ddqDesiredTSID, m_desiredJointTorquesTSID); From 806026f85179e3476f5c0fe1240e326de3868319 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 25 Feb 2025 17:50:12 +0100 Subject: [PATCH 07/26] add tracking of weight percentage in TSID contact wrench task --- .../WalkingControllers/WalkingModule/Module.h | 3 ++ src/WalkingModule/src/Module.cpp | 44 ++++++++++++------- 2 files changed, 30 insertions(+), 17 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 64f491244..c8df8ae64 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -146,6 +146,9 @@ namespace WalkingControllers std::deque m_desiredZMP; /**< Deque containing the desired ZMP position. */ + std::deque m_weightInLeftDesired; /**< Deque containing the desired weight percentage on the left foot */ + std::deque m_weightInRightDesired; /**< Deque containing the desired weight percentage on the right foot */ + iDynTree::ModelLoader m_loader; /**< Model loader class. */ iDynTree::VectorDynSize m_qDesiredIK; /**< Vector containing the results of the IK algorithm [rad]. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index f0fbda501..0c7deaa8a 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -89,6 +89,12 @@ bool WalkingModule::advanceReferenceSignals() m_desiredZMP.pop_front(); m_desiredZMP.push_back(m_desiredZMP.back()); + m_weightInLeftDesired.pop_front(); + m_weightInLeftDesired.push_back(m_weightInLeftDesired.back()); + + m_weightInRightDesired.pop_front(); + m_weightInRightDesired.push_back(m_weightInRightDesired.back()); + // at each sampling time the merge points are decreased by one. // If the first merge point is equal to 0 it will be dropped. // A new trajectory will be merged at the first merge point or if the deque is empty @@ -603,36 +609,32 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, iDynTree::VectorDynSize zeroNdofVector(m_robotControlHelper->getActuatedDoFs()); zeroNdofVector.zero(); - bool isLeftInContact = m_leftInContact.front(); - bool isRightInContact = m_rightInContact.front(); - + // compute the desired contact wrenches double robotMass = m_FKSolver->getKinDyn()->getRobotModel().getTotalMass(); - iDynTree::Vector3 robotWeight; - robotWeight.zero(); - robotWeight(2) = -9.81 * robotMass; + iDynTree::Vector3 robotWeightinLeft; + robotWeightinLeft.zero(); + robotWeightinLeft(2) = m_weightInLeftDesired.front() * robotMass * 9.81; + + iDynTree::Vector3 robotWeightinRight; + robotWeightinRight.zero(); + robotWeightinRight(2) = m_weightInRightDesired.front() * robotMass * 9.81; iDynTree::Wrench leftContactWrench; iDynTree::Wrench rightContactWrench; leftContactWrench.zero(); rightContactWrench.zero(); + leftContactWrench.setLinearVec3(robotWeightinLeft); + rightContactWrench.setLinearVec3(robotWeightinRight); - if (isLeftInContact) - { - leftContactWrench.setLinearVec3(robotWeight); - } - if (isRightInContact) - { - rightContactWrench.setLinearVec3(robotWeight); - } - + // set the desired set points bool ok{true}; ok = ok && m_BLFTSIDSolver->setCoMTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); ok = ok && m_BLFTSIDSolver->setLeftFootTrackingSetPoint(m_leftTrajectory.front(), m_leftTwistTrajectory.front(), zeroTwist); ok = ok && m_BLFTSIDSolver->setRightFootTrackingSetPoint(m_rightTrajectory.front(), m_rightTwistTrajectory.front(), zeroTwist); ok = ok && m_BLFTSIDSolver->setJointTrackingSetPoint(m_qDesiredIK, m_dqDesiredIK, zeroNdofVector); ok = ok && m_BLFTSIDSolver->setTorsoTrackingSetPoint(desiredTorsoRotation, zero3dVector, zero3dVector); - m_BLFTSIDSolver->setLeftContactActive(isLeftInContact); - m_BLFTSIDSolver->setRightContactActive(isRightInContact); + m_BLFTSIDSolver->setLeftContactActive(m_leftInContact.front()); + m_BLFTSIDSolver->setRightContactActive(m_rightInContact.front()); ok = ok && m_BLFTSIDSolver->setLeftContactWrenchSetPoint(leftContactWrench); ok = ok && m_BLFTSIDSolver->setRightContactWrenchSetPoint(rightContactWrench); ok = ok && m_BLFTSIDSolver->setTorqueRegularizationSetPoint(zeroNdofVector); @@ -647,6 +649,7 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, yError() << "[WalkingModule::solveBLFTSID] Unable to set the TSID set points."; } + // solve the optimization problem ok = ok && m_BLFTSIDSolver->solve(); if (!ok) @@ -1624,6 +1627,8 @@ bool WalkingModule::updateTrajectories(const size_t &mergePoint) std::vector mergePoints; std::vector isLeftFixedFrame; std::vector isStancePhase; + std::vector weightInLeft; + std::vector weightInRight; // get dcm position and velocity m_trajectoryGenerator->getDCMPositionTrajectory(DCMPositionDesired); @@ -1647,6 +1652,8 @@ bool WalkingModule::updateTrajectories(const size_t &mergePoint) m_trajectoryGenerator->getDesiredZMPPosition(desiredZMP); + m_trajectoryGenerator->getWeightPercentage(weightInLeft, weightInRight); + // append vectors to deques StdUtilities::appendVectorToDeque(leftTrajectory, m_leftTrajectory, mergePoint); StdUtilities::appendVectorToDeque(rightTrajectory, m_rightTrajectory, mergePoint); @@ -1667,6 +1674,9 @@ bool WalkingModule::updateTrajectories(const size_t &mergePoint) StdUtilities::appendVectorToDeque(desiredZMP, m_desiredZMP, mergePoint); + StdUtilities::appendVectorToDeque(weightInLeft, m_weightInLeftDesired, mergePoint); + StdUtilities::appendVectorToDeque(weightInRight, m_weightInRightDesired, mergePoint); + m_mergePoints.assign(mergePoints.begin(), mergePoints.end()); // the first merge point is always equal to 0 From 2a3dcdfc648ea7a74e2803e8fff27772a103ae14 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 25 Feb 2025 18:15:26 +0100 Subject: [PATCH 08/26] fix initialization of acceleration integrator and size of TSID variables --- src/WalkingModule/src/Module.cpp | 2 +- src/WholeBodyControllers/src/BLFTSID.cpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 0c7deaa8a..b59245817 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -760,7 +760,7 @@ bool WalkingModule::updateModule() if (m_useTSIDadmittance){ m_qDesiredTSID = m_qDesiredIK; - m_dqDesiredTSID = m_dqDesiredIK; + m_dqDesiredTSID.zero(); m_jointAccelerationIntegrator->setState(iDynTree::toEigen(m_qDesiredTSID), iDynTree::toEigen(m_dqDesiredTSID)); } diff --git a/src/WholeBodyControllers/src/BLFTSID.cpp b/src/WholeBodyControllers/src/BLFTSID.cpp index 9152769a4..a4f35c796 100644 --- a/src/WholeBodyControllers/src/BLFTSID.cpp +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -25,6 +25,9 @@ bool BLFTSID::initialize( std::shared_ptr kinDyn) { constexpr auto logPrefix = "[WholeBodyQPBlock::instantiateTSID]"; + m_jointAccelerations.resize(kinDyn->getNrOfDegreesOfFreedom()); + m_jointTorques.resize(kinDyn->getNrOfDegreesOfFreedom()); + auto getTask = [this, logPrefix](const std::string &taskName, auto &task) -> bool { auto ptr = m_qpTSID.problem.tsid->getTask(taskName).lock(); From 2d25a6b198307bc18fe1d400b96b7395675f40ae Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 26 Feb 2025 12:07:40 +0100 Subject: [PATCH 09/26] fix the setting of root task setpoint --- .../WalkingControllers/WholeBodyControllers/BLFTSID.h | 2 -- src/WholeBodyControllers/src/BLFTSID.cpp | 10 +++------- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h index b2caf6b17..880247daa 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h @@ -115,8 +115,6 @@ class BLFTSID { iDynTree::VectorDynSize m_jointAccelerations; iDynTree::VectorDynSize m_jointTorques; - - bool m_useRootLinkForHeight{false}; }; } // namespace WalkingControllers diff --git a/src/WholeBodyControllers/src/BLFTSID.cpp b/src/WholeBodyControllers/src/BLFTSID.cpp index a4f35c796..1a3f1ff5e 100644 --- a/src/WholeBodyControllers/src/BLFTSID.cpp +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -77,7 +77,6 @@ bool BLFTSID::initialize( m_qpTSID.tasks.rightContactWrenchRegularization) && getTask("TORQUE_REGULARIZATION", m_qpTSID.tasks.torqueRegularization); - ; } bool BLFTSID::solve() { @@ -134,12 +133,9 @@ bool BLFTSID::setCoMTrackingSetPoint(const iDynTree::Position &position, bool BLFTSID::setRootTrackingSetPoint( const iDynTree::Position &position, const iDynTree::Vector3 &linearVelocity, const iDynTree::Vector3 &linearAcceleration) { - if (m_useRootLinkForHeight) { - return m_qpTSID.tasks.rootTracking->setSetPoint( - iDynTree::toEigen(position), iDynTree::toEigen(linearVelocity), - iDynTree::toEigen(linearAcceleration)); - } - return true; + return m_qpTSID.tasks.rootTracking->setSetPoint( + iDynTree::toEigen(position), iDynTree::toEigen(linearVelocity), + iDynTree::toEigen(linearAcceleration)); } bool BLFTSID::setTorsoTrackingSetPoint( From 93e86815e4f564464371630a934934a980ffc0d0 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 26 Feb 2025 14:57:13 +0100 Subject: [PATCH 10/26] update setpoints of TSID tasks and add some logging --- src/Integrators/src/jointIntegrators.cpp | 5 +-- .../TaskSpaceInverseDynamicsBlf.ini | 2 +- .../WalkingControllers/WalkingModule/Module.h | 3 +- src/WalkingModule/src/Module.cpp | 37 ++++++++++++------- 4 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/Integrators/src/jointIntegrators.cpp b/src/Integrators/src/jointIntegrators.cpp index b6c1f7f7e..bcd543b20 100644 --- a/src/Integrators/src/jointIntegrators.cpp +++ b/src/Integrators/src/jointIntegrators.cpp @@ -127,8 +127,8 @@ bool JointAccelerationIntegrator::setState( if (!dynamics->setState({state})) { BipedalLocomotion::log()->error( - "[JointAccelerationIntegrator::setState] Failed to set state of " - "the dynamical system"); + "{} Failed to set state of the dynamical system", + logPrefix); return false; } @@ -160,7 +160,6 @@ bool JointAccelerationIntegrator::oneStepIntegration() { } const Eigen::VectorXd &JointAccelerationIntegrator::getJointPosition() const { - const auto &[stateX] = integrator->getSolution(); return m_jointPosition; } diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index a5f4c1bf0..59d2b6579 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -1,4 +1,4 @@ -tasks ("COM_TRACKING", "ROOT_TRACKING", "TORSO_TRACKING", "ANGULAR_MOMENTUM_TRACKING", +tasks ("COM_TRACKING", "ROOT_TRACKING", "TORSO_TRACKING", "LEFT_FOOT_TRACKING", "RIGHT_FOOT_TRACKING", "LEFT_FOOT_FEASIBLE_WRENCH", "RIGHT_FOOT_FEASIBLE_WRENCH", "JOINT_TRACKING", "JOINT_DYNAMICS", "BASE_DYNAMICS", diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index c8df8ae64..78d059bb1 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -158,7 +158,8 @@ namespace WalkingControllers iDynTree::VectorDynSize m_dqDesiredTSID; /**< Vector containing the desired joint velocity [rad/s]. */ iDynTree::VectorDynSize m_ddqDesiredTSID; /**< Vector containing the desired joint acceleration [rad/s^2]. */ iDynTree::VectorDynSize m_desiredJointTorquesTSID; /**< Vector containing the desired joint torques for the low-level [Nm]. */ - iDynTree::VectorDynSize m_desiredJointTorquesLL; /**< Vector containing the desired joint torques for the low-level [Nm]. */ + iDynTree::VectorDynSize m_desiredJointTorquesAdmittance; /**< Vector containing the desired joint torques for the low-level, + computed by the joint admittance controller [Nm]. */ iDynTree::Rotation m_inertial_R_worldFrame; /**< Rotation between the inertial and the world frame. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b59245817..17cd3e26a 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -466,11 +466,20 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) // Joint m_vectorsCollectionServer.populateMetadata("joints_state::positions::measured", m_robotControlHelper->getAxesList()); - m_vectorsCollectionServer.populateMetadata("joints_state::positions::desired", m_robotControlHelper->getAxesList()); + m_vectorsCollectionServer.populateMetadata("joints_state::positions::desired::ik", m_robotControlHelper->getAxesList()); m_vectorsCollectionServer.populateMetadata("joints_state::positions::retargeting", m_robotControlHelper->getAxesList()); m_vectorsCollectionServer.populateMetadata("joints_state::positions::retargeting_raw", m_robotControlHelper->getAxesList()); m_vectorsCollectionServer.populateMetadata("joints_state::velocities::measured", m_robotControlHelper->getAxesList()); + m_vectorsCollectionServer.populateMetadata("joints_state::velocities::desired::ik", m_robotControlHelper->getAxesList()); m_vectorsCollectionServer.populateMetadata("joints_state::velocities::retargeting", m_robotControlHelper->getAxesList()); + if (m_useTSIDadmittance) + { + m_vectorsCollectionServer.populateMetadata("joints_state::positions::desired::tsid", m_robotControlHelper->getAxesList()); + m_vectorsCollectionServer.populateMetadata("joints_state::velocity::desired::tsid", m_robotControlHelper->getAxesList()); + m_vectorsCollectionServer.populateMetadata("joints_state::acceleration::desired", m_robotControlHelper->getAxesList()); + m_vectorsCollectionServer.populateMetadata("joints_state::torque::desired::tsid", m_robotControlHelper->getAxesList()); + m_vectorsCollectionServer.populateMetadata("joints_state::torque::desired::admittance", m_robotControlHelper->getAxesList()); + } // root link information m_vectorsCollectionServer.populateMetadata("root_link::position::measured", {"x", "y", "z"}); @@ -510,7 +519,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_dqDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); m_ddqDesiredTSID.resize(m_robotControlHelper->getActuatedDoFs()); m_desiredJointTorquesTSID.resize(m_robotControlHelper->getActuatedDoFs()); - m_desiredJointTorquesLL.resize(m_robotControlHelper->getActuatedDoFs()); + m_desiredJointTorquesAdmittance.resize(m_robotControlHelper->getActuatedDoFs()); yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -1046,8 +1055,6 @@ bool WalkingModule::updateModule() desiredCoMVelocity(1) = outputZMPCoMControllerVelocity(1); desiredCoMVelocity(2) = m_retargetingClient->comHeightVelocity(); - iDynTree::Position desiredCOMPositionTSID = desiredCoMPosition; - iDynTree::Vector3 desiredCOMVelocityTSID = desiredCoMVelocity; if (m_firstRun) { @@ -1073,8 +1080,6 @@ bool WalkingModule::updateModule() yawRotation = yawRotation.inverse(); modifiedInertial = yawRotation * m_inertial_R_worldFrame; - iDynTree::Rotation desiredYawRotationTSID = yawRotation; - if (m_useQPIK) { // integrate dq because velocity control mode seems not available @@ -1110,9 +1115,6 @@ bool WalkingModule::updateModule() yError() << "[WalkingModule::updateModule] Unable to set the internal robot state."; return false; } - desiredCOMPositionTSID = m_FKSolver->getCoMPosition(); - desiredCOMVelocityTSID = m_FKSolver->getCoMVelocity(); - desiredYawRotationTSID = m_FKSolver->getNeckOrientation(); // restore robot state @@ -1183,8 +1185,8 @@ bool WalkingModule::updateModule() return false; } - if(!solveBLFTSID(desiredCOMPositionTSID, - desiredCOMVelocityTSID, desiredYawRotationTSID)){ + if(!solveBLFTSID(desiredCoMPosition, + desiredCoMVelocity, yawRotation)){ yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; return false; } @@ -1201,7 +1203,7 @@ bool WalkingModule::updateModule() advanceJointAdmittanceController(m_desiredJointTorquesTSID, m_qDesiredTSID, m_dqDesiredTSID, - m_desiredJointTorquesLL); + m_desiredJointTorquesAdmittance); // restore robot state if (!m_FKSolver->setInternalRobotState(m_robotControlHelper->getJointPosition(), @@ -1335,11 +1337,20 @@ bool WalkingModule::updateModule() // Joint m_vectorsCollectionServer.populateData("joints_state::positions::measured", m_robotControlHelper->getJointPosition()); - m_vectorsCollectionServer.populateData("joints_state::positions::desired", (m_useTSIDadmittance)? m_qDesiredTSID : m_qDesiredIK); + m_vectorsCollectionServer.populateData("joints_state::positions::desired::ik", m_qDesiredIK); m_vectorsCollectionServer.populateData("joints_state::positions::retargeting", m_retargetingClient->jointPositions()); m_vectorsCollectionServer.populateData("joints_state::positions::retargeting_raw", m_retargetingClient->rawJointPositions()); m_vectorsCollectionServer.populateData("joints_state::velocities::measured", m_robotControlHelper->getJointVelocity()); + m_vectorsCollectionServer.populateData("joints_state::velocities::desired::ik", m_dqDesiredIK); m_vectorsCollectionServer.populateData("joints_state::velocities::retargeting", m_retargetingClient->jointVelocities()); + if (m_useTSIDadmittance) + { + m_vectorsCollectionServer.populateData("joints_state::positions::desired::tsid", m_qDesiredTSID); + m_vectorsCollectionServer.populateData("joints_state::velocity::desired::tsid", m_dqDesiredTSID); + m_vectorsCollectionServer.populateData("joints_state::acceleration::desired", m_ddqDesiredTSID); + m_vectorsCollectionServer.populateData("joints_state::torque::desired::tsid", m_desiredJointTorquesTSID); + m_vectorsCollectionServer.populateData("joints_state::torque::desired::admittance", m_desiredJointTorquesAdmittance); + } // root link information m_vectorsCollectionServer.populateData("root_link::position::measured", m_FKSolver->getRootLinkToWorldTransform().getPosition()); From a909196ae15582c6717ceec7b50179b726d3afd9 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 26 Feb 2025 15:14:50 +0100 Subject: [PATCH 11/26] change setpoint of CoM tracking task in IK, and switch to torque control --- src/WalkingModule/src/Module.cpp | 55 +++++++++++++++++++------------- 1 file changed, 32 insertions(+), 23 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 17cd3e26a..2f59a6aae 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1045,20 +1045,29 @@ bool WalkingModule::updateModule() // inverse kinematics m_profiler->setInitTime("IK"); - iDynTree::Position desiredCoMPosition; - desiredCoMPosition(0) = outputZMPCoMControllerPosition(0); - desiredCoMPosition(1) = outputZMPCoMControllerPosition(1); - desiredCoMPosition(2) = m_retargetingClient->comHeight() + m_comHeightOffset; - - iDynTree::Vector3 desiredCoMVelocity; - desiredCoMVelocity(0) = outputZMPCoMControllerVelocity(0); - desiredCoMVelocity(1) = outputZMPCoMControllerVelocity(1); - desiredCoMVelocity(2) = m_retargetingClient->comHeightVelocity(); - + iDynTree::Position desiredCoMPositionIK; + desiredCoMPositionIK(0) = m_stableDCMModel->getCoMPosition()(0); + desiredCoMPositionIK(1) = m_stableDCMModel->getCoMPosition()(1); + desiredCoMPositionIK(2) = m_retargetingClient->comHeight() + m_comHeightOffset; + + iDynTree::Vector3 desiredCoMVelocityIK; + desiredCoMVelocityIK(0) = m_stableDCMModel->getCoMVelocity()(0); + desiredCoMVelocityIK(1) = m_stableDCMModel->getCoMVelocity()(1); + desiredCoMVelocityIK(2) = m_retargetingClient->comHeightVelocity(); + + iDynTree::Position desiredCoMPositionTSID; + desiredCoMPositionTSID(0) = outputZMPCoMControllerPosition(0); + desiredCoMPositionTSID(1) = outputZMPCoMControllerPosition(1); + desiredCoMPositionTSID(2) = m_retargetingClient->comHeight() + m_comHeightOffset; + + iDynTree::Vector3 desiredCoMVelocityTSID; + desiredCoMVelocityTSID(0) = outputZMPCoMControllerVelocity(0); + desiredCoMVelocityTSID(1) = outputZMPCoMControllerVelocity(1); + desiredCoMVelocityTSID(2) = m_retargetingClient->comHeightVelocity(); if (m_firstRun) { - double comVelocityNorm = iDynTree::toEigen(desiredCoMVelocity).norm(); + double comVelocityNorm = iDynTree::toEigen(desiredCoMVelocityTSID).norm(); if (comVelocityNorm > m_maxInitialCoMVelocity) { @@ -1094,8 +1103,8 @@ bool WalkingModule::updateModule() yawRotation = yawRotation.inverse() * m_trajectoryGenerator->getChestAdditionalRotation(); - if (!solveBLFIK(desiredCoMPosition, - desiredCoMVelocity, + if (!solveBLFIK(desiredCoMPositionIK, + desiredCoMVelocityIK, yawRotation, m_dqDesiredIK)) { @@ -1142,7 +1151,7 @@ bool WalkingModule::updateModule() } if (!m_IKSolver->computeIK(m_leftTrajectory.front(), m_rightTrajectory.front(), - desiredCoMPosition, m_qDesiredIK)) + desiredCoMPositionIK, m_qDesiredIK)) { yError() << "[WalkingModule::updateModule] Error during the inverse Kinematics iteration."; return false; @@ -1185,8 +1194,8 @@ bool WalkingModule::updateModule() return false; } - if(!solveBLFTSID(desiredCoMPosition, - desiredCoMVelocity, yawRotation)){ + if(!solveBLFTSID(desiredCoMPositionTSID, + desiredCoMVelocityTSID, yawRotation)){ yError() << "[WalkingModule::updateModule] Unable to solve the TSID problem"; return false; } @@ -1220,16 +1229,16 @@ bool WalkingModule::updateModule() if (m_useTSIDadmittance){ - // if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorques)) - // { - // yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; - // return false; - // } - if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorquesAdmittance)) { yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; return false; } + // if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + // { + // yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + // return false; + // } } else { if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredIK)) @@ -1284,7 +1293,7 @@ bool WalkingModule::updateModule() CoMPositionDesired[2] = m_retargetingClient->comHeight() + m_comHeightOffset; m_vectorsCollectionServer.populateData("com::position::desired", CoMPositionDesired); - m_vectorsCollectionServer.populateData("com::position::CoM_ZMP_controller", desiredCoMPosition); + m_vectorsCollectionServer.populateData("com::position::CoM_ZMP_controller", desiredCoMPositionTSID); // Manual definition of this value to add also the planned CoM height velocity std::vector CoMVelocityDesired(3); From f56e81de08a06361c687f4b90caa3aa1d6f9afd3 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 26 Feb 2025 15:39:48 +0100 Subject: [PATCH 12/26] add spatial acceleration from planner as setpoint of feet tracking task in TSID --- .../TrajectoryPlanner/TrajectoryGenerator.h | 9 +++++++++ .../src/TrajectoryGenerator.cpp | 14 ++++++++++++++ .../WalkingControllers/WalkingModule/Module.h | 4 ++++ src/WalkingModule/src/Module.cpp | 17 +++++++++++++++-- .../WholeBodyControllers/BLFTSID.h | 5 +++-- src/WholeBodyControllers/src/BLFTSID.cpp | 4 ++-- 6 files changed, 47 insertions(+), 6 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 895c91a16..83a875648 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -195,6 +195,15 @@ namespace WalkingControllers */ bool getFeetTwist(std::vector& lFootTwist, std::vector& rFootTwist); + + /** + * Get the feet spatial acceleration + * @param lFootAcceleration vector containing the left foot spatial acceleration; + * @param rFootAcceleration vector containing the right foot spatial acceleration. + * @return true/false in case of success/failure. + */ + bool getFeetAcceleration(std::vector& lFootAcceleration, + std::vector& rFootAcceleration); /** * Get the when the main frame of the left foot is the fix frame. * @param isLeftFixedFrame vector containing when the main frame of diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index aa0bf3436..683cc5ab2 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -724,6 +724,20 @@ bool TrajectoryGenerator::getFeetTwist(std::vector& lFootTwist, return true; } +bool TrajectoryGenerator::getFeetAcceleration(std::vector& lFootAcceleration, + std::vector& rFootAcceleration) +{ +if(!isTrajectoryComputed()) +{ +yError() << "[getFeetAcceleration] No trajectories are available"; +return false; +} + +m_feetGenerator->getFeetAccelerationInMixedRepresentation(lFootAcceleration, rFootAcceleration); + +return true; +} + bool TrajectoryGenerator::getWhenUseLeftAsFixed(std::vector& isLeftFixedFrame) { if(!isTrajectoryComputed()) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 78d059bb1..68dea2eb1 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -7,6 +7,7 @@ // std #include #include +#include #include #include @@ -131,6 +132,9 @@ namespace WalkingControllers std::deque m_leftTwistTrajectory; /**< Deque containing the twist trajectory of the left foot. */ std::deque m_rightTwistTrajectory; /**< Deque containing the twist trajectory of the right foot. */ + std::deque m_leftAccelerationTrajectory; /**< Deque containing the spatial acceleration trajectory of the left foot. */ + std::deque m_rightAccelerationTrajectory; /**< Deque containing the spatial acceleration trajectory of the left foot. */ + std::deque m_DCMPositionDesired; /**< Deque containing the desired DCM position. */ std::deque m_DCMVelocityDesired; /**< Deque containing the desired DCM velocity. */ std::deque m_leftInContact; /**< Deque containing the left foot state. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 2f59a6aae..fd99f1513 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -59,9 +59,15 @@ bool WalkingModule::advanceReferenceSignals() m_rightTwistTrajectory.pop_front(); m_rightTwistTrajectory.push_back(m_rightTwistTrajectory.back()); + m_rightAccelerationTrajectory.pop_front(); + m_rightAccelerationTrajectory.push_back(m_rightAccelerationTrajectory.back()); + m_leftTwistTrajectory.pop_front(); m_leftTwistTrajectory.push_back(m_leftTwistTrajectory.back()); + m_leftAccelerationTrajectory.pop_front(); + m_leftAccelerationTrajectory.push_back(m_leftAccelerationTrajectory.back()); + m_rightInContact.pop_front(); m_rightInContact.push_back(m_rightInContact.back()); @@ -638,8 +644,10 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, // set the desired set points bool ok{true}; ok = ok && m_BLFTSIDSolver->setCoMTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); - ok = ok && m_BLFTSIDSolver->setLeftFootTrackingSetPoint(m_leftTrajectory.front(), m_leftTwistTrajectory.front(), zeroTwist); - ok = ok && m_BLFTSIDSolver->setRightFootTrackingSetPoint(m_rightTrajectory.front(), m_rightTwistTrajectory.front(), zeroTwist); + ok = ok && m_BLFTSIDSolver->setLeftFootTrackingSetPoint(m_leftTrajectory.front(), m_leftTwistTrajectory.front(), + m_leftAccelerationTrajectory.front()); + ok = ok && m_BLFTSIDSolver->setRightFootTrackingSetPoint(m_rightTrajectory.front(), m_rightTwistTrajectory.front(), + m_rightAccelerationTrajectory.front()); ok = ok && m_BLFTSIDSolver->setJointTrackingSetPoint(m_qDesiredIK, m_dqDesiredIK, zeroNdofVector); ok = ok && m_BLFTSIDSolver->setTorsoTrackingSetPoint(desiredTorsoRotation, zero3dVector, zero3dVector); m_BLFTSIDSolver->setLeftContactActive(m_leftInContact.front()); @@ -1637,6 +1645,8 @@ bool WalkingModule::updateTrajectories(const size_t &mergePoint) std::vector rightTrajectory; std::vector leftTwistTrajectory; std::vector rightTwistTrajectory; + std::vector leftAccelerationTrajectory; + std::vector rightAccelerationTrajectory; std::vector DCMPositionDesired; std::vector DCMVelocityDesired; std::vector desiredZMP; @@ -1657,6 +1667,7 @@ bool WalkingModule::updateTrajectories(const size_t &mergePoint) // get feet trajectories m_trajectoryGenerator->getFeetTrajectories(leftTrajectory, rightTrajectory); m_trajectoryGenerator->getFeetTwist(leftTwistTrajectory, rightTwistTrajectory); + m_trajectoryGenerator->getFeetAcceleration(leftAccelerationTrajectory, rightAccelerationTrajectory); m_trajectoryGenerator->getFeetStandingPeriods(leftInContact, rightInContact); m_trajectoryGenerator->getWhenUseLeftAsFixed(isLeftFixedFrame); @@ -1679,6 +1690,8 @@ bool WalkingModule::updateTrajectories(const size_t &mergePoint) StdUtilities::appendVectorToDeque(rightTrajectory, m_rightTrajectory, mergePoint); StdUtilities::appendVectorToDeque(leftTwistTrajectory, m_leftTwistTrajectory, mergePoint); StdUtilities::appendVectorToDeque(rightTwistTrajectory, m_rightTwistTrajectory, mergePoint); + StdUtilities::appendVectorToDeque(leftAccelerationTrajectory, m_leftAccelerationTrajectory, mergePoint); + StdUtilities::appendVectorToDeque(rightAccelerationTrajectory, m_rightAccelerationTrajectory, mergePoint); StdUtilities::appendVectorToDeque(isLeftFixedFrame, m_isLeftFixedFrame, mergePoint); StdUtilities::appendVectorToDeque(DCMPositionDesired, m_DCMPositionDesired, mergePoint); diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h index 880247daa..7ec044e77 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFTSID.h @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -38,12 +39,12 @@ class BLFTSID { bool setLeftFootTrackingSetPoint(const iDynTree::Transform &desiredTransform, const iDynTree::Twist &desiredVelocity, - const iDynTree::Twist &desiredAcceleration); + const iDynTree::SpatialAcc &desiredAcceleration); bool setRightFootTrackingSetPoint(const iDynTree::Transform &desiredTransform, const iDynTree::Twist &desiredVelocity, - const iDynTree::Twist &desiredAcceleration); + const iDynTree::SpatialAcc &desiredAcceleration); bool setJointTrackingSetPoint(const iDynTree::VectorDynSize &jointPosition, const iDynTree::VectorDynSize &jointVelocity, diff --git a/src/WholeBodyControllers/src/BLFTSID.cpp b/src/WholeBodyControllers/src/BLFTSID.cpp index 1a3f1ff5e..424fe60e8 100644 --- a/src/WholeBodyControllers/src/BLFTSID.cpp +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -96,7 +96,7 @@ bool BLFTSID::solve() { bool BLFTSID::setLeftFootTrackingSetPoint( const iDynTree::Transform &desiredTransform, const iDynTree::Twist &desiredVelocity, - const iDynTree::Twist &desiredAcceleration) { + const iDynTree::SpatialAcc &desiredAcceleration) { return m_qpTSID.tasks.leftFootTracking->setSetPoint( BipedalLocomotion::Conversions::toManifPose(desiredTransform), BipedalLocomotion::Conversions::toManifTwist(desiredVelocity), @@ -106,7 +106,7 @@ bool BLFTSID::setLeftFootTrackingSetPoint( bool BLFTSID::setRightFootTrackingSetPoint( const iDynTree::Transform &desiredTransform, const iDynTree::Twist &desiredVelocity, - const iDynTree::Twist &desiredAcceleration) { + const iDynTree::SpatialAcc &desiredAcceleration) { return m_qpTSID.tasks.rightFootTracking->setSetPoint( BipedalLocomotion::Conversions::toManifPose(desiredTransform), BipedalLocomotion::Conversions::toManifTwist(desiredVelocity), From bfe10b3b3473ef626a068a7bfa8d0a3c6117b715 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 26 Feb 2025 15:52:42 +0100 Subject: [PATCH 13/26] add config files for ergocubSN001 --- .../common/joint_admittance_controller.ini | 13 ++ .../TaskSpaceInverseDynamicsBlf.ini | 173 ++++++++++++++++++ .../tsid_tasks/angular_momentum_task.ini | 17 ++ .../tsid_tasks/base_dynamics_task.ini | 14 ++ .../tsid_tasks/joint_dynamics_task.ini | 15 ++ .../ergoCubSN001/dcm_walking_with_joypad.ini | 6 + 6 files changed, 238 insertions(+) create mode 100644 src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini new file mode 100644 index 000000000..1a3eeecec --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini @@ -0,0 +1,13 @@ +[GAINS] + +kp (120.0, 80.0, 80.0, + 50.0, 50.0, 50.0, 50.0, + 50.0, 50.0, 50.0, 50.0, + 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, + 220.0, 220.0, 220.0, 220.0, 220.0, 220.0) + +kd (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.1, 0.1, 0.1, 0.1, + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini new file mode 100644 index 000000000..59d2b6579 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -0,0 +1,173 @@ +tasks ("COM_TRACKING", "ROOT_TRACKING", "TORSO_TRACKING", + "LEFT_FOOT_TRACKING", "RIGHT_FOOT_TRACKING", + "LEFT_FOOT_FEASIBLE_WRENCH", "RIGHT_FOOT_FEASIBLE_WRENCH", + "JOINT_TRACKING", "JOINT_DYNAMICS", "BASE_DYNAMICS", + "LEFT_FOOT_WRENCH_REGULARIZATION", "RIGHT_FOOT_WRENCH_REGULARIZATION", "TORQUE_REGULARIZATION") + +[TSID] +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +contact_wrench_variables_name ("lf_wrench", "rf_wrench") +verbosity false +automatic_scaling false + + +; [VARIABLES] +; variables_name ("robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench") +; variables_size (28, 22, 6, 6) + + +[LEFT_FOOT_TRACKING] +type SE3Task +priority 0 +kp_linear 10.0 +kp_angular 10.0 +kd_linear 1.0 +kd_angular 1.0 +frame_name "l_sole" +use_orientation_exogenous_feedback false +use_position_exogenous_feedback false +weight (1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0) +mask (true, true, true) +robot_acceleration_variable_name "robot_acceleration" + + +[RIGHT_FOOT_TRACKING] +type SE3Task +priority 0 +kp_linear 10.0 +kp_angular 10.0 +kd_linear 1.0 +kd_angular 1.0 +frame_name "r_sole" +use_orientation_exogenous_feedback false +use_position_exogenous_feedback false +weight (1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0) +mask (true, true, true) +robot_acceleration_variable_name "robot_acceleration" + + +[COM_TRACKING] +type CoMTask +kp_linear 10.0 +kd_linear 1.0 +priority 0 +mask (true, true, false) +use_exogenous_feedback false +weight (1000.0, 1000.0) +robot_acceleration_variable_name "robot_acceleration" + + +[TORSO_TRACKING] +type SO3Task +kp_angular 5.0 +kd_angular 5.0 +frame_name "chest" +priority 1 +weight (100.0, 100.0, 100.0) +robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" + + +[ROOT_TRACKING] +type R3Task +frame_name "root_link" +kp_linear 10.0 +kd_linear 1.0 +mask (false, false, true) +priority 0 +weight (1000.0) +robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" + + +[JOINT_TRACKING] + +# joints_list "torso_pitch", "torso_roll", "torso_yaw", +# "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", +# "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", +# "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", +# "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll" + +type JointTrackingTask +priority 1 +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) + +kd (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, 1.0, 1.0, 1.0, 1.0 + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +weight (10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 + 10.0, 10.0, 10.0, 10.0, 10.0, 10.0) +robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" + + +[LEFT_FOOT_FEASIBLE_WRENCH] +name "lf_feasibility_wrench" +type "FeasibleContactWrenchTask" +priority 0 + +variable_name "lf_wrench" +frame_name "l_sole" +number_of_slices 2 +static_friction_coefficient 0.5 +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) + + +[RIGHT_FOOT_FEASIBLE_WRENCH] +name "rf_feasibility_wrench" +type "FeasibleContactWrenchTask" +priority 0 + +variable_name "rf_wrench" +frame_name "r_sole" +number_of_slices 2 +static_friction_coefficient 0.5 +foot_limits_x (-0.1, 0.1) +foot_limits_y (-0.05, 0.05) + +[LEFT_FOOT_WRENCH_REGULARIZATION] +type "VariableRegularizationTask" +variable_name "lf_wrench" +weight_provider_type "ConstantWeightProvider" + +variable_size 6 +priority 1 +weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[RIGHT_FOOT_WRENCH_REGULARIZATION] +type "VariableRegularizationTask" +variable_name "rf_wrench" +weight_provider_type "ConstantWeightProvider" + +variable_size 6 +priority 1 +weight (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[TORQUE_REGULARIZATION] +type "VariableRegularizationTask" +variable_name "joint_torques" +weight_provider_type "ConstantWeightProvider" + +variable_size 23 +priority 1 +weight (0.01, 0.01, 0.01, + 0.01, 0.01, 0.01, 0.01, + 0.01, 0.01, 0.01, 0.01, + 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 + 0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[include JOINT_DYNAMICS "./tsid_tasks/joint_dynamics_task.ini"] +[include BASE_DYNAMICS "./tsid_tasks/base_dynamics_task.ini"] +[include ANGULAR_MOMENTUM_TRACKING "./tsid_tasks/angular_momentum_task.ini"] \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini new file mode 100644 index 000000000..cdd8f7d03 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/angular_momentum_task.ini @@ -0,0 +1,17 @@ +name "angular_momentum_task" +type "AngularMomentumTask" +kp 30.0 +priority 1 +use_exogenous_feedback false +weight (500.0, 500.0, 500.0) +max_number_of_contacts 2 +weight_provider_type "ConstantWeightProvider" + + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini new file mode 100644 index 000000000..0f3094bbf --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/base_dynamics_task.ini @@ -0,0 +1,14 @@ +name "base_dynamics_task" +type "BaseDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini new file mode 100644 index 000000000..014bdf8f5 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tsid_tasks/joint_dynamics_task.ini @@ -0,0 +1,15 @@ +name "joint_dynamics_task" +type "JointDynamicsTask" +priority 0 + +robot_acceleration_variable_name "robot_acceleration" +joint_torques_variable_name "joint_torques" +max_number_of_contacts 2 + +[CONTACT_0] +variable_name "lf_wrench" +frame_name "l_sole" + +[CONTACT_1] +variable_name "rf_wrench" +frame_name "r_sole" \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini index 62217c6a9..e93ef74bc 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini @@ -59,6 +59,12 @@ height_reference_frame root_link # include qp inverse kinematcs parameters [include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematicsBlf.ini"] +# include qp task space inverse dynamics parameters +[include TASK_SPACE_INVERSE_DYNAMICS "./dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini"] + +# include joint admittance controller parameters +[include JOINT_ADMITTANCE_CONTROLLER "./dcm_walking/common/joint_admittance_controller.ini"] + # include inverse kinematcs parameters [include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] From 1650523ecfaf8237eda71c578d981925422f1a68 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Wed, 26 Feb 2025 18:08:17 +0100 Subject: [PATCH 14/26] remove CoM-ZMP controller and add ankle strategy --- .../dcm_walking/common/plannerParams.ini | 10 +-- .../common/zmpControllerParams.ini | 8 +- .../TaskSpaceInverseDynamicsBlf.ini | 30 +++---- .../common/zmpControllerParams.ini | 8 +- .../WalkingControllers/WalkingModule/Module.h | 2 + src/WalkingModule/src/Module.cpp | 78 +++++++++++++++++-- 6 files changed, 100 insertions(+), 36 deletions(-) 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..7b40ff737 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 @@ -31,19 +31,19 @@ minWidth 0.12 maxAngleVariation 18.0 minAngleVariation 5.0 #Timings -maxStepDuration 1.31 -minStepDuration 0.7 +maxStepDuration 5.0 +minStepDuration 4.0 ##Nominal Values #Width nominalWidth 0.17 #Height stepHeight 0.035 -stepLandingVelocity -0.15 +stepLandingVelocity -0.0 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 1.3 +nominalDuration 4.5 lastStepSwitchTime 0.8 switchOverSwingRatio 0.3 @@ -75,7 +75,7 @@ mergePointRatios (0.4, 0.4) pitchDelta 0.0 ##Should be the first step with the left foot? -swingLeft 0 +swingLeft 1 startAlwaysSameFoot 1 ##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/zmpControllerParams.ini index 8f282a3b7..5e2f7e296 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/zmpControllerParams.ini @@ -4,13 +4,13 @@ useGainScheduling 1 smoothingTime 0.05 # if the gain scheduling is off only k*_walking parameters are used -kZMP_walking 1.0 +kZMP_walking 0.0 # kZMP_stance 1.0 -kCoM_walking 3.0 +kCoM_walking 0.0 -kZMP_stance 1.0 +kZMP_stance 0.0 # kZMP_stance 1.0 -kCoM_stance 3.0 +kCoM_stance 0.0 # old parameters #kZMP 2.0 low velocity reactive diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index 59d2b6579..8dbec74f9 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -20,8 +20,8 @@ automatic_scaling false [LEFT_FOOT_TRACKING] type SE3Task priority 0 -kp_linear 10.0 -kp_angular 10.0 +kp_linear 5.0 +kp_angular 5.0 kd_linear 1.0 kd_angular 1.0 frame_name "l_sole" @@ -35,8 +35,8 @@ robot_acceleration_variable_name "robot_acceleration" [RIGHT_FOOT_TRACKING] type SE3Task priority 0 -kp_linear 10.0 -kp_angular 10.0 +kp_linear 5.0 +kp_angular 5.0 kd_linear 1.0 kd_angular 1.0 frame_name "r_sole" @@ -49,8 +49,8 @@ robot_acceleration_variable_name "robot_acceleration" [COM_TRACKING] type CoMTask -kp_linear 10.0 -kd_linear 1.0 +kp_linear 5.0 +kd_linear 2.0 priority 0 mask (true, true, false) use_exogenous_feedback false @@ -61,10 +61,10 @@ robot_acceleration_variable_name "robot_acceleration" [TORSO_TRACKING] type SO3Task kp_angular 5.0 -kd_angular 5.0 +kd_angular 1.0 frame_name "chest" priority 1 -weight (100.0, 100.0, 100.0) +weight (500.0, 500.0, 500.0) robot_acceleration_variable_name "robot_acceleration" weight_provider_type "ConstantWeightProvider" @@ -72,11 +72,11 @@ weight_provider_type "ConstantWeightProvider" [ROOT_TRACKING] type R3Task frame_name "root_link" -kp_linear 10.0 +kp_linear 5.0 kd_linear 1.0 mask (false, false, true) priority 0 -weight (1000.0) +weight (500.0) robot_acceleration_variable_name "robot_acceleration" weight_provider_type "ConstantWeightProvider" @@ -103,11 +103,11 @@ kd (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, 1.0, 1.0) -weight (10.0, 10.0, 10.0, - 10.0, 10.0, 10.0, 10.0, - 10.0, 10.0, 10.0, 10.0, - 10.0, 10.0, 10.0, 10.0, 10.0, 10.0 - 10.0, 10.0, 10.0, 10.0, 10.0, 10.0) +weight (1.0, 1.0, 1.0, + 50.0, 50.0, 50.0, 1.0, + 50.0, 50.0, 50.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) robot_acceleration_variable_name "robot_acceleration" weight_provider_type "ConstantWeightProvider" diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/zmpControllerParams.ini index 3a68089ac..2494fd566 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/zmpControllerParams.ini @@ -5,12 +5,12 @@ smoothingTime 0.05 # if the gain scheduling is off only k*_walking parameters are used # kZMP_walking 1.8 -kZMP_walking 1.3 -kCoM_walking 5.0 +kZMP_walking 0.0 +kCoM_walking 0.0 # kZMP_stance 0.5 -kZMP_stance 0.1 -kCoM_stance 4.0 +kZMP_stance 0.0 +kCoM_stance 0.0 # old parameters #kZMP 2.0 low velocity reactive diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 68dea2eb1..18aedba68 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -261,6 +261,8 @@ namespace WalkingControllers */ bool computeGlobalCoP(Eigen::Ref globalCoP); + bool computeLocalCoPCorrection(Eigen::Ref leftCorrection, Eigen::Ref rightCorrection); + /** * Given the two planned feet, it computes the average yaw rotation * @return the average Yaw rotation diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index fd99f1513..7dc31015f 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -641,12 +641,24 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, leftContactWrench.setLinearVec3(robotWeightinLeft); rightContactWrench.setLinearVec3(robotWeightinRight); + + // compute ankle strategy + Eigen::Vector3d leftCorrection, rightCorrection; + computeLocalCoPCorrection(leftCorrection, rightCorrection); + auto leftTwist = m_leftTwistTrajectory.front(); + auto rightTwist = m_rightTwistTrajectory.front(); + iDynTree::Vector3 temp; + iDynTree::toEigen(temp) = iDynTree::toEigen(leftTwist.getAngularVec3()) + leftCorrection; + leftTwist.setAngularVec3(temp); + iDynTree::toEigen(temp) = iDynTree::toEigen(rightTwist.getAngularVec3()) + rightCorrection; + rightTwist.setAngularVec3(temp); + // set the desired set points bool ok{true}; ok = ok && m_BLFTSIDSolver->setCoMTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); - ok = ok && m_BLFTSIDSolver->setLeftFootTrackingSetPoint(m_leftTrajectory.front(), m_leftTwistTrajectory.front(), + ok = ok && m_BLFTSIDSolver->setLeftFootTrackingSetPoint(m_leftTrajectory.front(), leftTwist, m_leftAccelerationTrajectory.front()); - ok = ok && m_BLFTSIDSolver->setRightFootTrackingSetPoint(m_rightTrajectory.front(), m_rightTwistTrajectory.front(), + ok = ok && m_BLFTSIDSolver->setRightFootTrackingSetPoint(m_rightTrajectory.front(), rightTwist, m_rightAccelerationTrajectory.front()); ok = ok && m_BLFTSIDSolver->setJointTrackingSetPoint(m_qDesiredIK, m_dqDesiredIK, zeroNdofVector); ok = ok && m_BLFTSIDSolver->setTorsoTrackingSetPoint(desiredTorsoRotation, zero3dVector, zero3dVector); @@ -729,6 +741,56 @@ bool WalkingModule::computeGlobalCoP(Eigen::Ref globalCoP) return true; } +bool WalkingModule::computeLocalCoPCorrection(Eigen::Ref leftCorrection, Eigen::Ref rightCorrection) +{ + BipedalLocomotion::Contacts::ContactWrench leftFootContact, rightFootContact; + leftFootContact.wrench = iDynTree::toEigen(m_robotControlHelper->getLeftWrench().asVector()); + leftFootContact.pose = BipedalLocomotion::Conversions::toManifPose(m_FKSolver->getLeftFootToWorldTransform()); + + rightFootContact.wrench = iDynTree::toEigen(m_robotControlHelper->getRightWrench().asVector()); + rightFootContact.pose = BipedalLocomotion::Conversions::toManifPose(m_FKSolver->getRightFootToWorldTransform()); + + Eigen::Vector3d leftCoP, rightCoP; + leftCoP = leftFootContact.wrench.getLocalCoP(); + rightCoP = rightFootContact.wrench.getLocalCoP(); + + Eigen::Vector3d errorCoPLeft; + Eigen::Vector3d errorCoPRight; + + errorCoPLeft = -leftCoP; + double gain{1.0}; + Eigen::Vector3d localCorrectionLeft; + + localCorrectionLeft(0) = errorCoPLeft(1) * (-gain); + localCorrectionLeft(1) = errorCoPLeft(0) * gain; + localCorrectionLeft(2) = 0.0; + + errorCoPRight = -rightCoP; + Eigen::Vector3d localCorrectionRight; + + localCorrectionRight(0) = errorCoPRight(1) * (-gain); + localCorrectionRight(1) = errorCoPRight(0) * gain; + localCorrectionRight(2) = 0.0; + + // set to 0 if force too small + if (leftFootContact.wrench.force().z() < 10) + { + localCorrectionLeft.setZero(); + } + if (rightFootContact.wrench.force().z() < 10) + { + localCorrectionRight.setZero(); + } + + leftCorrection = leftFootContact.pose.rotation() * localCorrectionLeft; + leftCorrection(2) = 0.0; + rightCorrection = rightFootContact.pose.rotation() * localCorrectionRight; + rightCorrection(2) = 0.0; + + return true; +} + + bool WalkingModule::updateModule() { std::lock_guard guard(m_mutex); @@ -1237,16 +1299,16 @@ bool WalkingModule::updateModule() if (m_useTSIDadmittance){ - if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorquesAdmittance)) - { - yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; - return false; - } - // if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + // if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorquesAdmittance)) // { // yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; // return false; // } + if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + { + yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + return false; + } } else { if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredIK)) From 03879447d5dd2c5638bba825b3f5147fc93b60e7 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Thu, 27 Feb 2025 09:45:09 +0100 Subject: [PATCH 15/26] add logging of feet angular velocity correction --- .../WalkingControllers/WalkingModule/Module.h | 3 +++ src/WalkingModule/src/Module.cpp | 15 ++++++++++----- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 18aedba68..2a2cc4607 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -132,6 +132,9 @@ namespace WalkingControllers std::deque m_leftTwistTrajectory; /**< Deque containing the twist trajectory of the left foot. */ std::deque m_rightTwistTrajectory; /**< Deque containing the twist trajectory of the right foot. */ + Eigen::Vector3d m_leftAngularVelocityCorrection; + Eigen::Vector3d m_rightAngularVelocityCorrection; + std::deque m_leftAccelerationTrajectory; /**< Deque containing the spatial acceleration trajectory of the left foot. */ std::deque m_rightAccelerationTrajectory; /**< Deque containing the spatial acceleration trajectory of the left foot. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 7dc31015f..24d406db1 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -460,6 +460,9 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_vectorsCollectionServer.populateMetadata("left_foot::linear_force::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("left_foot::angular_torque::measured", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("left_foot::angular_velocity::correction", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("right_foot::angular_velocity::correction", {"x", "y", "z"}); + // Right foot m_vectorsCollectionServer.populateMetadata("right_foot::position::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("right_foot::position::desired", {"x", "y", "z"}); @@ -643,14 +646,13 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, // compute ankle strategy - Eigen::Vector3d leftCorrection, rightCorrection; - computeLocalCoPCorrection(leftCorrection, rightCorrection); + computeLocalCoPCorrection(m_leftAngularVelocityCorrection, m_rightAngularVelocityCorrection); auto leftTwist = m_leftTwistTrajectory.front(); auto rightTwist = m_rightTwistTrajectory.front(); iDynTree::Vector3 temp; - iDynTree::toEigen(temp) = iDynTree::toEigen(leftTwist.getAngularVec3()) + leftCorrection; + iDynTree::toEigen(temp) = iDynTree::toEigen(leftTwist.getAngularVec3()) + m_leftAngularVelocityCorrection; leftTwist.setAngularVec3(temp); - iDynTree::toEigen(temp) = iDynTree::toEigen(rightTwist.getAngularVec3()) + rightCorrection; + iDynTree::toEigen(temp) = iDynTree::toEigen(rightTwist.getAngularVec3()) + m_rightAngularVelocityCorrection; rightTwist.setAngularVec3(temp); // set the desired set points @@ -758,7 +760,7 @@ bool WalkingModule::computeLocalCoPCorrection(Eigen::Ref leftCo Eigen::Vector3d errorCoPRight; errorCoPLeft = -leftCoP; - double gain{1.0}; + double gain{0.1}; Eigen::Vector3d localCorrectionLeft; localCorrectionLeft(0) = errorCoPLeft(1) * (-gain); @@ -1389,6 +1391,9 @@ bool WalkingModule::updateModule() m_vectorsCollectionServer.populateData("left_foot::linear_velocity::desired", m_leftTwistTrajectory.front().getLinearVec3()); m_vectorsCollectionServer.populateData("left_foot::angular_velocity::desired", m_leftTwistTrajectory.front().getAngularVec3()); + m_vectorsCollectionServer.populateData("left_foot::angular_velocity::correction", m_leftAngularVelocityCorrection); + m_vectorsCollectionServer.populateData("right_foot::angular_velocity::correction", m_rightAngularVelocityCorrection); + // "lf_force_x", "lf_force_y", "lf_force_z", // "lf_force_roll", "lf_force_pitch", "lf_force_yaw", m_vectorsCollectionServer.populateData("left_foot::linear_force::measured", m_robotControlHelper->getLeftWrench().getLinearVec3()); From 17549ecded28b32ba311ade059ad4ece93b02dbf Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Thu, 27 Feb 2025 14:19:14 +0100 Subject: [PATCH 16/26] send torque to low-level --- src/WalkingModule/src/Module.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 24d406db1..1862548ed 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1301,16 +1301,16 @@ bool WalkingModule::updateModule() if (m_useTSIDadmittance){ - // if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorquesAdmittance)) - // { - // yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; - // return false; - // } - if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + if (!m_robotControlHelper->setTorqueReferences(m_desiredJointTorquesAdmittance)) { yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; return false; } + // if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredTSID)) + // { + // yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + // return false; + // } } else { if (!m_robotControlHelper->setDirectPositionReferences(m_qDesiredIK)) From 0769cbbb3b8425680c0e8c924a0233e791c2f804 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Thu, 27 Feb 2025 15:10:07 +0100 Subject: [PATCH 17/26] add logging of tsid kindyn signals --- .../KinDynWrapper/Wrapper.h | 12 +++++ src/KinDynWrapper/src/Wrapper.cpp | 10 +++++ .../WalkingControllers/WalkingModule/Module.h | 35 +++++++++++---- src/WalkingModule/src/Module.cpp | 44 +++++++++++++++++++ 4 files changed, 93 insertions(+), 8 deletions(-) diff --git a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h index 4e4bd9c8f..043eb3085 100644 --- a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h +++ b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h @@ -172,12 +172,24 @@ namespace WalkingControllers */ iDynTree::Transform getLeftFootToWorldTransform(); + /** + * Return the left foot twist. + * @return left foot twist. + */ + iDynTree::Twist getLeftFootTwist(); + /** * Return the transformation between the right foot frame (r_sole) and the world reference frame. * @return world_H_right_frame. */ iDynTree::Transform getRightFootToWorldTransform(); + /** + * Return the right foot twist. + * @return right foot twist. + */ + iDynTree::Twist getRightFootTwist(); + /** * Return the transformation between the left hand frame and the world reference frame. * @return world_H_left_hand. diff --git a/src/KinDynWrapper/src/Wrapper.cpp b/src/KinDynWrapper/src/Wrapper.cpp index 9078f1d04..5823cb07a 100644 --- a/src/KinDynWrapper/src/Wrapper.cpp +++ b/src/KinDynWrapper/src/Wrapper.cpp @@ -456,11 +456,21 @@ iDynTree::Transform WalkingFK::getLeftFootToWorldTransform() return m_kinDyn->getWorldTransform(m_frameLeftIndex); } +iDynTree::Twist WalkingFK::getLeftFootTwist() +{ + return m_kinDyn->getFrameVel(m_frameLeftIndex); +} + iDynTree::Transform WalkingFK::getRightFootToWorldTransform() { return m_kinDyn->getWorldTransform(m_frameRightIndex); } +iDynTree::Twist WalkingFK::getRightFootTwist() +{ + return m_kinDyn->getFrameVel(m_frameRightIndex); +} + iDynTree::Transform WalkingFK::getLeftHandToWorldTransform() { return m_kinDyn->getWorldTransform(m_frameLeftHandIndex); diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 2a2cc4607..fc2a511f3 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -5,11 +5,6 @@ #define WALKING_MODULE_HPP // std -#include -#include -#include -#include - #include #include #include @@ -17,19 +12,24 @@ // YARP #include #include - #include +// BipedalLocomotion #include #include #include #include #include - // iDynTree #include #include +#include +#include +#include +#include +#include +#include // WalkingControllers library #include @@ -168,6 +168,13 @@ namespace WalkingControllers iDynTree::VectorDynSize m_desiredJointTorquesAdmittance; /**< Vector containing the desired joint torques for the low-level, computed by the joint admittance controller [Nm]. */ + iDynTree::Position m_CoMPositionTSID; /**< CoM position computed by the TSID controller. */ + iDynTree::Vector3 m_CoMVelocityTSID; /**< CoM velocity computed by the TSID controller. */ + iDynTree::Transform m_leftFootPoseTSID; /**< Left foot pose computed by the TSID controller. */ + iDynTree::Transform m_rightFootPoseTSID; /**< Right foot pose computed by the TSID controller. */ + iDynTree::Twist m_leftFootTwistTSID; /**< Left foot twist computed by the TSID controller. */ + iDynTree::Twist m_rightFootTwistTSID; /**< Right foot twist computed by the TSID controller. */ + iDynTree::Rotation m_inertial_R_worldFrame; /**< Rotation between the inertial and the world frame. */ yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ @@ -229,7 +236,13 @@ namespace WalkingControllers const iDynTree::Vector3& desiredCoMVelocity, const iDynTree::Rotation& desiredNeckOrientation, iDynTree::VectorDynSize &output); - + /** + * Set and solve the TSID problem. + * @param desiredCoMPosition desired CoM position; + * @param desiredCoMVelocity desired CoM velocity; + * @param desiredTorsoRotation desired torso rotation (rotation matrix); + * @return true in case of success and false otherwise. + */ bool solveBLFTSID(const iDynTree::Position& desiredCoMPosition, const iDynTree::Vector3& desiredCoMVelocity, const iDynTree::Rotation& desiredTorsoRotation); @@ -242,6 +255,12 @@ namespace WalkingControllers void getBLFTSIDOutput(iDynTree::VectorDynSize &jointDesiredAcceleration, iDynTree::VectorDynSize &jointDesiredTorque); + /** + * Store the trajectories computed by the BLFTSID controller. + * @return true in case of success and false otherwise. + */ + bool storeBLFTSIDTrajectories(); + /** * Set the Admittance controller references, advance it, and return the output. * @param jointTorqueFeedforward joint torque feedforward term. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 1862548ed..b5288abaf 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -448,15 +448,21 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_vectorsCollectionServer.populateMetadata("com::position::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("com::position::desired", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("com::position::CoM_ZMP_controller", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("com::position::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("com::velocity::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("com::velocity::tsid", {"x", "y", "z"}); // Left foot m_vectorsCollectionServer.populateMetadata("left_foot::position::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("left_foot::position::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("left_foot::position::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("left_foot::orientation::measured", {"roll", "pitch", "yaw"}); m_vectorsCollectionServer.populateMetadata("left_foot::orientation::desired", {"roll", "pitch", "yaw"}); + m_vectorsCollectionServer.populateMetadata("left_foot::orientation::tsid", {"roll", "pitch", "yaw"}); m_vectorsCollectionServer.populateMetadata("left_foot::linear_velocity::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("left_foot::linear_velocity::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("left_foot::angular_velocity::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("left_foot::angular_velocity::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("left_foot::linear_force::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("left_foot::angular_torque::measured", {"x", "y", "z"}); @@ -466,10 +472,14 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) // Right foot m_vectorsCollectionServer.populateMetadata("right_foot::position::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("right_foot::position::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("right_foot::position::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("right_foot::orientation::measured", {"roll", "pitch", "yaw"}); m_vectorsCollectionServer.populateMetadata("right_foot::orientation::desired", {"roll", "pitch", "yaw"}); + m_vectorsCollectionServer.populateMetadata("right_foot::orientation::tsid", {"roll", "pitch", "yaw"}); m_vectorsCollectionServer.populateMetadata("right_foot::linear_velocity::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("right_foot::linear_velocity::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("right_foot::angular_velocity::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("right_foot::angular_velocity::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("right_foot::linear_force::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("right_foot::angular_torque::measured", {"x", "y", "z"}); @@ -698,6 +708,28 @@ void WalkingModule::getBLFTSIDOutput(iDynTree::VectorDynSize &jointDesiredAccele jointDesiredTorque = m_BLFTSIDSolver->getDesiredJointTorque(); } +bool WalkingModule::storeBLFTSIDTrajectories() +{ + + // set robot state to TSID joint values + if (!m_FKSolver->setInternalRobotState(m_qDesiredTSID, m_dqDesiredTSID)) + { + yError() << "[WalkingModule::updateModule] Unable to set the internal robot state."; + return false; + } + + m_CoMPositionTSID = m_FKSolver->getCoMPosition(); + m_CoMVelocityTSID = m_FKSolver->getCoMVelocity(); + + m_leftFootPoseTSID = m_FKSolver->getLeftFootToWorldTransform(); + m_rightFootPoseTSID = m_FKSolver->getRightFootToWorldTransform(); + + m_leftFootTwistTSID = m_FKSolver->getLeftFootTwist(); + m_rightFootTwistTSID = m_FKSolver->getRightFootTwist(); + + return true; +} + bool WalkingModule::advanceJointAdmittanceController(const iDynTree::VectorDynSize & jointTorqueFeedforward, const iDynTree::VectorDynSize & jointDesiredPosition, const iDynTree::VectorDynSize & jointDesiredVelocity, @@ -1280,6 +1312,8 @@ bool WalkingModule::updateModule() iDynTree::toEigen(m_dqDesiredTSID) = m_jointAccelerationIntegrator->getJointVelocity(); iDynTree::toEigen(m_qDesiredTSID) = m_jointAccelerationIntegrator->getJointPosition(); + storeBLFTSIDTrajectories(); + // admittance controller advanceJointAdmittanceController(m_desiredJointTorquesTSID, m_qDesiredTSID, @@ -1366,6 +1400,7 @@ bool WalkingModule::updateModule() m_vectorsCollectionServer.populateData("com::position::desired", CoMPositionDesired); m_vectorsCollectionServer.populateData("com::position::CoM_ZMP_controller", desiredCoMPositionTSID); + m_vectorsCollectionServer.populateData("com::position::tsid", m_CoMPositionTSID); // Manual definition of this value to add also the planned CoM height velocity std::vector CoMVelocityDesired(3); @@ -1374,10 +1409,12 @@ bool WalkingModule::updateModule() CoMVelocityDesired[2] = m_retargetingClient->comHeightVelocity(); m_vectorsCollectionServer.populateData("com::velocity::desired", CoMVelocityDesired); + m_vectorsCollectionServer.populateData("com::velocity::tsid", m_CoMVelocityTSID); // Left foot position m_vectorsCollectionServer.populateData("left_foot::position::measured", leftFoot.getPosition()); m_vectorsCollectionServer.populateData("left_foot::position::desired", m_leftTrajectory.front().getPosition()); + m_vectorsCollectionServer.populateData("left_foot::position::tsid", m_leftFootPoseTSID.getPosition()); // Left foot orientation const iDynTree::Vector3 leftFootOrientationMeasured = leftFoot.getRotation().asRPY(); @@ -1385,11 +1422,14 @@ bool WalkingModule::updateModule() const iDynTree::Vector3 leftFootOrientationDesired = m_leftTrajectory.front().getRotation().asRPY(); m_vectorsCollectionServer.populateData("left_foot::orientation::desired", leftFootOrientationDesired); + m_vectorsCollectionServer.populateData("left_foot::orientation::tsid", m_leftFootPoseTSID.getRotation().asRPY()); // "lf_des_dx", "lf_des_dy", "lf_des_dz", // "lf_des_droll", "lf_des_dpitch", "lf_des_dyaw", m_vectorsCollectionServer.populateData("left_foot::linear_velocity::desired", m_leftTwistTrajectory.front().getLinearVec3()); + m_vectorsCollectionServer.populateData("left_foot::linear_velocity::tsid", m_leftFootTwistTSID.getLinearVec3()); m_vectorsCollectionServer.populateData("left_foot::angular_velocity::desired", m_leftTwistTrajectory.front().getAngularVec3()); + m_vectorsCollectionServer.populateData("left_foot::angular_velocity::tsid", m_leftFootTwistTSID.getAngularVec3()); m_vectorsCollectionServer.populateData("left_foot::angular_velocity::correction", m_leftAngularVelocityCorrection); m_vectorsCollectionServer.populateData("right_foot::angular_velocity::correction", m_rightAngularVelocityCorrection); @@ -1402,17 +1442,21 @@ bool WalkingModule::updateModule() // Right foot position m_vectorsCollectionServer.populateData("right_foot::position::measured", rightFoot.getPosition()); m_vectorsCollectionServer.populateData("right_foot::position::desired", m_rightTrajectory.front().getPosition()); + m_vectorsCollectionServer.populateData("right_foot::position::tsid", m_rightFootPoseTSID.getPosition()); // Right foot orientation const iDynTree::Vector3 rightFootOrientationMeasured = rightFoot.getRotation().asRPY(); m_vectorsCollectionServer.populateData("right_foot::orientation::measured", rightFootOrientationMeasured); const iDynTree::Vector3 rightFootOrientationDesired = m_rightTrajectory.front().getRotation().asRPY(); m_vectorsCollectionServer.populateData("right_foot::orientation::desired", rightFootOrientationDesired); + m_vectorsCollectionServer.populateData("right_foot::orientation::tsid", m_rightFootPoseTSID.getRotation().asRPY()); // "rf_des_dx", "rf_des_dy", "rf_des_dz", // "rf_des_droll", "rf_des_dpitch", "rf_des_dyaw", m_vectorsCollectionServer.populateData("right_foot::linear_velocity::desired", m_rightTwistTrajectory.front().getLinearVec3()); + m_vectorsCollectionServer.populateData("right_foot::linear_velocity::tsid", m_rightFootTwistTSID.getLinearVec3()); m_vectorsCollectionServer.populateData("right_foot::angular_velocity::desired", m_rightTwistTrajectory.front().getAngularVec3()); + m_vectorsCollectionServer.populateData("right_foot::angular_velocity::tsid", m_rightFootTwistTSID.getAngularVec3()); // "rf_force_x", "rf_force_y", "rf_force_z", // "rf_force_roll", "rf_force_pitch", "rf_force_yaw", From 02d49d442048b3e2da33abdfb9e81dbf27d5c51d Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 27 Feb 2025 18:41:04 +0100 Subject: [PATCH 18/26] enable tsid-admittance controller on ergocubSN001 --- .../app/robots/ergoCubSN001/dcm_walking_with_joypad.ini | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini index e93ef74bc..2bb0ac237 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini @@ -16,6 +16,9 @@ skip_dcm_controller 1 # The goal port receives 2 or 3 doubles according to the selected planner controller goal_port_suffix /goal:i +# Remove this line if you don't want to use the TSID-Admittance control +use_TSID-Admittance 1 + # Scales the input coming from the goal port goal_port_scaling (0.5, 1.0, 0.5) From 6bfcea1dfe1f6fc7624ed9377ee55f2170c93826 Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 27 Feb 2025 18:45:03 +0100 Subject: [PATCH 19/26] downgrade walking with joypad for ergocubSN001 to 200 Hz --- .../dcm_walking/joypad_control/tasks/regularization.ini | 2 +- .../ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini | 2 +- .../app/robots/ergoCubSN001/dcm_walking_with_joypad.ini | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 758dfc72d..27793dae2 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 @@ -6,7 +6,7 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("stance", "walking") -sampling_time 0.001 +sampling_time 0.005 settling_time 0.5 [stance] 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 b804b6a85..e08bf7c84 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 @@ -4,7 +4,7 @@ kp_angular 5.0 states ("stance", "walking") -sampling_time 0.001 +sampling_time 0.005 settling_time 0.5 [stance] diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini index 2bb0ac237..ed31c8d06 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_with_joypad.ini @@ -34,7 +34,7 @@ name walking-coordinator # height of the com com_height 0.70 # sampling time -sampling_time 0.001 +sampling_time 0.005 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link height_reference_frame root_link From 3fa6e9a0afceac5bde3270fe1c689bb74cc39304 Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 27 Feb 2025 18:45:52 +0100 Subject: [PATCH 20/26] some gain tuning for ergocubSN001 --- .../common/joint_admittance_controller.ini | 13 ++++++------ .../dcm_walking/common/plannerParams.ini | 10 +++++----- .../TaskSpaceInverseDynamicsBlf.ini | 20 +++++++++---------- 3 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini index 1a3eeecec..0a0f144dc 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini @@ -1,13 +1,12 @@ [GAINS] - -kp (120.0, 80.0, 80.0, - 50.0, 50.0, 50.0, 50.0, - 50.0, 50.0, 50.0, 50.0, - 220.0, 220.0, 220.0, 220.0, 220.0, 220.0, - 220.0, 220.0, 220.0, 220.0, 220.0, 220.0) +kp (500.0, 500.0, 500.0, + 50.0, 50.0, 50.0, 50.0, + 50.0, 50.0, 50.0, 50.0, + 700.0, 1700.0, 700.0, 1000.0, 700.0, 700.0, + 700.0, 1700.0, 700.0, 1000.0, 700.0, 700.0) kd (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.1, 0.1, 0.1, 0.1, - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) \ No newline at end of file + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/plannerParams.ini index 09363081a..af52f14be 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/plannerParams.ini @@ -21,7 +21,7 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length -maxStepLength 0.32 +maxStepLength 0.28 minStepLength 0.01 maxLengthBackwardFactor 0.8 #Width @@ -31,8 +31,8 @@ minWidth 0.14 maxAngleVariation 18.0 minAngleVariation 5.0 #Timings -maxStepDuration 1.0 -minStepDuration 0.7 +maxStepDuration 8.0 +minStepDuration 7.0 ##Nominal Values #Width @@ -43,9 +43,9 @@ stepLandingVelocity -0.1 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 0.9 +nominalDuration 7.5 lastStepSwitchTime 0.15 -switchOverSwingRatio 0.2 +switchOverSwingRatio 1.0 #ZMP Delta leftZMPDelta (0.0 -0.005) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index 59d2b6579..60b62a41f 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -20,10 +20,10 @@ automatic_scaling false [LEFT_FOOT_TRACKING] type SE3Task priority 0 -kp_linear 10.0 -kp_angular 10.0 -kd_linear 1.0 -kd_angular 1.0 +kp_linear 50.0 +kp_angular 50.0 +kd_linear 14.0 +kd_angular 2.0 frame_name "l_sole" use_orientation_exogenous_feedback false use_position_exogenous_feedback false @@ -35,10 +35,10 @@ robot_acceleration_variable_name "robot_acceleration" [RIGHT_FOOT_TRACKING] type SE3Task priority 0 -kp_linear 10.0 -kp_angular 10.0 -kd_linear 1.0 -kd_angular 1.0 +kp_linear 50.0 +kp_angular 50.0 +kd_linear 14.0 +kd_angular 2.0 frame_name "r_sole" use_orientation_exogenous_feedback false use_position_exogenous_feedback false @@ -49,8 +49,8 @@ robot_acceleration_variable_name "robot_acceleration" [COM_TRACKING] type CoMTask -kp_linear 10.0 -kd_linear 1.0 +kp_linear 50.0 +kd_linear 14.0 priority 0 mask (true, true, false) use_exogenous_feedback false From a8efcccd1983db639069d2b5b863dc94dd9284ec Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 1 Apr 2025 12:04:48 +0200 Subject: [PATCH 21/26] add gz-sim world file for ergoCubGazeboV1_1 --- .../robots/ergoCubGazeboV1_1/world/world.sdf | 112 ++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1_1/world/world.sdf diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/world/world.sdf b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/world/world.sdf new file mode 100644 index 000000000..ee9adf854 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/world/world.sdf @@ -0,0 +1,112 @@ + + + + + 0.001 + 0.4 + 400 + + + + + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + model://ergoCubGazeboV1_1 + 0.0 0.0 0.75 0.0 -0.142 0.0 + + + + + 0.0 0.138 0.0 + + + + + -0.059 0.34 -0.225 0.71 0.0000 0.0000 0.0000 + + + + + -0.059 0.34 -0.225 0.71 0.0000 0.0000 0.0000 + + + + + 0.095 0.0157 -0.0038 -0.6000 -0.362 -0.016 + + + + + 0.095 0.0157 -0.0038 -0.6000 -0.362 -0.016 + + + + + + From fc051b6c2258823fe1199d421f7a4ee73e25e2fc Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 1 Apr 2025 12:05:57 +0200 Subject: [PATCH 22/26] set reference velocities to 0 in the admittance controller --- .../common/joint_admittance_controller.ini | 20 +++++++++---------- src/WalkingModule/src/Module.cpp | 4 +++- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini index 4bf7d4009..2e2809169 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini @@ -1,13 +1,13 @@ [GAINS] -kp (300.0, 300.0, 300.0, - 300.0, 300.0, 300.0, 300.0, - 300.0, 300.0, 300.0, 300.0, - 300.0, 300.0, 300.0, 300.0, 300.0, 300.0, - 300.0, 300.0, 300.0, 300.0, 300.0, 300.0) +kp (100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, 100.0, 100.0, 100.0) -kd (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.1, 0.1, 0.1, 0.1, - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) \ No newline at end of file +kd (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, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) \ No newline at end of file diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b5288abaf..1b9f23a0f 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -1315,9 +1315,11 @@ bool WalkingModule::updateModule() storeBLFTSIDTrajectories(); // admittance controller + iDynTree::VectorDynSize m_desiredJointVelocitiesAdmittance(m_robotControlHelper->getActuatedDoFs()); + m_desiredJointVelocitiesAdmittance.zero(); advanceJointAdmittanceController(m_desiredJointTorquesTSID, m_qDesiredTSID, - m_dqDesiredTSID, + m_desiredJointVelocitiesAdmittance, m_desiredJointTorquesAdmittance); // restore robot state From 4978759bd29457891e85be337308d22a1cc42037 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 1 Apr 2025 12:06:32 +0200 Subject: [PATCH 23/26] this commit has to be removed when merging this branch --- .../WalkingControllers/WalkingModule/Module.h | 6 ++--- src/WalkingModule/src/Module.cpp | 22 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index fc2a511f3..497a11197 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -73,9 +73,9 @@ namespace WalkingControllers double m_dT; /**< RFModule period. */ double m_time; /**< Current time. */ - double m_lastSetGoalTime; /**< Time of the last set goal. */ - double m_maxTimeToWaitForGoal; /**< Maximum time to wait for a goal. */ - std::string m_robot; /**< Robot name. */ + // double m_lastSetGoalTime; /**< Time of the last set goal. */ + // double m_maxTimeToWaitForGoal; /**< Maximum time to wait for a goal. */ + // std::string m_robot; /**< Robot name. */ bool m_useMPC; /**< True if the MPC controller is used. */ bool m_useQPIK; /**< True if the QP-IK is used. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 1b9f23a0f..8ddd884ad 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -150,7 +150,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) std::string goalSuffix = rf.check("goal_port_suffix", yarp::os::Value("/goal:i")).asString(); m_skipDCMController = rf.check("skip_dcm_controller", yarp::os::Value(false)).asBool(); m_removeZMPOffset = rf.check("remove_zmp_offset", yarp::os::Value(false)).asBool(); - m_maxTimeToWaitForGoal = rf.check("max_time_to_wait_for_goal", yarp::os::Value(1.0)).asFloat64(); + // m_maxTimeToWaitForGoal = rf.check("max_time_to_wait_for_goal", yarp::os::Value(1.0)).asFloat64(); m_goalScaling.resize(3); if (!YarpUtilities::getVectorFromSearchable(rf, "goal_port_scaling", m_goalScaling)) @@ -955,16 +955,16 @@ bool WalkingModule::updateModule() yError() << "[WalkingModule::updateModule] Unable to set the planner input"; return false; } - m_lastSetGoalTime = m_time; - } - else if (!m_firstRun && ((m_time - m_lastSetGoalTime) > m_maxTimeToWaitForGoal)) - { - yarp::sig::Vector dummy(3, 0.0); - if (!setPlannerInput(dummy)) - { - yError() << "[WalkingModule::updateModule] Unable to set the planner input"; - return false; - } + // m_lastSetGoalTime = m_time; + // } + // else if (!m_firstRun && ((m_time - m_lastSetGoalTime) > m_maxTimeToWaitForGoal)) + // { + // yarp::sig::Vector dummy(3, 0.0); + // if (!setPlannerInput(dummy)) + // { + // yError() << "[WalkingModule::updateModule] Unable to set the planner input"; + // return false; + // } } // if a new trajectory is required check if its the time to evaluate the new trajectory or From 70e2aa3e7fd5dc2b879b5c77549f4136fb2261bb Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 1 Apr 2025 17:56:54 +0200 Subject: [PATCH 24/26] set previous torque as regularizer for torque regularization task --- src/WalkingModule/src/Module.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 8ddd884ad..4747cc37c 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -678,7 +678,7 @@ bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, m_BLFTSIDSolver->setRightContactActive(m_rightInContact.front()); ok = ok && m_BLFTSIDSolver->setLeftContactWrenchSetPoint(leftContactWrench); ok = ok && m_BLFTSIDSolver->setRightContactWrenchSetPoint(rightContactWrench); - ok = ok && m_BLFTSIDSolver->setTorqueRegularizationSetPoint(zeroNdofVector); + ok = ok && m_BLFTSIDSolver->setTorqueRegularizationSetPoint(m_desiredJointTorquesTSID); if (m_useRootLinkForHeight) { @@ -931,6 +931,15 @@ bool WalkingModule::updateModule() m_comHeightOffset = 0.0; } + // warm start desired torque for tsid + iDynTree::VectorDynSize generalizedGravityForces(m_robotControlHelper->getActuatedDoFs()+6); + m_FKSolver->getKinDyn()->generalizedGravityForces(generalizedGravityForces); + iDynTree::toEigen(m_desiredJointTorquesTSID) = + iDynTree::toEigen(generalizedGravityForces).tail(m_robotControlHelper->getActuatedDoFs()); + + yInfo() << "[WalkingModule::updateModule] Warm start the desired torque for tsid." + << m_desiredJointTorquesTSID.toString(); + m_robotState = WalkingFSM::Prepared; yInfo() << "[WalkingModule::updateModule] The robot is prepared."; From bbc07bd484b96549851d30629bda08480e0dac20 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 1 Apr 2025 17:57:42 +0200 Subject: [PATCH 25/26] add plot of base position returned by tsid controller --- .../include/WalkingControllers/WalkingModule/Module.h | 1 + src/WalkingModule/src/Module.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 497a11197..625928a9f 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -174,6 +174,7 @@ namespace WalkingControllers iDynTree::Transform m_rightFootPoseTSID; /**< Right foot pose computed by the TSID controller. */ iDynTree::Twist m_leftFootTwistTSID; /**< Left foot twist computed by the TSID controller. */ iDynTree::Twist m_rightFootTwistTSID; /**< Right foot twist computed by the TSID controller. */ + iDynTree::Position m_rootLinkPositionTSID; /**< Root link position computed by the TSID controller. */ iDynTree::Rotation m_inertial_R_worldFrame; /**< Rotation between the inertial and the world frame. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 4747cc37c..4844627bc 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -501,6 +501,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) } // root link information + m_vectorsCollectionServer.populateMetadata("root_link::position::desired::tsid", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("root_link::position::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("root_link::orientation::measured", {"roll", "pitch", "yaw"}); m_vectorsCollectionServer.populateMetadata("root_link::linear_velocity::measured", {"x", "y", "z"}); @@ -721,6 +722,8 @@ bool WalkingModule::storeBLFTSIDTrajectories() m_CoMPositionTSID = m_FKSolver->getCoMPosition(); m_CoMVelocityTSID = m_FKSolver->getCoMVelocity(); + m_rootLinkPositionTSID = m_FKSolver->getRootLinkToWorldTransform().getPosition(); + m_leftFootPoseTSID = m_FKSolver->getLeftFootToWorldTransform(); m_rightFootPoseTSID = m_FKSolver->getRightFootToWorldTransform(); @@ -1492,6 +1495,7 @@ bool WalkingModule::updateModule() } // root link information + m_vectorsCollectionServer.populateData("root_link::position::desired::tsid", m_rootLinkPositionTSID); m_vectorsCollectionServer.populateData("root_link::position::measured", m_FKSolver->getRootLinkToWorldTransform().getPosition()); m_vectorsCollectionServer.populateData("root_link::orientation::measured", m_FKSolver->getRootLinkToWorldTransform().getRotation().asRPY()); m_vectorsCollectionServer.populateData("root_link::linear_velocity::measured", m_FKSolver->getRootLinkVelocity().getLinearVec3()); From eff43b1bbdfa41a113e701d49cc8af493384b643 Mon Sep 17 00:00:00 2001 From: Lorenzo Date: Tue, 1 Apr 2025 18:14:14 +0200 Subject: [PATCH 26/26] some controller gain tuning --- .../common/joint_admittance_controller.ini | 10 ++--- .../TaskSpaceInverseDynamicsBlf.ini | 38 +++++++++---------- src/WalkingModule/src/Module.cpp | 2 +- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini index 2e2809169..40c432fea 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini @@ -1,10 +1,10 @@ [GAINS] -kp (100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, 100.0, 100.0, 100.0) +kp (200.0, 200.0, 200.0, + 200.0, 200.0, 200.0, 200.0, + 200.0, 200.0, 200.0, 200.0, + 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, + 200.0, 200.0, 200.0, 200.0, 200.0, 200.0) kd (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/TaskSpaceInverseDynamicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini index 8dbec74f9..36eea80d7 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/TaskSpaceInverseDynamicsBlf.ini @@ -22,8 +22,8 @@ type SE3Task priority 0 kp_linear 5.0 kp_angular 5.0 -kd_linear 1.0 -kd_angular 1.0 +kd_linear 4.0 +kd_angular 4.0 frame_name "l_sole" use_orientation_exogenous_feedback false use_position_exogenous_feedback false @@ -37,8 +37,8 @@ type SE3Task priority 0 kp_linear 5.0 kp_angular 5.0 -kd_linear 1.0 -kd_angular 1.0 +kd_linear 4.0 +kd_angular 4.0 frame_name "r_sole" use_orientation_exogenous_feedback false use_position_exogenous_feedback false @@ -50,7 +50,7 @@ robot_acceleration_variable_name "robot_acceleration" [COM_TRACKING] type CoMTask kp_linear 5.0 -kd_linear 2.0 +kd_linear 4.0 priority 0 mask (true, true, false) use_exogenous_feedback false @@ -61,7 +61,7 @@ robot_acceleration_variable_name "robot_acceleration" [TORSO_TRACKING] type SO3Task kp_angular 5.0 -kd_angular 1.0 +kd_angular 4.0 frame_name "chest" priority 1 weight (500.0, 500.0, 500.0) @@ -73,7 +73,7 @@ weight_provider_type "ConstantWeightProvider" type R3Task frame_name "root_link" kp_linear 5.0 -kd_linear 1.0 +kd_linear 4.0 mask (false, false, true) priority 0 weight (500.0) @@ -97,15 +97,15 @@ 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) -kd (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, 1.0, 1.0, 1.0, 1.0 - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) +kd (4.0, 4.0, 4.0, + 4.0, 4.0, 4.0, 4.0, + 4.0, 4.0, 4.0, 4.0, + 4.0, 4.0, 4.0, 4.0, 4.0, 4.0 + 4.0, 4.0, 4.0, 4.0, 4.0, 4.0) weight (1.0, 1.0, 1.0, - 50.0, 50.0, 50.0, 1.0, - 50.0, 50.0, 50.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, 1.0, 1.0 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) robot_acceleration_variable_name "robot_acceleration" @@ -162,11 +162,11 @@ weight_provider_type "ConstantWeightProvider" variable_size 23 priority 1 -weight (0.01, 0.01, 0.01, - 0.01, 0.01, 0.01, 0.01, - 0.01, 0.01, 0.01, 0.01, - 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 - 0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +weight (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.1, 0.1, 0.1, 0.1 + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) [include JOINT_DYNAMICS "./tsid_tasks/joint_dynamics_task.ini"] [include BASE_DYNAMICS "./tsid_tasks/base_dynamics_task.ini"] diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 4844627bc..15ec77dbc 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -795,7 +795,7 @@ bool WalkingModule::computeLocalCoPCorrection(Eigen::Ref leftCo Eigen::Vector3d errorCoPRight; errorCoPLeft = -leftCoP; - double gain{0.1}; + double gain{5.0}; Eigen::Vector3d localCorrectionLeft; localCorrectionLeft(0) = errorCoPLeft(1) * (-gain);