From 70999c9169410b592494f34d228d2725f152d850 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Wed, 29 May 2024 22:44:12 +0900 Subject: [PATCH 1/5] Add ExternalTorqueSensor and VirtualTorqueSensor to mc_rbdyn --- include/mc_rbdyn/ExternalTorqueSensor.h | 53 +++++++++++++++++++++++++ include/mc_rbdyn/VirtualTorqueSensor.h | 53 +++++++++++++++++++++++++ src/CMakeLists.txt | 4 ++ src/mc_rbdyn/ExternalTorqueSensor.cpp | 36 +++++++++++++++++ src/mc_rbdyn/VirtualTorqueSensor.cpp | 35 ++++++++++++++++ 5 files changed, 181 insertions(+) create mode 100644 include/mc_rbdyn/ExternalTorqueSensor.h create mode 100644 include/mc_rbdyn/VirtualTorqueSensor.h create mode 100644 src/mc_rbdyn/ExternalTorqueSensor.cpp create mode 100644 src/mc_rbdyn/VirtualTorqueSensor.cpp diff --git a/include/mc_rbdyn/ExternalTorqueSensor.h b/include/mc_rbdyn/ExternalTorqueSensor.h new file mode 100644 index 0000000000..024d4ca4f2 --- /dev/null +++ b/include/mc_rbdyn/ExternalTorqueSensor.h @@ -0,0 +1,53 @@ +/* + * Copyright 2015-2023 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include + +namespace mc_rbdyn +{ + +struct Robot; + +/** This struct is intended to hold static information about an external torques sensor + * and the current reading of said sensor. + */ +struct MC_RBDYN_DLLAPI ExternalTorqueSensor : public Device +{ +public: + /** Default constructor, this does not represent a valid external torques sensor */ + ExternalTorqueSensor(); + + /** Construct a valid external torques sensor based on static information, this + * force sensor can then be used to provide sensing information to the robot. + * + * @param name Name of the sensor + * + * @param size Number of DoF + * + */ + ExternalTorqueSensor(const std::string & name, const int & size); + + ExternalTorqueSensor(const ExternalTorqueSensor & ets); + + ExternalTorqueSensor & operator=(const ExternalTorqueSensor & ets); + + ExternalTorqueSensor(ExternalTorqueSensor &&) = default; + + /** Destructor */ + ~ExternalTorqueSensor() noexcept override; + + inline const Eigen::VectorXd & torques() const { return externalJointTorques_; } + + inline void torques(Eigen::VectorXd & torques) { externalJointTorques_ = torques; } + + DevicePtr clone() const override; + +private: + Eigen::VectorXd externalJointTorques_; + int size_; +}; + +} // namespace mc_rbdyn diff --git a/include/mc_rbdyn/VirtualTorqueSensor.h b/include/mc_rbdyn/VirtualTorqueSensor.h new file mode 100644 index 0000000000..0f81291a45 --- /dev/null +++ b/include/mc_rbdyn/VirtualTorqueSensor.h @@ -0,0 +1,53 @@ +/* + * Copyright 2015-2023 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include + +namespace mc_rbdyn +{ + +struct Robot; + +/** This struct is intended to hold static information about an external torques sensor + * and the current reading of said sensor. + */ +struct MC_RBDYN_DLLAPI VirtualTorqueSensor : public Device +{ +public: + /** Default constructor, this does not represent a valid external torques sensor */ + VirtualTorqueSensor(); + + /** Construct a valid external torques sensor based on static information, this + * force sensor can then be used to provide sensing information to the robot. + * + * @param name Name of the sensor + * + * @param size Number of DoF + * + */ + VirtualTorqueSensor(const std::string & name, const int & size); + + VirtualTorqueSensor(const VirtualTorqueSensor & ets); + + VirtualTorqueSensor & operator=(const VirtualTorqueSensor & ets); + + VirtualTorqueSensor(VirtualTorqueSensor &&) = default; + + /** Destructor */ + ~VirtualTorqueSensor() noexcept override; + + inline const Eigen::VectorXd & torques() const { return virtualJointTorques_; } + + inline void torques(Eigen::VectorXd & torques) { virtualJointTorques_ = torques; } + + DevicePtr clone() const override; + +private: + Eigen::VectorXd virtualJointTorques_; + int size_; +}; + +} // namespace mc_rbdyn diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 34a54c1cd5..34bbc859a2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -237,6 +237,8 @@ set(mc_rbdyn_SRC mc_rbdyn/BodySensor.cpp mc_rbdyn/Frame.cpp mc_rbdyn/RobotFrame.cpp + mc_rbdyn/ExternalTorqueSensor.cpp + mc_rbdyn/VirtualTorqueSensor.cpp ) set(mc_rbdyn_HDR @@ -278,6 +280,8 @@ set(mc_rbdyn_HDR ../include/mc_rbdyn/Frame.h ../include/mc_rbdyn/RobotFrame.h ../include/mc_rbdyn/JointSensor.h + ../include/mc_rbdyn/ExternalTorqueSensor.h + ../include/mc_rbdyn/VirtualTorqueSensor.h ) set(mc_tvm_HDR_DIR ../include/mc_tvm) diff --git a/src/mc_rbdyn/ExternalTorqueSensor.cpp b/src/mc_rbdyn/ExternalTorqueSensor.cpp new file mode 100644 index 0000000000..d8854f75e6 --- /dev/null +++ b/src/mc_rbdyn/ExternalTorqueSensor.cpp @@ -0,0 +1,36 @@ +#include + +namespace mc_rbdyn +{ + +ExternalTorqueSensor::ExternalTorqueSensor() : ExternalTorqueSensor("", 0) {} + +ExternalTorqueSensor::ExternalTorqueSensor(const std::string & name, const int & size) +: Device(name), externalJointTorques_(Eigen::VectorXd::Zero(size)), size_(size) +{ +} + +ExternalTorqueSensor::ExternalTorqueSensor(const ExternalTorqueSensor & ets) +: ExternalTorqueSensor(ets.name_, ets.size_) +{ + externalJointTorques_ = ets.externalJointTorques_; +} + +ExternalTorqueSensor & ExternalTorqueSensor::operator=(const ExternalTorqueSensor & ets) +{ + if(&ets == this) { return *this; } + name_ = ets.name_; + parent_ = ets.parent_; + X_p_s_ = ets.X_p_s_; + externalJointTorques_ = ets.externalJointTorques_; + return *this; +} + +ExternalTorqueSensor::~ExternalTorqueSensor() noexcept = default; + +DevicePtr ExternalTorqueSensor::clone() const +{ + return DevicePtr(new ExternalTorqueSensor(*this)); +} + +} // namespace mc_rbdyn diff --git a/src/mc_rbdyn/VirtualTorqueSensor.cpp b/src/mc_rbdyn/VirtualTorqueSensor.cpp new file mode 100644 index 0000000000..037c169b56 --- /dev/null +++ b/src/mc_rbdyn/VirtualTorqueSensor.cpp @@ -0,0 +1,35 @@ +#include + +namespace mc_rbdyn +{ + +VirtualTorqueSensor::VirtualTorqueSensor() : VirtualTorqueSensor("", 0) {} + +VirtualTorqueSensor::VirtualTorqueSensor(const std::string & name, const int & size) +: Device(name), virtualJointTorques_(Eigen::VectorXd::Zero(size)), size_(size) +{ +} + +VirtualTorqueSensor::VirtualTorqueSensor(const VirtualTorqueSensor & ets) : VirtualTorqueSensor(ets.name_, ets.size_) +{ + virtualJointTorques_ = ets.virtualJointTorques_; +} + +VirtualTorqueSensor & VirtualTorqueSensor::operator=(const VirtualTorqueSensor & ets) +{ + if(&ets == this) { return *this; } + name_ = ets.name_; + parent_ = ets.parent_; + X_p_s_ = ets.X_p_s_; + virtualJointTorques_ = ets.virtualJointTorques_; + return *this; +} + +VirtualTorqueSensor::~VirtualTorqueSensor() noexcept = default; + +DevicePtr VirtualTorqueSensor::clone() const +{ + return DevicePtr(new VirtualTorqueSensor(*this)); +} + +} // namespace mc_rbdyn From 4348a04c40d733f418b1d095115925f54419c554 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Thu, 30 May 2024 11:39:03 +0900 Subject: [PATCH 2/5] Implement new ExternalTorqueSensor and VirtualTorqueSensor to mc_tvm/DynamicFunction.\nVirtualTorqueSensor is used as a feed forward at torque level in DynamicFunction's b_.\nExternalTorqueSensor is included in mc_tvm/Robot and two new getter tauExternal and alphaDExternal are added along with an updateEF methode.\nupdateEF methode is registered as an update function and an output in tvm to prevent repeted computation of alphaDExternal, it anticipates the addition of compliant tasks that depend on this alphaDExternal. --- include/mc_solver/DynamicsConstraint.h | 9 +++++++-- include/mc_tvm/DynamicFunction.h | 5 ++++- include/mc_tvm/Robot.h | 19 +++++++++++++++++-- src/mc_solver/DynamicsConstraint.cpp | 22 ++++++++++++++-------- src/mc_tvm/DynamicFunction.cpp | 13 +++++++++++-- src/mc_tvm/Robot.cpp | 16 +++++++++++++++- 6 files changed, 68 insertions(+), 16 deletions(-) diff --git a/include/mc_solver/DynamicsConstraint.h b/include/mc_solver/DynamicsConstraint.h index 55c194b28f..a1bc21e6c2 100644 --- a/include/mc_solver/DynamicsConstraint.h +++ b/include/mc_solver/DynamicsConstraint.h @@ -29,7 +29,11 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint * \param timeStep Solver timestep * \param infTorque If true, ignore the torque limits set in the robot model */ - DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, bool infTorque = false); + DynamicsConstraint(const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double timeStep, + bool infTorque = false, + bool compensateExternalForces = false); /** Constructor * Builds a damped joint limits constraint and a motion constr depending on @@ -48,7 +52,8 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint double timeStep, const std::array & damper, double velocityPercent = 1.0, - bool infTorque = false); + bool infTorque = false, + bool compensateExternalForces = false); /** Returns the tasks::qp::MotionConstr * diff --git a/include/mc_tvm/DynamicFunction.h b/include/mc_tvm/DynamicFunction.h index 2e2156a0ca..ea122400db 100644 --- a/include/mc_tvm/DynamicFunction.h +++ b/include/mc_tvm/DynamicFunction.h @@ -14,6 +14,8 @@ #include +#include + namespace mc_tvm { @@ -36,7 +38,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun SET_UPDATES(DynamicFunction, Jacobian, B) /** Construct the equation of motion for a given robot */ - DynamicFunction(const mc_rbdyn::Robot & robot); + DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateExternalForces = false); /** Add a contact to the function * @@ -73,6 +75,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun void updateb(); const mc_rbdyn::Robot & robot_; + const bool compensateExternalForces_; /** Holds data for the force part of the motion equation */ struct ForceContact diff --git a/include/mc_tvm/Robot.h b/include/mc_tvm/Robot.h index bc238e355d..d9188aa187 100644 --- a/include/mc_tvm/Robot.h +++ b/include/mc_tvm/Robot.h @@ -16,6 +16,8 @@ #include +#include + namespace mc_tvm { @@ -47,8 +49,8 @@ namespace mc_tvm */ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node { - SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C) - SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C) + SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C, ExternalForces) + SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C, ExternalForces) friend struct mc_rbdyn::Robot; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -95,6 +97,10 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node inline const tvm::VariablePtr & alphaD() const noexcept { return ddq_; } /** Access q second derivative (joint acceleration) */ inline tvm::VariablePtr & alphaD() noexcept { return ddq_; } + /** Access joint acceleration from external forces (const) */ + inline const Eigen::VectorXd & alphaDExternal() const noexcept { return ddq_ext_; } + /** Access joint acceleration from external forces */ + inline Eigen::VectorXd & alphaDExternal() noexcept { return ddq_ext_; } /** Access floating-base variable (const) */ inline const tvm::VariablePtr & qFloatingBase() const noexcept { return q_fb_; } @@ -133,6 +139,10 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node inline const tvm::VariablePtr & tau() const noexcept { return tau_; } /** Access tau variable */ inline tvm::VariablePtr & tau() { return tau_; } + /** Access tau external variable (const) */ + inline const Eigen::VectorXd & tauExternal() const noexcept { return tau_ext_; } + /** Access tau external variable */ + inline Eigen::VectorXd & tauExternal() { return tau_ext_; } /** Returns the CoM algorithm associated to this robot (const) */ inline const CoM & comAlgo() const noexcept { return *com_; } @@ -203,8 +213,12 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node tvm::VariablePtr dq_; /** Double derivative of q */ tvm::VariablePtr ddq_; + /** Joint acceleration from external forces */ + Eigen::VectorXd ddq_ext_; /** Tau variable */ tvm::VariablePtr tau_; + /** Tau external variable */ + Eigen::VectorXd tau_ext_; /** Normal accelerations of the bodies */ std::vector normalAccB_; /** Forward dynamics algorithm associated to this robot */ @@ -225,6 +239,7 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node void updateNormalAcceleration(); void updateH(); void updateC(); + void updateEF(); }; } // namespace mc_tvm diff --git a/src/mc_solver/DynamicsConstraint.cpp b/src/mc_solver/DynamicsConstraint.cpp index 6811620d8c..51a98953c2 100644 --- a/src/mc_solver/DynamicsConstraint.cpp +++ b/src/mc_solver/DynamicsConstraint.cpp @@ -64,23 +64,25 @@ static mc_rtc::void_ptr initialize_tasks(const mc_rbdyn::Robots & robots, } } -mc_rtc::void_ptr initialize_tvm(const mc_rbdyn::Robot & robot) +mc_rtc::void_ptr initialize_tvm(const mc_rbdyn::Robot & robot, bool compensateExternalForces) { - return mc_rtc::make_void_ptr(std::make_shared(robot)); + return mc_rtc::make_void_ptr( + std::make_shared(robot, compensateExternalForces)); } static mc_rtc::void_ptr initialize(QPSolver::Backend backend, const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExternalForces) { switch(backend) { case QPSolver::Backend::Tasks: return initialize_tasks(robots, robotIndex, timeStep, infTorque); case QPSolver::Backend::TVM: - return initialize_tvm(robots.robot(robotIndex)); + return initialize_tvm(robots.robot(robotIndex), compensateExternalForces); default: mc_rtc::log::error_and_throw("[DynamicsConstraint] Not implemented for solver backend: {}", backend); } @@ -89,9 +91,11 @@ static mc_rtc::void_ptr initialize(QPSolver::Backend backend, DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExternalForces) : KinematicsConstraint(robots, robotIndex, timeStep), - motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque)), robotIndex_(robotIndex) + motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExternalForces)), + robotIndex_(robotIndex) { } @@ -100,9 +104,11 @@ DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots, double timeStep, const std::array & damper, double velocityPercent, - bool infTorque) + bool infTorque, + bool compensateExternalForces) : KinematicsConstraint(robots, robotIndex, timeStep, damper, velocityPercent), - motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque)), robotIndex_(robotIndex) + motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExternalForces)), + robotIndex_(robotIndex) { } diff --git a/src/mc_tvm/DynamicFunction.cpp b/src/mc_tvm/DynamicFunction.cpp index 9023bd0922..9885f4280c 100644 --- a/src/mc_tvm/DynamicFunction.cpp +++ b/src/mc_tvm/DynamicFunction.cpp @@ -10,8 +10,9 @@ namespace mc_tvm { -DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot) -: tvm::function::abstract::LinearFunction(robot.mb().nrDof()), robot_(robot) +DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateExternalForces) +: tvm::function::abstract::LinearFunction(robot.mb().nrDof()), robot_(robot), + compensateExternalForces_(compensateExternalForces) { registerUpdates(Update::B, &DynamicFunction::updateb); registerUpdates(Update::Jacobian, &DynamicFunction::updateJacobian); @@ -103,6 +104,14 @@ sva::ForceVecd DynamicFunction::contactForce(const mc_rbdyn::RobotFrame & frame) void DynamicFunction::updateb() { b_ = robot_.tvmRobot().C(); + if(compensateExternalForces_) + { + b_ -= robot_.tvmRobot().tauExternal(); + if(robot_.hasDevice("virtualTorqueSensor")) + { + b_ += robot_.device("virtualTorqueSensor").torques(); + } + } } void DynamicFunction::updateJacobian() diff --git a/src/mc_tvm/Robot.cpp b/src/mc_tvm/Robot.cpp index 59d599aa89..ed0d0f4a9c 100644 --- a/src/mc_tvm/Robot.cpp +++ b/src/mc_tvm/Robot.cpp @@ -133,6 +133,8 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) dq_->setZero(); ddq_->setZero(); tau_->setZero(); + tau_ext_ = Eigen::VectorXd::Zero(robot.mb().nrJoints()); + ddq_ext_ = Eigen::VectorXd::Zero(robot.mb().nrJoints()); const auto & rjo = robot.refJointOrder(); refJointIndexToQIndex_.resize(rjo.size()); @@ -156,7 +158,7 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) /** Signal setup */ registerUpdates(Update::FK, &Robot::updateFK, Update::FV, &Robot::updateFV, Update::FA, &Robot::updateFA, Update::NormalAcceleration, &Robot::updateNormalAcceleration, Update::H, &Robot::updateH, Update::C, - &Robot::updateC); + &Robot::updateC, Update::ExternalForces, &Robot::updateEF); /** Output dependencies setup */ addOutputDependency(Output::FK, Update::FK); addOutputDependency(Output::FV, Update::FV); @@ -165,12 +167,15 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) addOutputDependency(Output::H, Update::H); addOutputDependency(Output::C, Update::C); addOutputDependency(Output::FV, Update::FV); + addOutputDependency(Output::ExternalForces, Update::ExternalForces); /** Internal dependencies setup */ addInternalDependency(Update::FV, Update::FK); addInternalDependency(Update::H, Update::FV); addInternalDependency(Update::C, Update::FV); addInternalDependency(Update::FA, Update::FV); addInternalDependency(Update::NormalAcceleration, Update::FV); + addInternalDependency(Update::ExternalForces, Update::H); + addInternalDependency(Update::ExternalForces, Update::FA); } void Robot::updateFK() @@ -235,4 +240,13 @@ tvm::VariablePtr Robot::qJoint(size_t jIdx) tvm::Space(offsetDof, offsetParam, offsetDof)); } +void Robot::updateEF() +{ + if(robot_.hasDevice("externalTorqueSensor")) + { + tau_ext_ = robot_.device("externalTorqueSensor").torques(); + } + ddq_ext_ = H().inverse() * tau_ext_; +} + } // namespace mc_tvm From 42c1f8a98af1640e27ed3f966b609b9382a8eb2c Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Thu, 30 May 2024 12:10:11 +0900 Subject: [PATCH 3/5] Add compliant version of EndEffectorTask and PostureTask for use with DynamicsConstraint including external forces to compensate for them, these tasks add gui entry and methods to uncompensate external forces and make the task compliant again. --- include/mc_tasks/CompliantEndEffectorTask.h | 73 ++++++++++++++++ include/mc_tasks/CompliantPostureTask.h | 40 +++++++++ src/CMakeLists.txt | 4 + src/mc_tasks/CompliantEndEffectorTask.cpp | 92 +++++++++++++++++++++ src/mc_tasks/CompliantPostureTask.cpp | 60 ++++++++++++++ 5 files changed, 269 insertions(+) create mode 100644 include/mc_tasks/CompliantEndEffectorTask.h create mode 100644 include/mc_tasks/CompliantPostureTask.h create mode 100644 src/mc_tasks/CompliantEndEffectorTask.cpp create mode 100644 src/mc_tasks/CompliantPostureTask.cpp diff --git a/include/mc_tasks/CompliantEndEffectorTask.h b/include/mc_tasks/CompliantEndEffectorTask.h new file mode 100644 index 0000000000..3de24c42f6 --- /dev/null +++ b/include/mc_tasks/CompliantEndEffectorTask.h @@ -0,0 +1,73 @@ +/* + * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include + +namespace mc_tasks +{ + +/*! \brief Controls an end-effector + * + * This task is a thin wrapper around the appropriate tasks in Tasks. + * The task objective is given in the world frame. For relative control + * see mc_tasks::RelativeCompliantEndEffectorTask + */ +struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask +{ +public: + /*! \brief Constructor + * + * \param bodyName Name of the body to control + * + * \param robots Robots controlled by this task + * + * \param robotIndex Index of the robot controlled by this task + * + * \param stiffness Task stiffness + * + * \param weight Task weight + * + */ + CompliantEndEffectorTask(const std::string & bodyName, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight); + + /** Change acceleration + * + * \p refAccel Should be of size 6 + */ + void refAccel(const Eigen::Vector6d & refAccel) noexcept; + + // Set the compliant behavior of the task + void makeCompliant(bool compliance); + + // Get compliance state of the task + bool isCompliant(void); + +protected: + void addToSolver(mc_solver::QPSolver & solver); + + void update(mc_solver::QPSolver & solver); + + void addToGUI(mc_rtc::gui::StateBuilder & gui); + + bool isCompliant_; + + mc_tvm::Robot * tvm_robot_; + + unsigned int rIdx_; + + std::string bodyName_; + + rbd::Jacobian * jac_; + + Eigen::Vector6d refAccel_; +}; + +} // namespace mc_tasks diff --git a/include/mc_tasks/CompliantPostureTask.h b/include/mc_tasks/CompliantPostureTask.h new file mode 100644 index 0000000000..b5246f21db --- /dev/null +++ b/include/mc_tasks/CompliantPostureTask.h @@ -0,0 +1,40 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include + +namespace mc_tasks +{ + +struct MC_TASKS_DLLAPI CompliantPostureTask : public PostureTask +{ +public: + CompliantPostureTask(const mc_solver::QPSolver & solver, unsigned int rIndex, double stiffness, double weight); + + /** Change reference acceleration + * + * \p refAccel Should be of size nrDof + */ + void refAccel(const Eigen::VectorXd & refAccel) noexcept; + + // Set task to be compliant or not + void makeCompliant(bool compliance); + // Get compliance state of the task + bool isCompliant(void); + +protected: + void update(mc_solver::QPSolver & solver); + + void addToGUI(mc_rtc::gui::StateBuilder & gui); + + bool isCompliant_; + + mc_tvm::Robot & tvm_robot_; + + Eigen::VectorXd refAccel_; +}; + +} // namespace mc_tasks diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 34bbc859a2..0f4797ca68 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -520,6 +520,8 @@ set(mc_tasks_SRC mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp mc_tasks/lipm_stabilizer/ZMPCC.cpp mc_tasks/lipm_stabilizer/Contact.cpp + mc_tasks/CompliantEndEffectorTask.cpp + mc_tasks/CompliantPostureTask.cpp ) set(mc_tasks_HDR @@ -556,6 +558,8 @@ set(mc_tasks_HDR ../include/mc_tasks/lipm_stabilizer/StabilizerTask.h ../include/mc_tasks/lipm_stabilizer/Contact.h ../include/mc_tasks/lipm_stabilizer/ZMPCC.h + ../include/mc_tasks/CompliantEndEffectorTask.h + ../include/mc_tasks/CompliantPostureTask.h ) add_library(mc_tasks SHARED ${mc_tasks_SRC} ${mc_tasks_HDR}) diff --git a/src/mc_tasks/CompliantEndEffectorTask.cpp b/src/mc_tasks/CompliantEndEffectorTask.cpp new file mode 100644 index 0000000000..c22ada2a81 --- /dev/null +++ b/src/mc_tasks/CompliantEndEffectorTask.cpp @@ -0,0 +1,92 @@ +/* + * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +#include +#include + +namespace mc_tasks +{ + +CompliantEndEffectorTask::CompliantEndEffectorTask(const std::string & bodyName, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight) +: EndEffectorTask(robots.robot(robotIndex).frame(bodyName), stiffness, weight), isCompliant_(false), + tvm_robot_(nullptr), rIdx_(robotIndex), bodyName_(bodyName), refAccel_(Eigen::Vector6d::Zero()) +{ + const mc_rbdyn::RobotFrame & frame = robots.robot(robotIndex).frame(bodyName); + + if(backend_ == Backend::Tasks) + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use TVM or TVMHierarchical backend", + backend_); + + type_ = "compliant_body6d"; + name_ = "compliant_body6d_" + frame.robot().name() + "_" + frame.name(); + EndEffectorTask::name(name_); +} + +void CompliantEndEffectorTask::refAccel(const Eigen::Vector6d & refAccel) noexcept +{ + refAccel_ = refAccel; +} + +void CompliantEndEffectorTask::makeCompliant(bool compliance) +{ + isCompliant_ = compliance; +} + +bool CompliantEndEffectorTask::isCompliant(void) +{ + return isCompliant_; +} + +void CompliantEndEffectorTask::addToSolver(mc_solver::QPSolver & solver) +{ + tvm_robot_ = &solver.robots().robot(rIdx_).tvmRobot(); + jac_ = new rbd::Jacobian(tvm_robot_->robot().mb(), bodyName_); + + EndEffectorTask::addToSolver(solver); +} + +void CompliantEndEffectorTask::update(mc_solver::QPSolver & solver) +{ + if(isCompliant_) + { + // mc_rtc::log::info("{} compliant mode", bodyName_); + auto J = jac_->jacobian(tvm_robot_->robot().mb(), tvm_robot_->robot().mbc()); + Eigen::Vector6d disturbance = J * tvm_robot_->alphaDExternal(); + Eigen::Vector6d disturbedAccel = refAccel_ + disturbance; + // mc_rtc::log::info("Jacobian: \n {}", J); + // mc_rtc::log::info("Cartesian disturbance acceleration: {}", disturbedAccel.transpose()); + // mc_rtc::log::info(" - Position disturbance acceleration: {}", disturbedAccel.tail(3).transpose()); + // mc_rtc::log::info(" - Orientation disturbance acceleration: {}", disturbedAccel.head(3).transpose()); + EndEffectorTask::positionTask->refAccel(disturbedAccel.tail(3)); + EndEffectorTask::orientationTask->refAccel(disturbedAccel.head(3)); + // auto cst = Eigen::Vector3d::Constant(1e60); + // EndEffectorTask::positionTask->refAccel(cst); + // EndEffectorTask::orientationTask->refAccel(cst); + } + else + { + // mc_rtc::log::info("{} non compliant mode", bodyName_); + EndEffectorTask::positionTask->refAccel(refAccel_.tail(3)); + EndEffectorTask::orientationTask->refAccel(refAccel_.head(3)); + } + EndEffectorTask::update(solver); +} + +void CompliantEndEffectorTask::addToGUI(mc_rtc::gui::StateBuilder & gui) +{ + gui.addElement({"Tasks", name_, "Compliance"}, mc_rtc::gui::Checkbox( + "Compliance is active", [this]() { return isCompliant_; }, + [this]() { isCompliant_ = !isCompliant_; })); + + EndEffectorTask::addToGUI(gui); +} + +} // namespace mc_tasks diff --git a/src/mc_tasks/CompliantPostureTask.cpp b/src/mc_tasks/CompliantPostureTask.cpp new file mode 100644 index 0000000000..2d07a9be95 --- /dev/null +++ b/src/mc_tasks/CompliantPostureTask.cpp @@ -0,0 +1,60 @@ +#include + +#include +#include + +namespace mc_tasks +{ + +CompliantPostureTask::CompliantPostureTask(const mc_solver::QPSolver & solver, + unsigned int rIndex, + double stiffness, + double weight) +: PostureTask(solver, rIndex, stiffness, weight), isCompliant_(false), + tvm_robot_(solver.robots().robot(rIndex).tvmRobot()), + refAccel_(Eigen::VectorXd::Zero(solver.robots().robot(rIndex).mb().nrDof())) +{ + if(backend_ == Backend::Tasks) + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use TVM or TVMHierarchical backend", + backend_); + name_ = "compliant_posture"; + type_ = "compliant_posture"; +} + +void CompliantPostureTask::refAccel(const Eigen::VectorXd & refAccel) noexcept +{ + refAccel_ = refAccel; +} + +void CompliantPostureTask::update(mc_solver::QPSolver & solver) +{ + if(isCompliant_) + { + Eigen::VectorXd disturbance = tvm_robot_.alphaDExternal(); + Eigen::VectorXd disturbedAccel = refAccel_ + disturbance; + PostureTask::refAccel(disturbedAccel); + } + else { PostureTask::refAccel(refAccel_); } + PostureTask::update(solver); +} + +void CompliantPostureTask::makeCompliant(bool compliance) +{ + isCompliant_ = compliance; +} + +bool CompliantPostureTask::isCompliant(void) +{ + return isCompliant_; +} + +void CompliantPostureTask::addToGUI(mc_rtc::gui::StateBuilder & gui) +{ + gui.addElement({"Tasks", name_, "Compliance"}, mc_rtc::gui::Checkbox( + "Compliance is active", [this]() { return isCompliant_; }, + [this]() { isCompliant_ = !isCompliant_; })); + PostureTask::addToGUI(gui); +} + +} // namespace mc_tasks From ac68e3f71430b9ba69337b608bb2a40d2cfc57c9 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Tue, 18 Jun 2024 13:30:40 +0900 Subject: [PATCH 4/5] Fix issue with TVM Robots' external force update not registered in TVM DynamicFunction --- src/mc_tvm/DynamicFunction.cpp | 2 ++ src/mc_tvm/Robot.cpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/mc_tvm/DynamicFunction.cpp b/src/mc_tvm/DynamicFunction.cpp index 9885f4280c..07698aae0e 100644 --- a/src/mc_tvm/DynamicFunction.cpp +++ b/src/mc_tvm/DynamicFunction.cpp @@ -21,6 +21,7 @@ DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateE auto & tvm_robot = robot.tvmRobot(); addInputDependency(Update::Jacobian, tvm_robot, Robot::Output::H); addInputDependency(Update::B, tvm_robot, Robot::Output::C); + addInputDependency(Update::B, tvm_robot, Robot::Output::ExternalForces); addVariable(tvm::dot(tvm_robot.q(), 2), true); addVariable(tvm_robot.tau(), true); jacobian_[tvm_robot.tau().get()] = -Eigen::MatrixXd::Identity(robot_.mb().nrDof(), robot_.mb().nrDof()); @@ -107,6 +108,7 @@ void DynamicFunction::updateb() if(compensateExternalForces_) { b_ -= robot_.tvmRobot().tauExternal(); + mc_rtc::log::info("Compensating for : {}", robot_.tvmRobot().tauExternal()); if(robot_.hasDevice("virtualTorqueSensor")) { b_ += robot_.device("virtualTorqueSensor").torques(); diff --git a/src/mc_tvm/Robot.cpp b/src/mc_tvm/Robot.cpp index ed0d0f4a9c..e2911d7145 100644 --- a/src/mc_tvm/Robot.cpp +++ b/src/mc_tvm/Robot.cpp @@ -133,8 +133,8 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) dq_->setZero(); ddq_->setZero(); tau_->setZero(); - tau_ext_ = Eigen::VectorXd::Zero(robot.mb().nrJoints()); - ddq_ext_ = Eigen::VectorXd::Zero(robot.mb().nrJoints()); + tau_ext_ = Eigen::VectorXd::Zero(robot.mb().nrDof()); + ddq_ext_ = Eigen::VectorXd::Zero(robot.mb().nrDof()); const auto & rjo = robot.refJointOrder(); refJointIndexToQIndex_.resize(rjo.size()); From 7195a1ba29e54c347ecf2e183e312bcf9f9c98ed Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Fri, 9 Aug 2024 15:48:13 +0900 Subject: [PATCH 5/5] Change boolean compliance in CompliantEndEffectorTask to 6D vector with gamma compliance, fix update in Dynamic function --- include/mc_tasks/CompliantEndEffectorTask.h | 5 +- src/mc_tasks/CompliantEndEffectorTask.cpp | 61 +++++++++++---------- src/mc_tasks/CompliantPostureTask.cpp | 1 + src/mc_tvm/DynamicFunction.cpp | 7 ++- 4 files changed, 42 insertions(+), 32 deletions(-) diff --git a/include/mc_tasks/CompliantEndEffectorTask.h b/include/mc_tasks/CompliantEndEffectorTask.h index 3de24c42f6..f827a98f69 100644 --- a/include/mc_tasks/CompliantEndEffectorTask.h +++ b/include/mc_tasks/CompliantEndEffectorTask.h @@ -6,6 +6,7 @@ #include #include +#include namespace mc_tasks { @@ -46,9 +47,11 @@ struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask // Set the compliant behavior of the task void makeCompliant(bool compliance); + void setComplianceVector(Eigen::Vector6d gamma); // Get compliance state of the task bool isCompliant(void); + Eigen::Vector6d getComplianceVector(void); protected: void addToSolver(mc_solver::QPSolver & solver); @@ -57,7 +60,7 @@ struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask void addToGUI(mc_rtc::gui::StateBuilder & gui); - bool isCompliant_; + Eigen::MatrixXd compliant_matrix_; mc_tvm::Robot * tvm_robot_; diff --git a/src/mc_tasks/CompliantEndEffectorTask.cpp b/src/mc_tasks/CompliantEndEffectorTask.cpp index c22ada2a81..73d6425c46 100644 --- a/src/mc_tasks/CompliantEndEffectorTask.cpp +++ b/src/mc_tasks/CompliantEndEffectorTask.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include "mc_rtc/gui/ArrayInput.h" namespace mc_tasks { @@ -15,8 +17,9 @@ CompliantEndEffectorTask::CompliantEndEffectorTask(const std::string & bodyName, unsigned int robotIndex, double stiffness, double weight) -: EndEffectorTask(robots.robot(robotIndex).frame(bodyName), stiffness, weight), isCompliant_(false), - tvm_robot_(nullptr), rIdx_(robotIndex), bodyName_(bodyName), refAccel_(Eigen::Vector6d::Zero()) +: EndEffectorTask(robots.robot(robotIndex).frame(bodyName), stiffness, weight), + compliant_matrix_(Eigen::Matrix6d::Zero()), tvm_robot_(nullptr), rIdx_(robotIndex), bodyName_(bodyName), + refAccel_(Eigen::Vector6d::Zero()) { const mc_rbdyn::RobotFrame & frame = robots.robot(robotIndex).frame(bodyName); @@ -37,12 +40,23 @@ void CompliantEndEffectorTask::refAccel(const Eigen::Vector6d & refAccel) noexce void CompliantEndEffectorTask::makeCompliant(bool compliance) { - isCompliant_ = compliance; + if(compliance) { compliant_matrix_.diagonal().setOnes(); } + else { compliant_matrix_.diagonal().setZero(); } +} + +void CompliantEndEffectorTask::setComplianceVector(Eigen::Vector6d gamma) +{ + compliant_matrix_.diagonal() = gamma; } bool CompliantEndEffectorTask::isCompliant(void) { - return isCompliant_; + return compliant_matrix_.diagonal().norm() > 0; +} + +Eigen::Vector6d CompliantEndEffectorTask::getComplianceVector(void) +{ + return compliant_matrix_.diagonal(); } void CompliantEndEffectorTask::addToSolver(mc_solver::QPSolver & solver) @@ -55,36 +69,25 @@ void CompliantEndEffectorTask::addToSolver(mc_solver::QPSolver & solver) void CompliantEndEffectorTask::update(mc_solver::QPSolver & solver) { - if(isCompliant_) - { - // mc_rtc::log::info("{} compliant mode", bodyName_); - auto J = jac_->jacobian(tvm_robot_->robot().mb(), tvm_robot_->robot().mbc()); - Eigen::Vector6d disturbance = J * tvm_robot_->alphaDExternal(); - Eigen::Vector6d disturbedAccel = refAccel_ + disturbance; - // mc_rtc::log::info("Jacobian: \n {}", J); - // mc_rtc::log::info("Cartesian disturbance acceleration: {}", disturbedAccel.transpose()); - // mc_rtc::log::info(" - Position disturbance acceleration: {}", disturbedAccel.tail(3).transpose()); - // mc_rtc::log::info(" - Orientation disturbance acceleration: {}", disturbedAccel.head(3).transpose()); - EndEffectorTask::positionTask->refAccel(disturbedAccel.tail(3)); - EndEffectorTask::orientationTask->refAccel(disturbedAccel.head(3)); - // auto cst = Eigen::Vector3d::Constant(1e60); - // EndEffectorTask::positionTask->refAccel(cst); - // EndEffectorTask::orientationTask->refAccel(cst); - } - else - { - // mc_rtc::log::info("{} non compliant mode", bodyName_); - EndEffectorTask::positionTask->refAccel(refAccel_.tail(3)); - EndEffectorTask::orientationTask->refAccel(refAccel_.head(3)); - } - EndEffectorTask::update(solver); + auto J = jac_->jacobian(tvm_robot_->robot().mb(), tvm_robot_->robot().mbc()); + Eigen::Vector6d disturbance = J * tvm_robot_->alphaDExternal(); + Eigen::Vector6d disturbedAccel = refAccel_ + compliant_matrix_ * disturbance; + + EndEffectorTask::positionTask->refAccel(disturbedAccel.tail(3)); + EndEffectorTask::orientationTask->refAccel(disturbedAccel.head(3)); + + // EndEffectorTask::update(solver); } void CompliantEndEffectorTask::addToGUI(mc_rtc::gui::StateBuilder & gui) { gui.addElement({"Tasks", name_, "Compliance"}, mc_rtc::gui::Checkbox( - "Compliance is active", [this]() { return isCompliant_; }, - [this]() { isCompliant_ = !isCompliant_; })); + "Compliance is active", [this]() { return isCompliant(); }, + [this]() { makeCompliant(!isCompliant()); })); + gui.addElement({"Tasks", name_, "Compliance"}, + mc_rtc::gui::ArrayInput( + "Compliance parameters", {"rx", "ry", "rz", "x", "y", "z"}, [this]() + { return getComplianceVector(); }, [this](Eigen::Vector6d v) { setComplianceVector(v); })); EndEffectorTask::addToGUI(gui); } diff --git a/src/mc_tasks/CompliantPostureTask.cpp b/src/mc_tasks/CompliantPostureTask.cpp index 2d07a9be95..707db81fd4 100644 --- a/src/mc_tasks/CompliantPostureTask.cpp +++ b/src/mc_tasks/CompliantPostureTask.cpp @@ -32,6 +32,7 @@ void CompliantPostureTask::update(mc_solver::QPSolver & solver) if(isCompliant_) { Eigen::VectorXd disturbance = tvm_robot_.alphaDExternal(); + // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); Eigen::VectorXd disturbedAccel = refAccel_ + disturbance; PostureTask::refAccel(disturbedAccel); } diff --git a/src/mc_tvm/DynamicFunction.cpp b/src/mc_tvm/DynamicFunction.cpp index 07698aae0e..760030d6bc 100644 --- a/src/mc_tvm/DynamicFunction.cpp +++ b/src/mc_tvm/DynamicFunction.cpp @@ -21,7 +21,10 @@ DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateE auto & tvm_robot = robot.tvmRobot(); addInputDependency(Update::Jacobian, tvm_robot, Robot::Output::H); addInputDependency(Update::B, tvm_robot, Robot::Output::C); - addInputDependency(Update::B, tvm_robot, Robot::Output::ExternalForces); + if(compensateExternalForces_) + { + addInputDependency(Update::B, tvm_robot, Robot::Output::ExternalForces); + } addVariable(tvm::dot(tvm_robot.q(), 2), true); addVariable(tvm_robot.tau(), true); jacobian_[tvm_robot.tau().get()] = -Eigen::MatrixXd::Identity(robot_.mb().nrDof(), robot_.mb().nrDof()); @@ -108,7 +111,7 @@ void DynamicFunction::updateb() if(compensateExternalForces_) { b_ -= robot_.tvmRobot().tauExternal(); - mc_rtc::log::info("Compensating for : {}", robot_.tvmRobot().tauExternal()); + // mc_rtc::log::info("Compensating for : {}", robot_.tvmRobot().tauExternal().transpose()); if(robot_.hasDevice("virtualTorqueSensor")) { b_ += robot_.device("virtualTorqueSensor").torques();