diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e9870509..15103220 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 00000000..369cafd3 --- /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 00000000..990654d5 --- /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 00000000..bcd543b2 --- /dev/null +++ b/src/Integrators/src/jointIntegrators.cpp @@ -0,0 +1,168 @@ +// 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( + "{} Failed to set state of the dynamical system", + logPrefix); + 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 { + 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 00000000..f8ca4ee8 --- /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 00000000..cf47820b --- /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 00000000..9c6f4a38 --- /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/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h index 4e4bd9c8..043eb308 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 9078f1d0..5823cb07 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/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index e2637b6f..bed3ee23 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 cbf26422..14e77b6b 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) { diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 895c91a1..83a87564 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 aa0bf343..683cc5ab 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/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index a7e2d0d3..1dbca068 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 00000000..40c432fe --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/joint_admittance_controller.ini @@ -0,0 +1,13 @@ +[GAINS] + +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, + 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/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini index d051caba..7b40ff73 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 8f282a3b..5e2f7e29 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 new file mode 100644 index 00000000..36eea80d --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/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 5.0 +kp_angular 5.0 +kd_linear 4.0 +kd_angular 4.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 5.0 +kp_angular 5.0 +kd_linear 4.0 +kd_angular 4.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 5.0 +kd_linear 4.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 4.0 +frame_name "chest" +priority 1 +weight (500.0, 500.0, 500.0) +robot_acceleration_variable_name "robot_acceleration" +weight_provider_type "ConstantWeightProvider" + + +[ROOT_TRACKING] +type R3Task +frame_name "root_link" +kp_linear 5.0 +kd_linear 4.0 +mask (false, false, true) +priority 0 +weight (500.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 (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, + 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" +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.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"] +[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 00000000..cdd8f7d0 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/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/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 00000000..0f3094bb --- /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 00000000..014bdf8f --- /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/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini index b6fe27b7..2916c13a 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/app/robots/ergoCubGazeboV1_1/world/world.sdf b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/world/world.sdf new file mode 100644 index 00000000..ee9adf85 --- /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 + + + + + + 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 00000000..0a0f144d --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/joint_admittance_controller.ini @@ -0,0 +1,12 @@ +[GAINS] +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) 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 09363081..af52f14b 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/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/common/zmpControllerParams.ini index 3a68089a..2494fd56 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/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 00000000..60b62a41 --- /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 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 +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 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 +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 50.0 +kd_linear 14.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/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini index 758dfc72..27793dae 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 b804b6a8..e08bf7c8 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/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 00000000..cdd8f7d0 --- /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 00000000..0f3094bb --- /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 00000000..014bdf8f --- /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 62217c6a..ed31c8d0 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) @@ -31,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 @@ -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 781e7937..625928a9 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -5,8 +5,6 @@ #define WALKING_MODULE_HPP // std -#include -#include #include #include #include @@ -14,18 +12,24 @@ // YARP #include #include - #include - +// BipedalLocomotion +#include +#include #include #include #include - // iDynTree #include #include +#include +#include +#include +#include +#include +#include // WalkingControllers library #include @@ -39,6 +43,11 @@ #include #include +#include +#include + +#include +#include #include @@ -64,12 +73,13 @@ 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. */ + 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 +98,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 +108,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. */ @@ -107,6 +132,12 @@ 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. */ + 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. */ @@ -122,10 +153,28 @@ 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_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_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::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. */ @@ -188,6 +237,45 @@ 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); + + /** + * 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); + + /** + * 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. + * @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. @@ -196,6 +284,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 7d1ea4ff..15ec77db 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -32,7 +32,6 @@ #include #include #include -#include using namespace WalkingControllers; @@ -60,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()); @@ -90,6 +95,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 @@ -133,12 +144,13 @@ 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(); 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)) @@ -314,6 +326,35 @@ 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); + } + + // initialize the linear inverted pendulum model m_stableDCMModel = std::make_unique(); if (!m_stableDCMModel->initialize(generalOptions)) @@ -407,37 +448,60 @@ 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"}); + 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"}); + 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"}); // 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::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"}); @@ -456,6 +520,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 +533,13 @@ 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()); + m_desiredJointTorquesTSID.resize(m_robotControlHelper->getActuatedDoFs()); + m_desiredJointTorquesAdmittance.resize(m_robotControlHelper->getActuatedDoFs()); yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -555,6 +625,133 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition, return ok; } +bool WalkingModule::solveBLFTSID(const iDynTree::Position& desiredCoMPosition, + const iDynTree::Vector3& desiredCoMVelocity, + const iDynTree::Rotation& desiredTorsoRotation) +{ + iDynTree::Vector3 zero3dVector; + zero3dVector.zero(); + + iDynTree::Twist zeroTwist; + zeroTwist.zero(); + + iDynTree::VectorDynSize zeroNdofVector(m_robotControlHelper->getActuatedDoFs()); + zeroNdofVector.zero(); + + // compute the desired contact wrenches + double robotMass = m_FKSolver->getKinDyn()->getRobotModel().getTotalMass(); + 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); + + + // compute ankle strategy + 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()) + m_leftAngularVelocityCorrection; + leftTwist.setAngularVec3(temp); + iDynTree::toEigen(temp) = iDynTree::toEigen(rightTwist.getAngularVec3()) + m_rightAngularVelocityCorrection; + 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(), leftTwist, + m_leftAccelerationTrajectory.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); + 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(m_desiredJointTorquesTSID); + + if (m_useRootLinkForHeight) + { + ok = ok && m_BLFTSIDSolver->setRootTrackingSetPoint(desiredCoMPosition, desiredCoMVelocity, zero3dVector); + } + + if (!ok) + { + yError() << "[WalkingModule::solveBLFTSID] Unable to set the TSID set points."; + } + + // solve the optimization problem + ok = ok && m_BLFTSIDSolver->solve(); + + if (!ok) + { + yError() << "[WalkingModule::solveBLFTSID] Unable to solve the optimization problem."; + } + + return ok; +} + +void WalkingModule::getBLFTSIDOutput(iDynTree::VectorDynSize &jointDesiredAcceleration, + iDynTree::VectorDynSize &jointDesiredTorque) +{ + jointDesiredAcceleration = m_BLFTSIDSolver->getDesiredJointAcceleration(); + 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_rootLinkPositionTSID = m_FKSolver->getRootLinkToWorldTransform().getPosition(); + + 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, + 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; @@ -581,6 +778,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{5.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); @@ -605,7 +852,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,9 +862,9 @@ bool WalkingModule::updateModule() return true; } - yarp::sig::Vector buffer(m_qDesired.size()); - iDynTree::toYarp(m_qDesired, buffer); - // instantiate Integrator object + yarp::sig::Vector buffer(m_qDesiredIK.size()); + iDynTree::toYarp(m_qDesiredIK, buffer); + // instantiate Integrators object yarp::sig::Matrix jointLimits(m_robotControlHelper->getActuatedDoFs(), 2); for (int i = 0; i < m_robotControlHelper->getActuatedDoFs(); i++) @@ -627,6 +874,13 @@ bool WalkingModule::updateModule() } m_velocityIntegral = std::make_unique(m_dT, buffer, jointLimits); + if (m_useTSIDadmittance){ + m_qDesiredTSID = m_qDesiredIK; + m_dqDesiredTSID.zero(); + 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()); @@ -645,10 +899,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; @@ -680,6 +934,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."; @@ -704,16 +967,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 @@ -898,19 +1161,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::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::Vector3 desiredCoMVelocity; - desiredCoMVelocity(0) = outputZMPCoMControllerVelocity(0); - desiredCoMVelocity(1) = outputZMPCoMControllerVelocity(1); - desiredCoMVelocity(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) { @@ -938,7 +1211,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; @@ -946,20 +1219,29 @@ bool WalkingModule::updateModule() yawRotation = yawRotation.inverse() * m_trajectoryGenerator->getChestAdditionalRotation(); - if (!solveBLFIK(desiredCoMPosition, - desiredCoMVelocity, + if (!solveBLFIK(desiredCoMPositionIK, + desiredCoMVelocityIK, 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); + + // 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; + } + + // restore robot state if (!m_FKSolver->setInternalRobotState(m_robotControlHelper->getJointPosition(), m_robotControlHelper->getJointVelocity())) @@ -985,7 +1267,7 @@ bool WalkingModule::updateModule() } if (!m_IKSolver->computeIK(m_leftTrajectory.front(), m_rightTrajectory.front(), - desiredCoMPosition, m_qDesired)) + desiredCoMPositionIK, m_qDesiredIK)) { yError() << "[WalkingModule::updateModule] Error during the inverse Kinematics iteration."; return false; @@ -1017,10 +1299,73 @@ 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(desiredCoMPositionTSID, + desiredCoMVelocityTSID, yawRotation)){ + 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(); + + storeBLFTSIDTrajectories(); + + // admittance controller + iDynTree::VectorDynSize m_desiredJointVelocitiesAdmittance(m_robotControlHelper->getActuatedDoFs()); + m_desiredJointVelocitiesAdmittance.zero(); + advanceJointAdmittanceController(m_desiredJointTorquesTSID, + m_qDesiredTSID, + m_desiredJointVelocitiesAdmittance, + m_desiredJointTorquesAdmittance); + + // 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_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)) + { + yError() << "[WalkingModule::updateModule] Error while setting the reference position to iCub."; + return false; + } } // send data to the logger @@ -1068,7 +1413,8 @@ 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); + 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); @@ -1077,10 +1423,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(); @@ -1088,11 +1436,17 @@ 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); // "lf_force_x", "lf_force_y", "lf_force_z", // "lf_force_roll", "lf_force_pitch", "lf_force_yaw", @@ -1102,17 +1456,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", @@ -1121,13 +1479,23 @@ 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::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::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()); @@ -1264,15 +1632,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; @@ -1403,6 +1771,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; @@ -1413,6 +1783,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); @@ -1421,6 +1793,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); @@ -1436,11 +1809,15 @@ 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); 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); @@ -1456,6 +1833,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 diff --git a/src/WholeBodyControllers/CMakeLists.txt b/src/WholeBodyControllers/CMakeLists.txt index 6ced55e0..7ed46107 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 00000000..7ec044e7 --- /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 + +#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::SpatialAcc &desiredAcceleration); + + bool + setRightFootTrackingSetPoint(const iDynTree::Transform &desiredTransform, + const iDynTree::Twist &desiredVelocity, + const iDynTree::SpatialAcc &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; + const iDynTree::VectorDynSize &getDesiredJointTorque() 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; +}; + +} // 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 00000000..424fe60e --- /dev/null +++ b/src/WholeBodyControllers/src/BLFTSID.cpp @@ -0,0 +1,191 @@ +// 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]"; + + 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(); + 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("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) && + 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::SpatialAcc &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::SpatialAcc &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) { + return m_qpTSID.tasks.rootTracking->setSetPoint( + iDynTree::toEigen(position), iDynTree::toEigen(linearVelocity), + iDynTree::toEigen(linearAcceleration)); +} + +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; +} + +const iDynTree::VectorDynSize &BLFTSID::getDesiredJointTorque() const { + return m_jointTorques; +} \ No newline at end of file