diff --git a/src/JoypadModule/src/Module.cpp b/src/JoypadModule/src/Module.cpp index aa52d361..83978e16 100644 --- a/src/JoypadModule/src/Module.cpp +++ b/src/JoypadModule/src/Module.cpp @@ -131,7 +131,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf) yError() << "[configure] Unable to get a string from searchable"; return false; } - yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName); + yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem"); if (!YarpUtilities::getStringFromSearchable(rf, "robotGoalOutputPort_name", portName)) { @@ -146,7 +146,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf) yError() << "[configure] Unable to get a string from searchable"; return false; } - yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName); + yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem"); return true; } @@ -260,21 +260,21 @@ bool JoypadModule::updateModule() { yInfo() << "[updateModule] Connecting both ports"; if (!yarp::os::Network::isConnected(m_rpcClientPortName, m_rpcServerPortName)) - yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName); + yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem"); if (!yarp::os::Network::isConnected(m_robotGoalOutputPortName, m_robotGoalInputPortName)) - yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName); + yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem"); } else if (m_connectPortsSeparately && connectGoal > 0) { yInfo() << "[updateModule] Connecting goal port"; if (!yarp::os::Network::isConnected(m_robotGoalOutputPortName, m_robotGoalInputPortName)) - yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName); + yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem"); } else if (m_connectPortsSeparately && connectRpc > 0) { yInfo() << "[updateModule] Connecting RPC port"; if (!yarp::os::Network::isConnected(m_rpcClientPortName, m_rpcServerPortName)) - yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName); + yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem"); } // disconnect the ports else if (disconnect > 0) diff --git a/src/RobotInterface/CMakeLists.txt b/src/RobotInterface/CMakeLists.txt index c2008b6d..c201d445 100644 --- a/src/RobotInterface/CMakeLists.txt +++ b/src/RobotInterface/CMakeLists.txt @@ -4,7 +4,7 @@ add_walking_controllers_library( NAME RobotInterface - SOURCES src/Helper.cpp src/PIDHandler.cpp - PUBLIC_HEADERS include/WalkingControllers/RobotInterface/Helper.h include/WalkingControllers/RobotInterface/PIDHandler.h + SOURCES src/Helper.cpp src/PIDHandler.cpp src/MotorTemperatureChecker.cpp + PUBLIC_HEADERS include/WalkingControllers/RobotInterface/Helper.h include/WalkingControllers/RobotInterface/PIDHandler.h include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities ctrlLib PRIVATE_LINK_LIBRARIES Eigen3::Eigen) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index e2637b6f..0162023b 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -16,6 +16,8 @@ #include #include #include +#include + #include #include @@ -48,6 +50,7 @@ namespace WalkingControllers 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. */ + yarp::dev::IMotor *m_motorInterface{nullptr}; /**< Motor interface. */ std::unique_ptr m_PIDHandler; /**< Pointer to the PID handler object. */ @@ -66,6 +69,13 @@ namespace WalkingControllers iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */ iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */ iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */ + + iDynTree::VectorDynSize m_motorTemperatures; /**< Motor temperature [Celsius]. */ + mutable std::mutex m_motorTemperatureMutex; /**< Mutex for the motor temperature. */ + std::atomic_bool m_motorTemperatureTreadIsRunning; /**< True if the motor temperature is updated. */ + double m_motorTemperatureDt{0.5}; + std::thread m_motorTemperatureThread; /**< Thread for the motor temperature. */ + // yarp::sig::Vector m_positionFeedbackDegFiltered; yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */ std::unique_ptr m_positionFilter; /**< Joint position low pass filter .*/ @@ -123,10 +133,13 @@ namespace WalkingControllers bool configureForceTorqueSensor(const std::string& portPrefix, const std::string& portInputName, const std::string& wholeBodyDynamicsPortName, + const std::string& carrier, const double& samplingTime, bool useWrenchFilter, double cutFrequency, MeasuredWrench& measuredWrench); + + void readMotorTemperature(); public: /** @@ -205,6 +218,9 @@ namespace WalkingControllers */ const iDynTree::VectorDynSize& getJointVelocity() const; + + iDynTree::VectorDynSize getMotorTemperature() const; + /** * Get the joint upper limit * @return the joint upper bound in radiants diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h new file mode 100644 index 00000000..c03acd62 --- /dev/null +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef WALKING_CONTROLLERS_MOTOR_TEMPERATURE_CHECKER +#define WALKING_CONTROLLERS_MOTOR_TEMPERATURE_CHECKER + +// std +#include + +#include + +#include + +namespace WalkingControllers +{ + class MotorsTemperatureChecker + { + private: + std::vector m_maxTemperature; + std::vector m_samplesAboveTheLimit; + iDynTree::VectorDynSize m_limits; + int m_maxNumberOfSampleAboveTheLimits; + + public: + + const std::vector& getMaxTemperature() const; + std::vector getMotorsOverLimit() const; + bool isThereAMotorOverLimit() const; + bool setMotorTemperatures(const iDynTree::VectorDynSize& temperature); + bool configure(const yarp::os::Searchable& config, int dofs); + }; +}; +#endif diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index cbf26422..92e1eff5 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -269,6 +269,7 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) options.put("localPortPrefix", "/" + name + "/remoteControlBoard"); yarp::os::Property& remoteControlBoardsOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS"); remoteControlBoardsOpts.put("writeStrict", "on"); + remoteControlBoardsOpts.put("carrier", "shmem"); // get the actuated DoFs m_actuatedDOFs = m_axesList.size(); @@ -313,6 +314,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) } } + m_motorTemperatures.resize(m_actuatedDOFs); + // open the device if(!m_robotDevice.open(options)) { @@ -363,6 +366,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) return false; } + if (!m_robotDevice.view(m_motorInterface) || !m_motorInterface) + { + yError() << "[configureRobot] Cannot obtain IMotor interface"; + return false; + } + // resize the buffers m_positionFeedbackDeg.resize(m_actuatedDOFs, 0.0); m_velocityFeedbackDeg.resize(m_actuatedDOFs, 0.0); @@ -482,12 +491,35 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) return false; } + m_motorTemperatureThread = std::thread(&RobotInterface::readMotorTemperature, this); + return true; } +void RobotInterface::readMotorTemperature() +{ + m_motorTemperatureTreadIsRunning = true; + + while(m_motorTemperatureTreadIsRunning) + { + { + std::lock_guard lock(m_motorTemperatureMutex); + + if(!m_motorInterface->getTemperatures(m_motorTemperatures.data())) + { + yError() << "[RobotInterface::readMotorTemperature] Unable to get the motor temperatures."; + } + } + + // TODO GR you should load the time required for the read and close it + yarp::os::Time::delay(m_motorTemperatureDt); + } +} + bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix, const std::string& portInputName, const std::string& wholeBodyDynamicsPortName, + const std::string& carrier, const double& samplingTime, bool useWrenchFilter, double cutFrequency, @@ -497,7 +529,7 @@ bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix, measuredWrench.port = std::make_unique>(); measuredWrench.port->open("/" + portPrefix + portInputName); // connect port - if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName)) + if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName, carrier)) { yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port " << wholeBodyDynamicsPortName << " to " << "/" + portPrefix + portInputName; @@ -643,6 +675,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con ok = ok && this->configureForceTorqueSensor(portPrefix, leftFootWrenchInputPorts[i], leftFootWrenchOutputPorts[i], + "shmem", samplingTime, useWrenchFilter, cutFrequency, m_leftFootMeasuredWrench[i]); @@ -653,6 +686,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con ok = ok && this->configureForceTorqueSensor(portPrefix, rightFootWrenchInputPorts[i], rightFootWrenchOutputPorts[i], + "shmem", samplingTime, useWrenchFilter, cutFrequency, m_rightFootMeasuredWrench[i]); @@ -1096,6 +1130,12 @@ bool RobotInterface::setVelocityReferences(const iDynTree::VectorDynSize& desire bool RobotInterface::close() { + m_motorTemperatureTreadIsRunning = false; + if (m_motorTemperatureThread.joinable()) + { + m_motorTemperatureThread.join(); + } + // close all the ports for(auto& wrench : m_leftFootMeasuredWrench) wrench.port->close(); @@ -1119,11 +1159,18 @@ const iDynTree::VectorDynSize& RobotInterface::getJointPosition() const { return m_positionFeedbackRad; } + const iDynTree::VectorDynSize& RobotInterface::getJointVelocity() const { return m_velocityFeedbackRad; } +iDynTree::VectorDynSize RobotInterface::getMotorTemperature() const +{ + std::lock_guard lock(m_motorTemperatureMutex); + return m_motorTemperatures; +} + const iDynTree::Wrench& RobotInterface::getLeftWrench() const { return m_leftWrench; diff --git a/src/RobotInterface/src/MotorTemperatureChecker.cpp b/src/RobotInterface/src/MotorTemperatureChecker.cpp new file mode 100644 index 00000000..2a1790e3 --- /dev/null +++ b/src/RobotInterface/src/MotorTemperatureChecker.cpp @@ -0,0 +1,116 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include + +#include +#include + +using namespace WalkingControllers; + +bool MotorsTemperatureChecker::configure(const yarp::os::Searchable& config, int dofs) +{ + m_limits.resize(dofs); + if(!YarpUtilities::getVectorFromSearchable(config, "temperature_limits", m_limits)) + { + yError() << "[MotorsTemperatureChecker::configure] Unable to get the parameter temperature limits"; + return false; + } + + m_samplesAboveTheLimit = std::vector(dofs, 0); + + if(!YarpUtilities::getNumberFromSearchable(config, "max_samples_above_the_limits", m_maxNumberOfSampleAboveTheLimits)) + { + yError() << "[MotorsTemperatureChecker::configure] Unable to get the parameter 'max_samples_above_the_limits'"; + return false; + } + + m_maxTemperature.resize(dofs); + for (int i = 0; i < dofs; i++) + { + m_maxTemperature[i] = 0; + } + + return true; +} + +bool MotorsTemperatureChecker::isThereAMotorOverLimit() const +{ + if (m_maxNumberOfSampleAboveTheLimits < 0) + { + return false; + } + + for (const auto sample : m_samplesAboveTheLimit) + { + if (sample > m_maxNumberOfSampleAboveTheLimits) + { + return true; + } + } + return false; +} + +std::vector MotorsTemperatureChecker::getMotorsOverLimit() const +{ + std::vector indeces; + + if (m_maxNumberOfSampleAboveTheLimits < 0) + { + return indeces; + } + + for (int i =0; i < m_samplesAboveTheLimit.size(); i++) + { + if (m_samplesAboveTheLimit[i] > m_maxNumberOfSampleAboveTheLimits) + { + indeces.push_back(i); + } + } + return indeces; +} + +bool MotorsTemperatureChecker::setMotorTemperatures(const iDynTree::VectorDynSize& temperature) +{ + if (temperature.size() != m_limits.size()) + { + yError() << "[MotorsTemperatureChecker::setMotorTemperatures] Unexpected size of the temperature vector" + << "provided: " << temperature.size() << " expected: " << m_limits.size(); + return false; + } + + if (m_maxNumberOfSampleAboveTheLimits < 0) + { + return true; + } + + for (int i = 0; i < temperature.size(); i++) + { + + if (m_limits(i) < 0) + { + continue; + } + + if (temperature(i) > m_limits(i)) + { + m_samplesAboveTheLimit[i]++; + } + else + { + m_samplesAboveTheLimit[i] = 0; + } + + if (temperature(i) > m_maxTemperature[i]) + { + m_maxTemperature[i] = temperature(i); + } + } + + return true; +} + +const std::vector& MotorsTemperatureChecker::getMaxTemperature() const +{ + return m_maxTemperature; +} diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/inverseKinematics.ini index 558e9e63..5513b5eb 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/inverseKinematics.ini @@ -17,10 +17,17 @@ solver-verbosity 0 max-cpu-time 20 #DEGREES +; jointRegularization (0.0, 0.0, 0.0, 0.0, +; 0.0, 0.0, 0.0, +; 12.0, 7.0, -12.0, 41.0, 0.0, 0.0, 0.0, +; 12.0, 7.0, -12.0, 41.0, 0.0, 0.0, 0.0, +; 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, +; 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) + jointRegularization (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 12.0, 7.0, -12.0, 41.0, 0.0, 0.0, 0.0, - 12.0, 7.0, -12.0, 41.0, 0.0, 0.0, 0.0, + 0.0, 5.0, 0.0, 10.0, 0.0, 0.0, 0.0, + 0.0, 5.0, 0.0, 10.0, 0.0, 0.0, 0.0, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/jointRetargeting.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/jointRetargeting.ini index 6d22ce37..03da2ea5 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/jointRetargeting.ini @@ -25,4 +25,4 @@ robot_orientation_port_name /torsoYaw:o smoothing_time_approaching 2.0 smoothing_time_walking 1.0 com_height_scaling_factor 0.5 -variation_range (-0.05 0.0) +variation_range (-0.1 0.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini index b16519e4..55015a88 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/qpInverseKinematicsBlf.ini @@ -1,4 +1,6 @@ use_feedforward_term_for_joint_retargeting false +use_angular_momentum_task true + [IK] robot_velocity_variable_name "robot_velocity" @@ -26,6 +28,8 @@ frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 +[include ANGULAR_MOMENTUM_TASK "./tasks/angular_momentum.ini"] + [include TORSO_TASK "./tasks/torso.ini"] [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] [include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"] diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/robotControl.ini index fbcd0164..f5ebf087 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/robotControl.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/robotControl.ini @@ -34,3 +34,12 @@ good_tracking_required (false, false, false, false false, false, true, true, false, false, false, true, true, true, true, true, true, true, true, true, true, true, true) + +temperature_limits (-1, -1, -1, -1, + 90, 90, 90, + -1, -1, -1, -1, -1, -1, -1, + -1, -1, -1, -1, -1, -1, -1, + 90, 100, 90, 90, 90, 90, + 90, 100, 90, 90, 90, 90) + +max_samples_above_the_limits 2000 diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/angular_momentum.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/angular_momentum.ini new file mode 100644 index 00000000..5f408e64 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/angular_momentum.ini @@ -0,0 +1,16 @@ + +robot_velocity_variable_name "robot_velocity" +mask (false, false, true) + + +states ("STANCE", "WALKING") +sampling_time 0.001 +settling_time 3.0 + +[STANCE] +name "stance" +weight (0.0) + +[WALKING] +name "walking" +weight (5.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini index 47fdc9b7..d2daa48a 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/regularization.ini @@ -1,8 +1,8 @@ robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.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, + 5.0, 5.0, 1.0, + 0.5, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 0.5, 1.0, 1.0, 1.0, 1.0, 1.0, 1.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) @@ -30,7 +30,7 @@ name "walking" weight (0.0, 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, - 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 2.0, 2.0, 1.0, 0.0, 0.0, 0.0, + 1.0, 2.0, 2.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini index af2c0882..8b340a61 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/retargeting.ini @@ -31,7 +31,7 @@ name "walking" weight (2.0, 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, - 2.0, 0.0, 2.0, 2.0, 2.0, 2.0, 2.0, - 2.0, 0.0, 2.0, 2.0, 2.0, 2.0, 2.0, + 0.0, 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, + 0.0, 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini index dc626bbb..e1920ca9 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/torso.ini @@ -1,6 +1,6 @@ robot_velocity_variable_name "robot_velocity" frame_name "chest" -kp_angular 5.0 +kp_angular 10.0 states ("STANCE", "WALKING") @@ -13,4 +13,4 @@ weight (0.0, 0.0, 0.0) [WALKING] name "walking" -weight (5.0, 5.0, 5.0) +weight (10.0, 10.0, 10.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/common/plannerParams.ini index 7e61da1a..8ddd4979 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/common/plannerParams.ini @@ -21,7 +21,7 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length -maxStepLength 0.32 +maxStepLength 0.31 minStepLength 0.01 maxLengthBackwardFactor 0.8 #Width @@ -31,8 +31,8 @@ minWidth 0.14 maxAngleVariation 18.0 minAngleVariation 5.0 #Timings -maxStepDuration 1.5 -minStepDuration 0.7 +maxStepDuration 1.6 +minStepDuration 0.65 ##Nominal Values #Width @@ -43,13 +43,13 @@ stepLandingVelocity -0.1 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 0.9 +nominalDuration 1.5 lastStepSwitchTime 0.15 switchOverSwingRatio 0.2 #ZMP Delta -leftZMPDelta (0.0 -0.005) -rightZMPDelta (0.0 0.005) +leftZMPDelta (0.0 -0.00) +rightZMPDelta (0.0 0.00) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/robotControl.ini index 7c38fb40..b3c6cf77 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/robotControl.ini @@ -34,3 +34,12 @@ good_tracking_required (false, false, false false, false, true, true, false, false, false, true, true, true, true, true, true, true, true, true, true, true, true) + +temperature_limits (-1, -1, -1, + 90, 90, 90, + -1, -1, -1, -1, -1, -1, -1, + -1, -1, -1, -1, -1, -1, -1, + 90, 100, 90, 90, 90, 90, + 90, 100, 90, 90, 90, 90) + +max_samples_above_the_limits 2000 \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/inverseKinematics.ini index ef948ede..d918cf87 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/inverseKinematics.ini @@ -31,13 +31,19 @@ max-cpu-time 20 # 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, # 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) +; jointRegularization (7, 0.12, -0.01, +; 12.0, 7.0, -12.0, 40.769, +; 12.0, 7.0, -12.0, 40.769, +; 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, +; 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) + + jointRegularization (7, 0.12, -0.01, - 12.0, 7.0, -12.0, 40.769, - 12.0, 7.0, -12.0, 40.769, + 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) - # jointRegularization (7, 0.12, -0.01, # -70.0, 46.70, 25.0, 40.769, # -70.0, 46.70, 25.0, 40.769, diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 82acb692..3e62a76e 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -1,3 +1,5 @@ +use_angular_momentum_task true + [IK] robot_velocity_variable_name "robot_velocity" @@ -24,5 +26,10 @@ frame_name "l_sole" kp_linear 7.0 kp_angular 5.0 +[ANGULAR_MOMENTUM_TASK] +robot_velocity_variable_name "robot_velocity" +mask (false, false, true) +weight (5.0) + [include TORSO_TASK "./tasks/torso.ini"] [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/robotControl.ini index b80dc53b..f2484438 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/robotControl.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/robotControl.ini @@ -30,3 +30,13 @@ good_tracking_required (true, true, true, true, true, true, false, true, true, true, true, true, true, true, true, true, true, true, true) + + +temperature_limits (90, 90, 90, + -1, -1, -1, -1, + -1, -1, -1, -1, + 90, 100, 90, 90, 90, 90, + 90, 100, 90, 90, 90, 90) + +max_samples_above_the_limits 2000 + \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini index 758dfc72..59264db5 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,7 +1,7 @@ robot_velocity_variable_name "robot_velocity" -kp (5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, 5.0, +kp (5.0, 5.0, 1.0, + 0.5, 5.0, 5.0, 0.5, + 0.5, 5.0, 5.0, 0.5, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) @@ -11,16 +11,16 @@ settling_time 0.5 [stance] name "stance" -weight (1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, +weight (1.0, 1.0, 0.5, + 1.0, 2.0, 2.0, 1.0, + 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) [walking] name "walking" -weight (1.0, 1.0, 1.0, - 2.0, 2.0, 2.0, 2.0, - 2.0, 2.0, 2.0, 2.0, +weight (1.0, 1.0, 0.5, + 1.0, 2.0, 2.0, 1.0, + 1.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini index b804b6a8..d72a3059 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/tasks/torso.ini @@ -1,6 +1,6 @@ robot_velocity_variable_name "robot_velocity" frame_name "chest" -kp_angular 5.0 +kp_angular 10.0 states ("stance", "walking") @@ -9,8 +9,8 @@ settling_time 0.5 [stance] name "stance" -weight (5.0, 5.0, 5.0) +weight (10.0, 10.0, 10.0) [walking] name "walking" -weight (5.0, 5.0, 5.0) +weight (10.0, 10.0, 10.0) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 638b262c..e469b847 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -29,6 +29,8 @@ // WalkingControllers library #include #include +#include + #include #include #include @@ -61,6 +63,8 @@ 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. */ bool m_useMPC; /**< True if the MPC controller is used. */ @@ -75,6 +79,7 @@ namespace WalkingControllers iDynTree::Position m_zmpOffsetLocal; /** < Offset in the local frame*/ std::unique_ptr m_robotControlHelper; /**< Robot control helper. */ + std::unique_ptr m_motorTemperatureChecker; /**< Robot control helper. */ std::unique_ptr m_trajectoryGenerator; /**< Pointer to the trajectory generator object. */ std::unique_ptr m_freeSpaceEllipseManager; /**< Pointer to the free space ellipse manager. */ std::unique_ptr m_walkingController; /**< Pointer to the walking DCM MPC object. */ @@ -118,11 +123,14 @@ namespace WalkingControllers 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_motorTemperature; /**< Vector containing the results of the IK algorithm [rad]. */ iDynTree::Rotation m_inertial_R_worldFrame; /**< Rotation between the inertial and the world frame. */ yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ yarp::os::BufferedPort m_desiredUnyciclePositionPort; /**< Desired robot position port. */ + yarp::os::BufferedPort m_walkingStatusPort; /**< Desired robot position port. */ + std::string m_statusString; bool m_newTrajectoryRequired; /**< if true a new trajectory will be merged soon. (after m_newTrajectoryMergeCounter - 2 cycles). */ size_t m_newTrajectoryMergeCounter; /**< The new trajectory will be merged after m_newTrajectoryMergeCounter - 2 cycles. */ @@ -179,6 +187,7 @@ namespace WalkingControllers bool solveBLFIK(const iDynTree::Position& desiredCoMPosition, const iDynTree::Vector3& desiredCoMVelocity, const iDynTree::Rotation& desiredNeckOrientation, + const iDynTree::SpatialMomentum& desiredCentroidalMomentum, iDynTree::VectorDynSize &output); /** @@ -194,6 +203,8 @@ namespace WalkingControllers */ iDynTree::Rotation computeAverageYawRotationFromPlannedFeet() const; + iDynTree::Twist computeAverageTwistFromPlannedFeet() const; + /** * Generate the first trajectory. * This method has to be called before updateTrajectories() method. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index d74bb0fc..45770663 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -136,8 +136,10 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) 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(); + std::string walkingStatusSuffix = rf.check("walking_status_suffix", yarp::os::Value("/status:o")).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_goalScaling.resize(3); if (!YarpUtilities::getVectorFromSearchable(rf, "goal_port_scaling", m_goalScaling)) @@ -182,6 +184,13 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) return false; } + m_motorTemperatureChecker = std::make_unique(); + if (!m_motorTemperatureChecker->configure(robotControlHelperOptions, m_robotControlHelper->getActuatedDoFs())) + { + yError() << "[WalkingModule::configure] Unable to configure the motor temperature helper."; + return false; + } + yarp::os::Bottle &forceTorqueSensorsOptions = rf.findGroup("FT_SENSORS"); forceTorqueSensorsOptions.append(generalOptions); if (!m_robotControlHelper->configureForceTorqueSensors(forceTorqueSensorsOptions)) @@ -220,6 +229,14 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) return false; } + std::string walkingStatusPort = "/" + getName() + walkingStatusSuffix; + if (!m_walkingStatusPort.open(walkingStatusPort)) + { + yError() << "[WalkingModule::configure] Could not open" << walkingStatusPort << " port."; + return false; + } + + // initialize the trajectory planner m_trajectoryGenerator = std::make_unique(); yarp::os::Bottle &trajectoryPlannerOptions = rf.findGroup("TRAJECTORY_PLANNER"); @@ -409,6 +426,9 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) m_vectorsCollectionServer.populateMetadata("joints_state::velocities::measured", m_robotControlHelper->getAxesList()); m_vectorsCollectionServer.populateMetadata("joints_state::velocities::retargeting", m_robotControlHelper->getAxesList()); + // motor temperature + m_vectorsCollectionServer.populateMetadata("motors_state::temperature::measured", m_robotControlHelper->getAxesList()); + // root link information m_vectorsCollectionServer.populateMetadata("root_link::position::measured", {"x", "y", "z"}); m_vectorsCollectionServer.populateMetadata("root_link::orientation::measured", {"roll", "pitch", "yaw"}); @@ -442,6 +462,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) // resize variables m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); + m_motorTemperature.resize(m_robotControlHelper->getActuatedDoFs()); yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -497,6 +518,7 @@ bool WalkingModule::close() bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition, const iDynTree::Vector3 &desiredCoMVelocity, const iDynTree::Rotation &desiredNeckOrientation, + const iDynTree::SpatialMomentum ¢roidalMomentumDesired, iDynTree::VectorDynSize &output) { const std::string phase = m_isStancePhase.front() ? "stance" : "walking"; @@ -510,7 +532,7 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition, ok = ok && m_BLFIKSolver->setCoMSetPoint(desiredCoMPosition, desiredCoMVelocity); ok = ok && m_BLFIKSolver->setRetargetingJointSetPoint(m_retargetingClient->jointPositions(), m_retargetingClient->jointVelocities()); - + ok = ok && m_BLFIKSolver->setAngularMomentumSetPoint(centroidalMomentumDesired.getAngularVec3()); if (m_useRootLinkForHeight) { ok = ok && m_BLFIKSolver->setRootSetPoint(desiredCoMPosition, desiredCoMVelocity); @@ -556,6 +578,8 @@ bool WalkingModule::updateModule() { std::lock_guard guard(m_mutex); + int initialStatusStringLength = -1; + if (m_robotState == WalkingFSM::Preparing) { if (!m_robotControlHelper->getFeedbacksRaw(m_feedbackAttempts, m_feedbackAttemptDelay)) @@ -669,12 +693,39 @@ bool WalkingModule::updateModule() desiredUnicyclePosition = m_desiredUnyciclePositionPort.read(false); if (desiredUnicyclePosition != nullptr) { + m_statusString = ""; + // take m_time cast to int do the modulus 3 and then append a string to m_statusString of 3 caracters where the first + // modulo 3 is the number of = and the rest is the number of spaces + m_statusString += std::string((static_cast(m_time) % 3), '='); + m_statusString += std::string(3 - (static_cast(m_time) % 3), ' '); + m_statusString += " "; + initialStatusStringLength = m_statusString.size(); + applyGoalScaling(*desiredUnicyclePosition); if (!setPlannerInput(*desiredUnicyclePosition)) { 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)) + { + m_statusString = ""; + // take m_time cast to int do the modulus 3 and then append a string to m_statusString of 3 caracters where the first + // modulo 3 is the number of = and the rest is the number of spaces + m_statusString += std::string((static_cast(m_time) % 3), '='); + m_statusString += std::string(3 - (static_cast(m_time) % 3), ' '); + m_statusString += " "; + initialStatusStringLength = m_statusString.size(); + + m_statusString += "Walking TimeOut "; + 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 @@ -732,6 +783,14 @@ bool WalkingModule::updateModule() return false; } + m_motorTemperature = m_robotControlHelper->getMotorTemperature(); + + if (!m_motorTemperatureChecker->setMotorTemperatures(m_motorTemperature)) + { + yError() << "[WalkingModule::updateModule] Unable to set the motor temperature to the helper."; + return false; + } + m_profiler->setEndTime("Feedback"); auto retargetingPhase = m_isStancePhase.front() ? RetargetingClient::Phase::Stance : RetargetingClient::Phase::Walking; @@ -893,6 +952,10 @@ bool WalkingModule::updateModule() yawRotation = yawRotation.inverse(); modifiedInertial = yawRotation * m_inertial_R_worldFrame; + // compute the desired torso velocity + const iDynTree::Twist desiredTorsoVelocity = this->computeAverageTwistFromPlannedFeet(); + auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * desiredTorsoVelocity; + if (m_useQPIK) { // integrate dq because velocity control mode seems not available @@ -910,6 +973,7 @@ bool WalkingModule::updateModule() if (!solveBLFIK(desiredCoMPosition, desiredCoMVelocity, yawRotation, + centroidalMomentumDesired, m_dqDesired)) { yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with " @@ -1065,6 +1129,9 @@ bool WalkingModule::updateModule() m_vectorsCollectionServer.populateData("joints_state::velocities::measured", m_robotControlHelper->getJointVelocity()); m_vectorsCollectionServer.populateData("joints_state::velocities::retargeting", m_retargetingClient->jointVelocities()); + // motor temperature + m_vectorsCollectionServer.populateData("motors_state::temperature::measured", m_motorTemperature); + // root link information m_vectorsCollectionServer.populateData("root_link::position::measured", m_FKSolver->getRootLinkToWorldTransform().getPosition()); m_vectorsCollectionServer.populateData("root_link::orientation::measured", m_FKSolver->getRootLinkToWorldTransform().getRotation().asRPY()); @@ -1078,6 +1145,15 @@ bool WalkingModule::updateModule() m_vectorsCollectionServer.sendData(); } + auto& statusMsg = m_walkingStatusPort.prepare(); + if (m_statusString.size() == initialStatusStringLength) + { + m_statusString += "All Good"; + } + + statusMsg.clear(); + statusMsg.addString(m_statusString); + m_walkingStatusPort.write(); propagateTime(); @@ -1114,6 +1190,21 @@ iDynTree::Rotation WalkingModule::computeAverageYawRotationFromPlannedFeet() con return iDynTree::Rotation::RotZ(meanYaw); } +iDynTree::Twist WalkingModule::computeAverageTwistFromPlannedFeet() const +{ + iDynTree::Twist twist; + iDynTree::Vector3 meanLinearVelocity, meanAngularVelocity; + iDynTree::toEigen(meanLinearVelocity) = (iDynTree::toEigen(m_leftTwistTrajectory.front().getLinearVec3()) + + iDynTree::toEigen(m_rightTwistTrajectory.front().getLinearVec3())) / 2.0; + iDynTree::toEigen(meanAngularVelocity) = (iDynTree::toEigen(m_leftTwistTrajectory.front().getAngularVec3()) + + iDynTree::toEigen(m_rightTwistTrajectory.front().getAngularVec3())) / 2.0; + + twist.setLinearVec3(meanLinearVelocity); + twist.setAngularVec3(meanAngularVelocity); + + return twist; +} + bool WalkingModule::prepareRobot(bool onTheFly) { if (m_robotState != WalkingFSM::Configured && m_robotState != WalkingFSM::Stopped) @@ -1497,8 +1588,27 @@ bool WalkingModule::startWalking() bool WalkingModule::setPlannerInput(const yarp::sig::Vector &plannerInput) { - m_plannerInput = plannerInput; + if (m_motorTemperatureChecker->isThereAMotorOverLimit()) + { + yWarning() << "[WalkingModule::setPlannerInput] The motor temperature is over the limit."; + std::vector indeces = m_motorTemperatureChecker->getMotorsOverLimit(); + std::string msg = "The following motors temperature is over the limits: "; + for (auto index : indeces) + { + msg += m_robotControlHelper->getAxesList()[index] + + ": Max temperature: " + + std::to_string(m_motorTemperatureChecker->getMaxTemperature()[index]) + " C."; + } + msg += "The trajectory will be set to zero."; + m_statusString += msg; + yWarning() << msg; + m_plannerInput.zero(); + } + else + { + m_plannerInput = plannerInput; + } // the trajectory was already finished the new trajectory will be attached as soon as possible if (m_mergePoints.empty()) { diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index d11452b2..ef3d8bcf 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -16,6 +16,8 @@ #include #include #include +#include + #include @@ -45,6 +47,8 @@ class BLFIK bool setRootSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity); bool setTorsoSetPoint(const iDynTree::Rotation& rotation); const iDynTree::VectorDynSize& getDesiredJointVelocity() const; + bool setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum); + iDynTree::Twist getDesiredTorsoVelocity() const; private: std::shared_ptr @@ -53,6 +57,8 @@ class BLFIK m_jointRegularizationWeight; std::shared_ptr m_jointRetargetingWeight; + std::shared_ptr + m_angularMomentumWeight; BipedalLocomotion::IK::QPInverseKinematics m_qpIK; BipedalLocomotion::System::VariablesHandler m_variableHandler; @@ -64,11 +70,13 @@ class BLFIK std::shared_ptr m_rootTask; std::shared_ptr m_jointRetargetingTask; std::shared_ptr m_jointRegularizationTask; + std::shared_ptr m_angularMomentumTask; iDynTree::VectorDynSize m_jointVelocity; bool m_usejointRetargeting{false}; bool m_useRootLinkForHeight{false}; bool m_useFeedforwardTermForJointRetargeting{false}; + bool m_useAngularMomentumTask{false}; }; } // namespace WalkingControllers diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index bef835ce..76662884 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -40,6 +40,9 @@ bool BLFIK::initialize( m_usejointRetargeting = false; ptr->getParameter("use_joint_retargeting", m_usejointRetargeting); + m_useAngularMomentumTask = false; + ptr->getParameter("use_angular_momentum_task", m_useAngularMomentumTask); + ptr->getParameter("use_feedforward_term_for_joint_retargeting", m_useFeedforwardTermForJointRetargeting); @@ -57,6 +60,27 @@ bool BLFIK::initialize( = std::make_shared(); ok = ok && m_torsoWeight->initialize(ptr->getGroup("TORSO_TASK")); + if (m_useAngularMomentumTask) + { + m_angularMomentumTask = std::make_shared(); + ok = ok && m_angularMomentumTask->setKinDyn(kinDyn); + ok = ok && m_angularMomentumTask->initialize(ptr->getGroup("ANGULAR_MOMENTUM_TASK")); + + m_angularMomentumWeight = std::make_shared(); + ok = ok && m_angularMomentumWeight->initialize(ptr->getGroup("ANGULAR_MOMENTUM_TASK")); + + ok = ok + && m_qpIK.addTask(m_angularMomentumTask, + "angular_momentum_task", + lowPriority, + m_angularMomentumWeight); + } + if (!ok) + { + BipedalLocomotion::log()->error("{} Unable to initialize the angular momentum task.", prefix); + return false; + } + if (m_usejointRetargeting) { m_jointRetargetingWeight = std::make_shared< @@ -131,6 +155,10 @@ bool BLFIK::solve() { bool ok = m_torsoWeight->advance(); ok = ok && m_jointRegularizationWeight->advance(); + if (m_angularMomentumWeight) + { + ok = ok && m_angularMomentumWeight->advance(); + } if (m_usejointRetargeting) { ok = ok && m_jointRetargetingWeight->advance(); @@ -151,6 +179,10 @@ bool BLFIK::setPhase(const std::string& phase) { bool ok = m_torsoWeight->setState(phase); ok = ok && m_jointRegularizationWeight->setState(phase); + if (m_angularMomentumWeight) + { + ok = ok && m_angularMomentumWeight->setState(phase); + } if (m_usejointRetargeting) ok = ok && m_jointRetargetingWeight->setState(phase); @@ -219,3 +251,21 @@ const iDynTree::VectorDynSize& BLFIK::getDesiredJointVelocity() const { return m_jointVelocity; } + +iDynTree::Twist BLFIK::getDesiredTorsoVelocity() const +{ + iDynTree::Twist tmp; + tmp.zero(); + iDynTree::toEigen(tmp.getAngularVec3()) = m_torsoTask->getB(); + return tmp; +} + +bool BLFIK::setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum) +{ + if (!m_angularMomentumTask) + { + return true; + } + + return m_angularMomentumTask->setSetPoint(iDynTree::toEigen(angularMomentum)); +}