diff --git a/include/mc_rbdyn/Robot.h b/include/mc_rbdyn/Robot.h index f3dd837fe1..b1ae9cec23 100644 --- a/include/mc_rbdyn/Robot.h +++ b/include/mc_rbdyn/Robot.h @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -1114,6 +1115,42 @@ struct MC_RBDYN_DLLAPI Robot */ mc_tvm::Convex & tvmConvex(const std::string & name) const; + /** @name External Torques + * + * These functions are used to get or set: + * - The estimation of external torques being applied on the robot and its 'equivalent' acceleration + * - The additional feedforward compensation torque and its 'equivalent' acceleration + * + * @{ + */ + + /** Set the external torques */ + void setExternalTorques(const Eigen::VectorXd & torques); + + /** Get the external torques */ + const Eigen::VectorXd & externalTorques(void) const; + + /** Set the external torques equivalent accelerations */ + void setExternalTorquesAcc(const Eigen::VectorXd & accelerations); + + /** Get the external torques equivalent accelerations */ + const Eigen::VectorXd & externalTorquesAcc(void) const; + + /** Set the compensation torques */ + void setCompensationTorques(const Eigen::VectorXd & torques); + + /** Get the compensation torques */ + const std::optional & compensationTorques(void) const; + + /** Set the compensation torques equivalent accelerations */ + void setCompensationTorquesAcc(const Eigen::VectorXd & accelerations); + + /** Get the compensation torques equivalent accelerations */ + const std::optional & compensationTorquesAcc(void) const; + + /** @} */ + /* End of External Forces group */ + private: Robots * robots_; unsigned int robots_idx_; @@ -1149,6 +1186,14 @@ struct MC_RBDYN_DLLAPI Robot std::unordered_map frames_; /** Mass of this robot */ double mass_ = 0.0; + /** Non modeled external forces acting on the robot **/ + Eigen::VectorXd externalTorques_; + /** Joint accelerations from external forces acting on the robot **/ + Eigen::VectorXd externalTorquesEquivalentAcc_; + /** External forces to be compensated in the commanded torque **/ + std::optional externalTorqueCompensation_ = std::nullopt; + /** Joint accelerations from the compensation in the commanded torque **/ + std::optional compensationEquivalentAcc_ = std::nullopt; protected: struct NewRobotToken diff --git a/include/mc_rtc/Configuration.h b/include/mc_rtc/Configuration.h index 6904f6b5de..05ea70e2db 100644 --- a/include/mc_rtc/Configuration.h +++ b/include/mc_rtc/Configuration.h @@ -1807,11 +1807,11 @@ template<> struct formatter : public formatter { template - #if FMT_VERSION <= 9 * 10000 - auto format(const mc_rtc::Configuration & c, FormatContext & ctx) - #else - auto format(const mc_rtc::Configuration & c, FormatContext & ctx) const -> decltype(ctx.out()) - #endif +#if FMT_VERSION <= 9 * 10000 + auto format(const mc_rtc::Configuration & c, FormatContext & ctx) +#else + auto format(const mc_rtc::Configuration & c, FormatContext & ctx) const -> decltype(ctx.out()) +#endif { return formatter::format(static_cast(c), ctx); } diff --git a/include/mc_solver/DynamicsConstraint.h b/include/mc_solver/DynamicsConstraint.h index 55c194b28f..4872df1054 100644 --- a/include/mc_solver/DynamicsConstraint.h +++ b/include/mc_solver/DynamicsConstraint.h @@ -28,8 +28,15 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint * \param robotIndex The index of the robot affected by this constraint * \param timeStep Solver timestep * \param infTorque If true, ignore the torque limits set in the robot model + * \param compensateExtTorques If true, compensates external disturbances using a feedworward torque signal. The + * constraint will search for the compensation value in robot by calling `compensationTorques()` method, if not an + * estimation of external torques acting on the robot will be used by calling `externalTorques()` method. */ - 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 compensateExtTorques = false); /** Constructor * Builds a damped joint limits constraint and a motion constr depending on @@ -42,13 +49,25 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint * offset} * \param velocityPercent Maximum joint velocity percentage, 0.5 is advised * \param infTorque If true, ignore the torque limits set in the robot model + * \param compensateExtTorques If true, compensates external disturbances using a feedworward torque signal. The + * constraint will search for the compensation value in robot by calling `compensationTorques()` method, if not an + * estimation of external torques acting on the robot will be used by calling `externalTorques()` method. */ DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, const std::array & damper, double velocityPercent = 1.0, - bool infTorque = false); + bool infTorque = false, + bool compensateExtTorques = false); + + /** \brief Update the constraint + * + * This is called at every iteration of the controller once the constraint has been added to a solver + * + * \param solver Solver in which the constraint has been inserted + */ + void update(QPSolver & solver) override; /** Returns the tasks::qp::MotionConstr * diff --git a/include/mc_solver/QPSolver.h b/include/mc_solver/QPSolver.h index 68709b927b..9fa96de448 100644 --- a/include/mc_solver/QPSolver.h +++ b/include/mc_solver/QPSolver.h @@ -344,11 +344,11 @@ template<> struct formatter : public formatter { template - #if FMT_VERSION <= 9 * 10000 - auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) - #else - auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out()) - #endif +#if FMT_VERSION <= 9 * 10000 + auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) +#else + auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out()) +#endif { using Backend = mc_solver::QPSolver::Backend; switch(backend) diff --git a/include/mc_tvm/DynamicFunction.h b/include/mc_tvm/DynamicFunction.h index 2e2156a0ca..d13682f838 100644 --- a/include/mc_tvm/DynamicFunction.h +++ b/include/mc_tvm/DynamicFunction.h @@ -36,7 +36,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 +73,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..4f4893e448 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 @@ -96,6 +98,11 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node /** 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_; } /** Access free-flyer variable */ @@ -134,6 +141,21 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node /** 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_; } + + /** Access tau compensation (const) */ + inline const std::optional & tauCompensation() const noexcept { return tau_comp_; } + /** Access tau compensation */ + inline std::optional & tauCompensation() noexcept { return tau_comp_; } + + /** Access joint acceleration from compensation torques (const) */ + inline const std::optional & alphaDCompensation() const noexcept { return ddq_comp_; } + /** Access joint acceleration from compensation torques */ + inline std::optional & alphaDCompensation() noexcept { return ddq_comp_; } + /** Returns the CoM algorithm associated to this robot (const) */ inline const CoM & comAlgo() const noexcept { return *com_; } @@ -203,8 +225,16 @@ 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_; + /** Joint acceleration from compensation torques */ + std::optional ddq_comp_; /** Tau variable */ tvm::VariablePtr tau_; + /** Tau external variable */ + Eigen::VectorXd tau_ext_; + /** Tau compensation variable */ + std::optional tau_comp_; /** Normal accelerations of the bodies */ std::vector normalAccB_; /** Forward dynamics algorithm associated to this robot */ @@ -225,6 +255,7 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node void updateNormalAcceleration(); void updateH(); void updateC(); + void updateExternalForces(); }; } // namespace mc_tvm diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp index 04ae4cdf8c..da260e79f0 100644 --- a/src/mc_rbdyn/Robot.cpp +++ b/src/mc_rbdyn/Robot.cpp @@ -18,10 +18,13 @@ #include #include +#include #include #include #include +#include +#include #include #include @@ -510,6 +513,9 @@ Robot::Robot(NewRobotToken, flexibility_ = module_.flexibility(); zmp_ = Eigen::Vector3d::Zero(); + + externalTorques_ = Eigen::VectorXd::Zero(mb().nrDof()); + externalTorquesEquivalentAcc_ = Eigen::VectorXd::Zero(mb().nrDof()); } Robot::~Robot() @@ -1567,4 +1573,56 @@ mc_tvm::Convex & Robot::tvmConvex(const std::string & name) const return *it->second; } +void Robot::setExternalTorques(const Eigen::VectorXd & torques) +{ + externalTorques_.noalias() = torques; +} + +const Eigen::VectorXd & Robot::externalTorques(void) const +{ + return externalTorques_; +} + +void Robot::setExternalTorquesAcc(const Eigen::VectorXd & accelerations) +{ + externalTorquesEquivalentAcc_.noalias() = accelerations; +} + +const Eigen::VectorXd & Robot::externalTorquesAcc(void) const +{ + return externalTorquesEquivalentAcc_; +} + +void Robot::setCompensationTorques(const Eigen::VectorXd & torques) +{ + if(!externalTorqueCompensation_) { externalTorqueCompensation_.emplace(torques.size()); } + else if(externalTorqueCompensation_->size() == torques.size()) + { + externalTorqueCompensation_->resize(torques.size()); + } + + externalTorqueCompensation_->noalias() = torques; +} + +const std::optional & Robot::compensationTorques(void) const +{ + return externalTorqueCompensation_; +} + +void Robot::setCompensationTorquesAcc(const Eigen::VectorXd & accelerations) +{ + if(!externalTorqueCompensation_) { compensationEquivalentAcc_.emplace(accelerations.size()); } + else if(externalTorqueCompensation_->size() == accelerations.size()) + { + + compensationEquivalentAcc_->resize(accelerations.size()); + } + compensationEquivalentAcc_->noalias() = accelerations; +} + +const std::optional & Robot::compensationTorquesAcc(void) const +{ + return compensationEquivalentAcc_; +} + } // namespace mc_rbdyn diff --git a/src/mc_rbdyn/configuration_io.cpp b/src/mc_rbdyn/configuration_io.cpp index 1a33959791..bead2b37ab 100644 --- a/src/mc_rbdyn/configuration_io.cpp +++ b/src/mc_rbdyn/configuration_io.cpp @@ -29,11 +29,11 @@ template<> struct formatter : public formatter { template - #if FMT_VERSION <= 9 * 10000 - auto format(const rbd::Joint::Type & t, FormatContext & ctx) - #else - auto format(const rbd::Joint::Type & t, FormatContext & ctx) const -> decltype(ctx.out()) - #endif +#if FMT_VERSION <= 9 * 10000 + auto format(const rbd::Joint::Type & t, FormatContext & ctx) +#else + auto format(const rbd::Joint::Type & t, FormatContext & ctx) const -> decltype(ctx.out()) +#endif { switch(t) { diff --git a/src/mc_solver/DynamicsConstraint.cpp b/src/mc_solver/DynamicsConstraint.cpp index 09b82a5845..7aa697c7ed 100644 --- a/src/mc_solver/DynamicsConstraint.cpp +++ b/src/mc_solver/DynamicsConstraint.cpp @@ -19,7 +19,8 @@ namespace mc_solver static mc_rtc::void_ptr initialize_tasks(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExtTorques) { const auto & robot = robots.robot(robotIndex); std::vector> tl = robot.tl(); @@ -54,33 +55,69 @@ static mc_rtc::void_ptr initialize_tasks(const mc_rbdyn::Robots & robots, { sjList.push_back(tasks::qp::SpringJoint(flex.jointName, flex.K, flex.C, flex.O)); } - return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, - tDBound, timeStep, sjList); + if(compensateExtTorques) + { + if(robot.compensationTorques()) + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, sjList, + robot.compensationTorques().value()); + } + else + { + + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, sjList, robot.externalTorques()); + } + } + else + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, sjList); + } } else { - return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, tDBound, - timeStep); + if(compensateExtTorques) + { + if(robot.compensationTorques()) + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, robot.compensationTorques().value()); + } + else + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, robot.externalTorques()); + } + } + else + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, tDBound, + timeStep); + } } } -mc_rtc::void_ptr initialize_tvm(const mc_rbdyn::Robot & robot) +mc_rtc::void_ptr initialize_tvm(const mc_rbdyn::Robot & robot, bool compensateExtTorques) { - return mc_rtc::make_void_ptr(std::make_shared(robot)); + return mc_rtc::make_void_ptr( + std::make_shared(robot, compensateExtTorques)); } static mc_rtc::void_ptr initialize(QPSolver::Backend backend, const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExtTorques) { switch(backend) { case QPSolver::Backend::Tasks: - return initialize_tasks(robots, robotIndex, timeStep, infTorque); + return initialize_tasks(robots, robotIndex, timeStep, infTorque, compensateExtTorques); case QPSolver::Backend::TVM: - return initialize_tvm(robots.robot(robotIndex)); + return initialize_tvm(robots.robot(robotIndex), compensateExtTorques); default: mc_rtc::log::error_and_throw("[DynamicsConstraint] Not implemented for solver backend: {}", backend); } @@ -89,9 +126,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 compensateExtTorques) : KinematicsConstraint(robots, robotIndex, timeStep), - motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque)), robotIndex_(robotIndex) + motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExtTorques)), + robotIndex_(robotIndex) { } @@ -100,12 +139,31 @@ DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots, double timeStep, const std::array & damper, double velocityPercent, - bool infTorque) + bool infTorque, + bool compensateExtTorques) : KinematicsConstraint(robots, robotIndex, timeStep, damper, velocityPercent), - motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque)), robotIndex_(robotIndex) + motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExtTorques)), + robotIndex_(robotIndex) { } +void DynamicsConstraint::update(QPSolver & solver) +{ + if(backend_ == QPSolver::Backend::Tasks) + { + auto & robot = solver.robot(robotIndex_); + if(robot.compensationTorques()) + { + static_cast(motion_constr_.get()) + ->setExternalTorques(robot.compensationTorques().value()); + } + else + { + static_cast(motion_constr_.get())->setExternalTorques(robot.externalTorques()); + } + } +} + void DynamicsConstraint::addToSolverImpl(QPSolver & solver) { KinematicsConstraint::addToSolverImpl(solver); diff --git a/src/mc_solver/TVMQPSolver.cpp b/src/mc_solver/TVMQPSolver.cpp index 7d0cf4236f..ac133c9859 100644 --- a/src/mc_solver/TVMQPSolver.cpp +++ b/src/mc_solver/TVMQPSolver.cpp @@ -221,6 +221,15 @@ bool TVMQPSolver::runClosedLoop(bool integrateControlState) robot.forwardKinematics(); robot.forwardVelocity(); robot.forwardAcceleration(); + + // Update robot with realRobot's external/compenstation torques informations + robot.setExternalTorques(realRobot.externalTorques()); + robot.setExternalTorquesAcc(realRobot.externalTorquesAcc()); + if(realRobot.compensationTorques()) + { + robot.setCompensationTorques(realRobot.compensationTorques().value()); + robot.setCompensationTorquesAcc(realRobot.compensationTorquesAcc().value()); + } } // Solve QP and integrate diff --git a/src/mc_solver/TasksQPSolver.cpp b/src/mc_solver/TasksQPSolver.cpp index 1360529f5c..6e2a711f16 100644 --- a/src/mc_solver/TasksQPSolver.cpp +++ b/src/mc_solver/TasksQPSolver.cpp @@ -155,6 +155,21 @@ const sva::ForceVecd TasksQPSolver::desiredContactForce(const mc_rbdyn::Contact bool TasksQPSolver::run_impl(FeedbackType fType) { bool success = false; + + for(size_t i = 0; i < robots().size(); ++i) + { + auto & realRobot = realRobots().robot(i); + if(realRobot.externalTorques().size() == 0) continue; + auto fd = rbd::ForwardDynamics(realRobot.mb()); + fd.computeH(realRobot.mb(), realRobot.mbc()); + auto Hinv = fd.H().ldlt(); + realRobot.setExternalTorquesAcc(Hinv.solve(realRobot.externalTorques())); + if(realRobot.compensationTorques() && !realRobot.compensationTorquesAcc()) + { + realRobot.setCompensationTorquesAcc(Hinv.solve(realRobot.compensationTorques().value())); + } + } + switch(fType) { case FeedbackType::None: @@ -315,6 +330,14 @@ bool TasksQPSolver::runClosedLoop(bool integrateControlState) robot.forwardKinematics(); robot.forwardVelocity(); robot.forwardAcceleration(); + // Update robot with realRobot's external/compenstation torques informations + robot.setExternalTorques(realRobot.externalTorques()); + robot.setExternalTorquesAcc(realRobot.externalTorquesAcc()); + if(realRobot.compensationTorques()) + { + robot.setCompensationTorques(realRobot.compensationTorques().value()); + robot.setCompensationTorquesAcc(realRobot.compensationTorquesAcc().value()); + } } // Update tasks and constraints from estimated robots diff --git a/src/mc_tvm/DynamicFunction.cpp b/src/mc_tvm/DynamicFunction.cpp index 9023bd0922..b1fe6b1ece 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); @@ -20,6 +21,10 @@ DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot) auto & tvm_robot = robot.tvmRobot(); addInputDependency(Update::Jacobian, tvm_robot, Robot::Output::H); addInputDependency(Update::B, tvm_robot, Robot::Output::C); + 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()); @@ -103,6 +108,14 @@ sva::ForceVecd DynamicFunction::contactForce(const mc_rbdyn::RobotFrame & frame) void DynamicFunction::updateb() { b_ = robot_.tvmRobot().C(); + if(compensateExternalForces_) + { + if(robot_.tvmRobot().tauCompensation()) { b_ -= robot_.tvmRobot().tauCompensation().value(); } + else + { + b_ -= robot_.tvmRobot().tauExternal(); + } + } } void DynamicFunction::updateJacobian() diff --git a/src/mc_tvm/Robot.cpp b/src/mc_tvm/Robot.cpp index 7b4d4f9aa5..01f1afe95b 100644 --- a/src/mc_tvm/Robot.cpp +++ b/src/mc_tvm/Robot.cpp @@ -136,6 +136,8 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) dq_->setZero(); ddq_->setZero(); tau_->setZero(); + 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()); @@ -159,7 +161,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::updateExternalForces); /** Output dependencies setup */ addOutputDependency(Output::FK, Update::FK); addOutputDependency(Output::FV, Update::FV); @@ -168,12 +170,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() @@ -238,4 +243,17 @@ tvm::VariablePtr Robot::qJoint(size_t jIdx) tvm::Space(offsetDof, offsetParam, offsetDof)); } +void Robot::updateExternalForces() +{ + tau_ext_ = robot_.externalTorques(); + tau_comp_ = robot_.compensationTorques(); + auto H_inv = H().ldlt(); + ddq_ext_ = H_inv.solve(tau_ext_); + if(tau_comp_) + { + if(ddq_comp_->size() != tau_comp_->size()) { ddq_comp_->resize(tau_comp_->size()); } + ddq_comp_ = H_inv.solve(tau_comp_.value()); + } +} + } // namespace mc_tvm