From 6c4f18ecf1fe1c58eead008b831e3a0fc26bb6aa Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Oct 2024 18:17:50 +0200 Subject: [PATCH 01/14] Enable the temperature reading in RobotInterface::Helper --- .../RobotInterface/Helper.h | 15 +++++++ src/RobotInterface/src/Helper.cpp | 43 +++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index e2637b6f..c451d584 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 .*/ @@ -127,6 +137,8 @@ namespace WalkingControllers bool useWrenchFilter, double cutFrequency, MeasuredWrench& measuredWrench); + + void readMotorTemperature(); public: /** @@ -205,6 +217,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/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index cbf26422..79898a6a 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -313,6 +313,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config) } } + m_motorTemperatures.resize(m_actuatedDOFs); + // open the device if(!m_robotDevice.open(options)) { @@ -363,6 +365,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,9 +490,31 @@ 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, @@ -1096,6 +1126,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 +1155,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; From 69aaa5e3068a4a397959dcfff5dd8fbfd351a8de Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Oct 2024 19:08:02 +0200 Subject: [PATCH 02/14] Implement the MotorsTemperatureChecker and the logic to prevent the robot to walk when the temperature is greather then a value --- src/RobotInterface/CMakeLists.txt | 4 +- .../RobotInterface/MotorTemperatureChecker.h | 33 ++++++ .../src/MotorTemperatureChecker.cpp | 111 ++++++++++++++++++ .../WalkingControllers/WalkingModule/Module.h | 4 + src/WalkingModule/src/Module.cpp | 36 +++++- 5 files changed, 185 insertions(+), 3 deletions(-) create mode 100644 src/RobotInterface/include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h create mode 100644 src/RobotInterface/src/MotorTemperatureChecker.cpp 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/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/MotorTemperatureChecker.cpp b/src/RobotInterface/src/MotorTemperatureChecker.cpp new file mode 100644 index 00000000..e41cdbf3 --- /dev/null +++ b/src/RobotInterface/src/MotorTemperatureChecker.cpp @@ -0,0 +1,111 @@ +// 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 (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/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 638b262c..d2f6d431 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 @@ -75,6 +77,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,6 +121,7 @@ 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. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index d74bb0fc..6a49523c 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -182,6 +182,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)) @@ -442,6 +449,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."; @@ -732,6 +740,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; @@ -1497,8 +1513,26 @@ 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 are over the limits: "; + for (auto index : indeces) + { + msg += m_robotControlHelper->getAxesList()[index] + + ": Max temperature: " + + std::to_string(m_motorTemperatureChecker->getMaxTemperature()[index]) + " celsius."; + } + msg += "The trajectory will be set to zero."; + 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()) { From 50a00fda17b74926a0505fc31d5f0566403bf65e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Oct 2024 19:23:59 +0200 Subject: [PATCH 03/14] Add a logic to stop the robot if the goal is not set for more than a given ammount of time --- .../WalkingControllers/WalkingModule/Module.h | 2 ++ src/WalkingModule/src/Module.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index d2f6d431..2aa3b2f2 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -63,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. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 6a49523c..a726db36 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -138,6 +138,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) std::string goalSuffix = rf.check("goal_port_suffix", yarp::os::Value("/goal:i")).asString(); m_skipDCMController = rf.check("skip_dcm_controller", yarp::os::Value(false)).asBool(); m_removeZMPOffset = rf.check("remove_zmp_offset", yarp::os::Value(false)).asBool(); + m_maxTimeToWaitForGoal = rf.check("max_time_to_wait_for_goal", yarp::os::Value(1.0)).asFloat64(); m_goalScaling.resize(3); if (!YarpUtilities::getVectorFromSearchable(rf, "goal_port_scaling", m_goalScaling)) @@ -683,6 +684,17 @@ bool WalkingModule::updateModule() yError() << "[WalkingModule::updateModule] Unable to set the planner input"; return false; } + m_lastSetGoalTime = m_time; + } + else if (!m_firstRun && ((m_time - m_lastSetGoalTime) > m_maxTimeToWaitForGoal)) + { + yWarning() << "[WalkingModule::updateModule] The goal has not been set for more than " << m_maxTimeToWaitForGoal << " seconds."; + yarp::sig::Vector dummy(3, 0.0); + if (!setPlannerInput(dummy)) + { + yError() << "[WalkingModule::updateModule] Unable to set the planner input"; + return false; + } } // if a new trajectory is required check if its the time to evaluate the new trajectory or From cfd5ee97485b01d41e5f629756675057cf5ee426 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Oct 2024 19:24:10 +0200 Subject: [PATCH 04/14] Log the temperature --- src/WalkingModule/src/Module.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index a726db36..b018af9a 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -417,6 +417,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"}); @@ -1093,6 +1096,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()); From 2cdc0addb79d7e3a490fd0724fec856317b1b1c5 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 10:33:08 +0200 Subject: [PATCH 05/14] Avoid to check the temperature if negative --- src/RobotInterface/src/MotorTemperatureChecker.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/RobotInterface/src/MotorTemperatureChecker.cpp b/src/RobotInterface/src/MotorTemperatureChecker.cpp index e41cdbf3..2a1790e3 100644 --- a/src/RobotInterface/src/MotorTemperatureChecker.cpp +++ b/src/RobotInterface/src/MotorTemperatureChecker.cpp @@ -86,8 +86,13 @@ bool MotorsTemperatureChecker::setMotorTemperatures(const iDynTree::VectorDynSiz for (int i = 0; i < temperature.size(); i++) { + + if (m_limits(i) < 0) + { + continue; + } - if (temperature(i) > m_limits(i)) + if (temperature(i) > m_limits(i)) { m_samplesAboveTheLimit[i]++; } From 9f3b9bc0d1205f5bdc74fdbaeb7b382fe67b797d Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 11:22:43 +0200 Subject: [PATCH 06/14] Add publish of the temperature warning --- .../WalkingControllers/WalkingModule/Module.h | 2 + src/WalkingModule/src/Module.cpp | 42 ++++++++++++++++++- 2 files changed, 42 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 2aa3b2f2..b66173f7 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -129,6 +129,8 @@ namespace WalkingControllers 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. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b018af9a..b7e59e02 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -136,6 +136,7 @@ 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(); @@ -228,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"); @@ -568,6 +577,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)) @@ -681,6 +692,14 @@ 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)) { @@ -691,7 +710,16 @@ bool WalkingModule::updateModule() } 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(); + yWarning() << "[WalkingModule::updateModule] The goal has not been set for more than " << m_maxTimeToWaitForGoal << " seconds."; + m_statusString += "Walking TimeOut "; yarp::sig::Vector dummy(3, 0.0); if (!setPlannerInput(dummy)) { @@ -1112,6 +1140,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(); @@ -1535,14 +1572,15 @@ bool WalkingModule::setPlannerInput(const yarp::sig::Vector &plannerInput) { yWarning() << "[WalkingModule::setPlannerInput] The motor temperature is over the limit."; std::vector indeces = m_motorTemperatureChecker->getMotorsOverLimit(); - std::string msg = "The following motors temperature are over the limits: "; + 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]) + " celsius."; + + std::to_string(m_motorTemperatureChecker->getMaxTemperature()[index]) + " C."; } msg += "The trajectory will be set to zero."; + m_statusString += msg; yWarning() << msg; m_plannerInput.zero(); From 26770d783a9d596349aa6c3920d25ca55e1fa4f8 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 11:23:22 +0200 Subject: [PATCH 07/14] Update the config of ergocubsn002 for the temperature reading --- .../dcm_walking/autonomous/robotControl.ini | 9 +++++++++ .../iFeel_joint_retargeting/robotControl.ini | 9 +++++++++ .../dcm_walking/joypad_control/robotControl.ini | 10 ++++++++++ 3 files changed, 28 insertions(+) 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..0b35a722 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, 90, 90, 90, 90, 90, + 90, 90, 90, 90, 90, 90) + +max_samples_above_the_limits 2000 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..1df6db10 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, 90, 90, 90, 90, 90, + 90, 90, 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/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/robotControl.ini index b80dc53b..81a144bf 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, 90, 90, 90, 90, 90, + 90, 90, 90, 90, 90, 90) + +max_samples_above_the_limits 2000 + \ No newline at end of file From 1a7cc857f299a2d963984018da87bb26fbf8dfc2 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 11:32:56 +0200 Subject: [PATCH 08/14] Remove useless print --- src/WalkingModule/src/Module.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b7e59e02..8a06fb4d 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -718,7 +718,6 @@ bool WalkingModule::updateModule() m_statusString += " "; initialStatusStringLength = m_statusString.size(); - yWarning() << "[WalkingModule::updateModule] The goal has not been set for more than " << m_maxTimeToWaitForGoal << " seconds."; m_statusString += "Walking TimeOut "; yarp::sig::Vector dummy(3, 0.0); if (!setPlannerInput(dummy)) From ddda296c369f63d9292c74bc05d643513a4417c4 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 16:51:05 +0200 Subject: [PATCH 09/14] [ethoCubSN002] Increase the termperature limit for the hip roll from 90 to 100 --- .../ergoCubSN002/dcm_walking/autonomous/robotControl.ini | 4 ++-- .../dcm_walking/iFeel_joint_retargeting/robotControl.ini | 4 ++-- .../ergoCubSN002/dcm_walking/joypad_control/robotControl.ini | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) 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 0b35a722..f5ebf087 100644 --- a/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/robotControl.ini +++ b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/robotControl.ini @@ -39,7 +39,7 @@ temperature_limits (-1, -1, -1, -1, 90, 90, 90, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - 90, 90, 90, 90, 90, 90, - 90, 90, 90, 90, 90, 90) + 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/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/iFeel_joint_retargeting/robotControl.ini index 1df6db10..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 @@ -39,7 +39,7 @@ temperature_limits (-1, -1, -1, 90, 90, 90, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - 90, 90, 90, 90, 90, 90, - 90, 90, 90, 90, 90, 90) + 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/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/joypad_control/robotControl.ini index 81a144bf..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 @@ -35,8 +35,8 @@ good_tracking_required (true, true, true, temperature_limits (90, 90, 90, -1, -1, -1, -1, -1, -1, -1, -1, - 90, 90, 90, 90, 90, 90, - 90, 90, 90, 90, 90, 90) + 90, 100, 90, 90, 90, 90, + 90, 100, 90, 90, 90, 90) max_samples_above_the_limits 2000 \ No newline at end of file From 7c1f3a7960bd139c67bca821bb5ef2bca7cbd366 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 16:51:57 +0200 Subject: [PATCH 10/14] [ergoCubSN002] Tune the walking velocity --- .../ergoCubSN002/dcm_walking/common/plannerParams.ini | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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..e4aa49f8 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.34 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,9 +43,9 @@ stepLandingVelocity -0.1 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 0.9 +nominalDuration 1.5 lastStepSwitchTime 0.15 -switchOverSwingRatio 0.2 +switchOverSwingRatio 0.15 #ZMP Delta leftZMPDelta (0.0 -0.005) From 0eb11f7ec5353acb937c794f90b4ae397a723b06 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 16:53:11 +0200 Subject: [PATCH 11/14] Add angular momentum task in the IK --- .../joypad_control/inverseKinematics.ini | 12 ++++-- .../joypad_control/qpInverseKinematicsBlf.ini | 7 ++++ .../joypad_control/tasks/regularization.ini | 18 ++++---- .../joypad_control/tasks/torso.ini | 6 +-- .../WalkingControllers/WalkingModule/Module.h | 3 ++ src/WalkingModule/src/Module.cpp | 23 +++++++++- .../WholeBodyControllers/BLFIK.h | 6 +++ src/WholeBodyControllers/src/BLFIK.cpp | 42 +++++++++++++++++++ 8 files changed, 101 insertions(+), 16 deletions(-) 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/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 b66173f7..e469b847 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -187,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); /** @@ -202,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 8a06fb4d..45770663 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -518,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"; @@ -531,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); @@ -951,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 @@ -968,6 +973,7 @@ bool WalkingModule::updateModule() if (!solveBLFIK(desiredCoMPosition, desiredCoMVelocity, yawRotation, + centroidalMomentumDesired, m_dqDesired)) { yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with " @@ -1184,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) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index d11452b2..41bf4ecb 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 @@ -64,11 +68,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..dfbabe2d 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")); + + Eigen::VectorXd angularMomentumWeight; + ok = ok && ptr->getGroup("ANGULAR_MOMENTUM_TASK").lock()->getParameter("weight", angularMomentumWeight); + + ok = ok + && m_qpIK.addTask(m_angularMomentumTask, + "angular_momentum_task", + lowPriority, + angularMomentumWeight); + } + if (!ok) + { + BipedalLocomotion::log()->error("{} Unable to initialize the angular momentum task.", prefix); + return false; + } + if (m_usejointRetargeting) { m_jointRetargetingWeight = std::make_shared< @@ -219,3 +243,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)); +} \ No newline at end of file From ee0837d63320665786794230b0d212043bf704d4 Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 16:53:45 +0200 Subject: [PATCH 12/14] Use shared memory carrier in the walking and in the joypad --- src/JoypadModule/src/Module.cpp | 12 ++++++------ .../WalkingControllers/RobotInterface/Helper.h | 1 + src/RobotInterface/src/Helper.cpp | 6 +++++- 3 files changed, 12 insertions(+), 7 deletions(-) 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/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index c451d584..0162023b 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -133,6 +133,7 @@ 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, diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index 79898a6a..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(); @@ -518,6 +519,7 @@ void RobotInterface::readMotorTemperature() 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, @@ -527,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; @@ -673,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]); @@ -683,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]); From d7a31e1ce6f20fd65371c04bf4ff317e7cf2f60f Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 21 Nov 2024 12:17:01 +0100 Subject: [PATCH 13/14] IROS code angular momentum --- .../dcm_walking/autonomous/inverseKinematics.ini | 11 +++++++++-- .../dcm_walking/autonomous/jointRetargeting.ini | 2 +- .../autonomous/qpInverseKinematicsBlf.ini | 4 ++++ .../autonomous/tasks/angular_momentum.ini | 16 ++++++++++++++++ .../autonomous/tasks/regularization.ini | 10 +++++----- .../dcm_walking/autonomous/tasks/retargeting.ini | 4 ++-- .../dcm_walking/autonomous/tasks/torso.ini | 4 ++-- .../WholeBodyControllers/BLFIK.h | 2 ++ src/WholeBodyControllers/src/BLFIK.cpp | 16 ++++++++++++---- 9 files changed, 53 insertions(+), 16 deletions(-) create mode 100644 src/WalkingModule/app/robots/ergoCubSN002/dcm_walking/autonomous/tasks/angular_momentum.ini 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/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/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index 41bf4ecb..ef3d8bcf 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -57,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; diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index dfbabe2d..76662884 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -66,14 +66,14 @@ bool BLFIK::initialize( ok = ok && m_angularMomentumTask->setKinDyn(kinDyn); ok = ok && m_angularMomentumTask->initialize(ptr->getGroup("ANGULAR_MOMENTUM_TASK")); - Eigen::VectorXd angularMomentumWeight; - ok = ok && ptr->getGroup("ANGULAR_MOMENTUM_TASK").lock()->getParameter("weight", angularMomentumWeight); + 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, - angularMomentumWeight); + m_angularMomentumWeight); } if (!ok) { @@ -155,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(); @@ -175,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); @@ -260,4 +268,4 @@ bool BLFIK::setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum) } return m_angularMomentumTask->setSetPoint(iDynTree::toEigen(angularMomentum)); -} \ No newline at end of file +} From 02a073a6d741fbce26ad2b767a2328d4b7ddaa16 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 15 Jan 2025 10:22:01 +0100 Subject: [PATCH 14/14] Updated conf after Humanoids --- .../ergoCubSN002/dcm_walking/common/plannerParams.ini | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 e4aa49f8..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.34 +maxStepLength 0.31 minStepLength 0.01 maxLengthBackwardFactor 0.8 #Width @@ -45,11 +45,11 @@ comHeightDelta 0.01 #Timings nominalDuration 1.5 lastStepSwitchTime 0.15 -switchOverSwingRatio 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