From 975fc83421f18f8c29e40a188b5565e4dad32aa6 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 30 Sep 2022 12:57:53 +0200 Subject: [PATCH 1/4] Add the possibility to retarget the CoM x and y coordinate in RetargetingHelper --- .../RetargetingHelper/Helper.h | 19 +- src/RetargetingHelper/src/Helper.cpp | 333 +++++++++++++----- 2 files changed, 257 insertions(+), 95 deletions(-) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index ec4f8719..745ba0ca 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -10,6 +10,7 @@ #define WALKING_CONTROLLERS_RETARGETING_HELPER_HELPER_H // std +#include #include #include @@ -73,7 +74,8 @@ class RetargetingClient struct HDERetargeting : public RetargetingElement { KinematicState joints; - KinematicState com; + KinematicState comHeight; + KinematicState comHorizontal; }; struct HandRetargeting : public RetargetingElement @@ -85,7 +87,8 @@ class RetargetingClient bool m_useHandRetargeting; /**< True if the hand retargeting is used */ bool m_useVirtualizer; /**< True if the virtualizer is used */ bool m_useJointRetargeting; /**< True if the joint retargeting is used */ - bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ + bool m_useCoMHeightRetargeting; /**< True if the com height retargeting is used */ + bool m_useCoMHorizontalRetargeting; /**< True if the horizontal com retargeting is used */ HandRetargeting m_leftHand; /**< Left hand retargeting element */ HandRetargeting m_rightHand; /**< Right hand retargeting element */ @@ -101,6 +104,11 @@ class RetargetingClient /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */ double m_comHeightScalingFactor; + iDynTree::Vector2 m_comHorizontalInputOffset; + iDynTree::Vector2 m_comXLimits; + iDynTree::Vector2 m_comYLimits; + bool m_comHorizontalRetargetingReady{false}; + /** Mapping between the retarget joints and the controlled. */ std::unordered_map m_retargetedJointsToControlJoints; @@ -181,6 +189,13 @@ class RetargetingClient */ const iDynTree::VectorDynSize& jointVelocities() const; + + /** + * Get the position of com horizontal position + */ + const iDynTree::Vector2& comHorizonal() const; + + /** * Get the CoM height * @return height of the CoM diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 63e07feb..941b2c1f 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -9,11 +9,11 @@ #include #include -#include #include +#include -#include #include +#include using namespace WalkingControllers; @@ -24,62 +24,69 @@ void RetargetingClient::convertYarpVectorPoseIntoTransform(const yarp::sig::Vect transform.setRotation(iDynTree::Rotation::RPY(vector(3), vector(4), vector(5))); } -bool RetargetingClient::initialize(const yarp::os::Searchable &config, - const std::string &name, - const double &period, +bool RetargetingClient::initialize(const yarp::os::Searchable& config, + const std::string& name, + const double& period, const std::vector& controlledJointNames) { - if(config.isNull()) + if (config.isNull()) { yInfo() << "[RetargetingClient::initialize] the hand retargeting is disable"; m_useHandRetargeting = false; m_useVirtualizer = false; m_useJointRetargeting = false; m_useCoMHeightRetargeting = false; + m_useCoMHorizontalRetargeting = false; return true; } m_useHandRetargeting = config.check("use_hand_retargeting", yarp::os::Value(false)).asBool(); m_useJointRetargeting = config.check("use_joint_retargeting", yarp::os::Value(false)).asBool(); m_useVirtualizer = config.check("use_virtualizer", yarp::os::Value(false)).asBool(); - m_useCoMHeightRetargeting = config.check("use_com_retargeting", yarp::os::Value(false)).asBool(); + m_useCoMHeightRetargeting + = config.check("use_com_height_retargeting", yarp::os::Value(false)).asBool(); + m_useCoMHorizontalRetargeting + = config.check("use_com_horizontal_retargeting", yarp::os::Value(false)).asBool(); - if(m_useJointRetargeting && m_useHandRetargeting) + if (m_useJointRetargeting && m_useHandRetargeting) { - yError() << "[RetargetingClient::initialize] You cannot enable the joint retargeting along with the hand retargeting."; + yError() << "[RetargetingClient::initialize] You cannot enable the joint retargeting along " + "with the hand retargeting."; return false; } m_hdeRetargeting.joints.position.resize(controlledJointNames.size()); m_hdeRetargeting.joints.velocity.resize(controlledJointNames.size()); m_hdeRetargeting.joints.smoother.yarpBuffer.resize(controlledJointNames.size()); - m_hdeRetargeting.com.smoother.yarpBuffer(1); + m_hdeRetargeting.comHeight.smoother.yarpBuffer(1); + m_hdeRetargeting.comHorizontal.smoother.yarpBuffer(2); - if(!m_useHandRetargeting && !m_useVirtualizer && - !m_useJointRetargeting && !m_useCoMHeightRetargeting) + if (!m_useHandRetargeting && !m_useVirtualizer && !m_useJointRetargeting + && !m_useCoMHeightRetargeting) { return true; } // get the approaching phase duration - if(!YarpUtilities::getNumberFromSearchable(config, "approaching_phase_duration", m_approachPhaseDuration)) + if (!YarpUtilities::getNumberFromSearchable(config, + "approaching_phase_duration", + m_approachPhaseDuration)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } std::string portName; - if(m_useHandRetargeting) + if (m_useHandRetargeting) { const yarp::os::Bottle& option = config.findGroup("HAND_RETARGETING"); - auto initializeHand = [&](auto& hand, const std::string& portNameLabel) -> bool - { + auto initializeHand = [&](auto& hand, const std::string& portNameLabel) -> bool { // open left hand port - if(!YarpUtilities::getStringFromSearchable(option, portNameLabel, - portName)) + if (!YarpUtilities::getStringFromSearchable(option, portNameLabel, portName)) { - yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; + yError() << "[RetargetingClient::initialize] Unable to get the string from " + "searchable."; return false; } hand.port.open("/" + name + portName); @@ -97,53 +104,64 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, "smoothing_time_walking", hand.smoother.smoothingTimeInWalking)) { - yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + yError() << "[RetargetingClient::initialize] Unable to get the number from " + "searchable."; return false; } hand.smoother.yarpBuffer.resize(6); - hand.smoother.smoother = std::make_unique(6, period, hand.smoother.smoothingTimeInApproaching); + hand.smoother.smoother + = std::make_unique(6, + period, + hand.smoother + .smoothingTimeInApproaching); return true; }; - if(!initializeHand(m_leftHand, "left_hand_transform_port_name") - || !initializeHand(m_rightHand, "right_hand_transform_port_name")) + if (!initializeHand(m_leftHand, "left_hand_transform_port_name") + || !initializeHand(m_rightHand, "right_hand_transform_port_name")) return false; } - if(m_useJointRetargeting) + if (m_useJointRetargeting) { const yarp::os::Bottle& option = config.findGroup("JOINT_RETARGETING"); std::vector retargetJointNames; - yarp::os::Value *retargetJointNamesYarp; - if(!option.check("retargeting_joint_list", retargetJointNamesYarp)) + yarp::os::Value* retargetJointNamesYarp; + if (!option.check("retargeting_joint_list", retargetJointNamesYarp)) { - yError() << "[RetargetingClient::initialize] Unable to find joints_list into config file."; + yError() << "[RetargetingClient::initialize] Unable to find joints_list into config " + "file."; return false; } - if(!YarpUtilities::yarpListToStringVector(retargetJointNamesYarp, retargetJointNames)) + if (!YarpUtilities::yarpListToStringVector(retargetJointNamesYarp, retargetJointNames)) { - yError() << "[RetargetingClient::initialize] Unable to convert yarp list into a vector of strings."; + yError() << "[RetargetingClient::initialize] Unable to convert yarp list into a vector " + "of strings."; return false; } // find the indices - for(const std::string& joint : retargetJointNames) + for (const std::string& joint : retargetJointNames) { - const auto element = std::find(controlledJointNames.begin(), controlledJointNames.end(), joint); + const auto element + = std::find(controlledJointNames.begin(), controlledJointNames.end(), joint); if (element == controlledJointNames.end()) { yError() << "[RetargetingClient::initialize] Unable to find the joint named: " << joint << " in the list of the controlled joints."; return false; } - m_retargetedJointsToControlJoints[joint] = std::distance(controlledJointNames.begin(), element); + m_retargetedJointsToControlJoints[joint] + = std::distance(controlledJointNames.begin(), element); } - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", - m_hdeRetargeting.joints.smoother.smoothingTimeInApproaching)) + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_approaching", + m_hdeRetargeting.joints.smoother + .smoothingTimeInApproaching)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; @@ -151,7 +169,8 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, if (!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", - m_hdeRetargeting.joints.smoother.smoothingTimeInWalking)) + m_hdeRetargeting.joints.smoother + .smoothingTimeInWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; @@ -160,15 +179,15 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_hdeRetargeting.joints.smoother.smoother = std::make_unique(controlledJointNames.size(), period, - m_hdeRetargeting.joints.smoother.smoothingTimeInApproaching); + m_hdeRetargeting.joints.smoother + .smoothingTimeInApproaching); } - if(m_useVirtualizer) + if (m_useVirtualizer) { const yarp::os::Bottle& option = config.findGroup("VIRTUALIZER"); - if(!YarpUtilities::getStringFromSearchable(option, "robot_orientation_port_name", - portName)) + if (!YarpUtilities::getStringFromSearchable(option, "robot_orientation_port_name", portName)) { yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; @@ -178,11 +197,12 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, if (m_useCoMHeightRetargeting) { - const yarp::os::Bottle& option = config.findGroup("COM_RETARGETING"); + const yarp::os::Bottle& option = config.findGroup("COM_HEIGHT_RETARGETING"); if (!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", - m_hdeRetargeting.com.smoother.smoothingTimeInApproaching)) + m_hdeRetargeting.comHeight.smoother + .smoothingTimeInApproaching)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; @@ -190,16 +210,18 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, if (!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", - m_hdeRetargeting.com.smoother.smoothingTimeInWalking)) + m_hdeRetargeting.comHeight.smoother + .smoothingTimeInWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - m_hdeRetargeting.com.smoother.smoother + m_hdeRetargeting.comHeight.smoother.smoother = std::make_unique(1, period, - m_hdeRetargeting.com.smoother.smoothingTimeInApproaching); + m_hdeRetargeting.comHeight.smoother + .smoothingTimeInApproaching); if (!YarpUtilities::getNumberFromSearchable(option, "com_height_scaling_factor", @@ -210,7 +232,48 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } } - if (m_useJointRetargeting || m_useCoMHeightRetargeting) + if (m_useCoMHorizontalRetargeting) + { + const yarp::os::Bottle& option = config.findGroup("COM_HORIZONTAL_RETARGETING"); + + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_approaching", + m_hdeRetargeting.comHorizontal.smoother + .smoothingTimeInApproaching)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_walking", + m_hdeRetargeting.comHorizontal.smoother + .smoothingTimeInWalking)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + m_hdeRetargeting.comHeight.smoother.smoother + = std::make_unique(2, + period, + m_hdeRetargeting.comHorizontal.smoother + .smoothingTimeInApproaching); + + if (!YarpUtilities::getVectorFromSearchable(option, "com_x_limits", m_comXLimits)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + + if (!YarpUtilities::getVectorFromSearchable(option, "com_y_limits", m_comYLimits)) + { + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + return false; + } + } + + if (m_useJointRetargeting || m_useCoMHeightRetargeting || m_useCoMHorizontalRetargeting) { if (!YarpUtilities::getStringFromSearchable(config, "hde_port_name", portName)) { @@ -225,18 +288,19 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, bool RetargetingClient::reset(WalkingFK& kinDynWrapper) { - m_leftHand.transform = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getLeftHandToWorldTransform(); - m_rightHand.transform = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getRightHandToWorldTransform(); + m_leftHand.transform = kinDynWrapper.getHeadToWorldTransform().inverse() + * kinDynWrapper.getLeftHandToWorldTransform(); + m_rightHand.transform = kinDynWrapper.getHeadToWorldTransform().inverse() + * kinDynWrapper.getRightHandToWorldTransform(); - if(m_useHandRetargeting) + if (m_useHandRetargeting) { - auto resetHandSmoother = [](auto& hand) - { - iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(0) = - iDynTree::toEigen(hand.transform.getPosition()); + auto resetHandSmoother = [](auto& hand) { + iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(0) + = iDynTree::toEigen(hand.transform.getPosition()); - iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(3) = - iDynTree::toEigen(hand.transform.getRotation().asRPY()); + iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(3) + = iDynTree::toEigen(hand.transform.getRotation().asRPY()); hand.smoother.smoother->init(hand.yarpReadBuffer); }; @@ -248,45 +312,61 @@ bool RetargetingClient::reset(WalkingFK& kinDynWrapper) // joint retargeting m_hdeRetargeting.joints.position = kinDynWrapper.getJointPos(); m_hdeRetargeting.joints.velocity.zero(); - iDynTree::toEigen(m_hdeRetargeting.joints.smoother.yarpBuffer) = iDynTree::toEigen(m_hdeRetargeting.joints.position); + iDynTree::toEigen(m_hdeRetargeting.joints.smoother.yarpBuffer) + = iDynTree::toEigen(m_hdeRetargeting.joints.position); if (m_useJointRetargeting) { - m_hdeRetargeting.joints.smoother.smoother->init(m_hdeRetargeting.joints.smoother.yarpBuffer); + m_hdeRetargeting.joints.smoother.smoother->init( + m_hdeRetargeting.joints.smoother.yarpBuffer); + } + m_comHorizontalRetargetingReady = false; + + m_hdeRetargeting.comHorizontal.position.zero(); + m_hdeRetargeting.comHorizontal.velocity.zero(); + iDynTree::toEigen(m_hdeRetargeting.comHorizontal.smoother.yarpBuffer) + = iDynTree::toEigen(m_hdeRetargeting.comHorizontal.position); + + if (m_useCoMHorizontalRetargeting) + { + m_hdeRetargeting.comHorizontal.smoother.smoother->init( + m_hdeRetargeting.comHorizontal.smoother.yarpBuffer); } - m_hdeRetargeting.com.position = kinDynWrapper.getCoMPosition()(2); - m_hdeRetargeting.com.velocity = 0; - m_comConstantHeight = m_hdeRetargeting.com.position; + m_hdeRetargeting.comHeight.position = kinDynWrapper.getCoMPosition()(2); + m_hdeRetargeting.comHeight.velocity = 0; + m_comConstantHeight = m_hdeRetargeting.comHeight.position; - if(m_useCoMHeightRetargeting) + if (m_useCoMHeightRetargeting) { - m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_hdeRetargeting.com.position; - m_hdeRetargeting.joints.smoother.smoother->init(m_hdeRetargeting.com.smoother.yarpBuffer); + m_hdeRetargeting.comHeight.smoother.yarpBuffer(0) = m_hdeRetargeting.comHeight.position; + m_hdeRetargeting.joints.smoother.smoother->init( + m_hdeRetargeting.comHeight.smoother.yarpBuffer); // let's read the port to reset the comHeightInput bool okCoMHeight = false; unsigned int attempt = 0; do { - if(!okCoMHeight) + if (!okCoMHeight) { auto data = m_hdeRetargeting.port.read(false); - if(data != nullptr) + if (data != nullptr) { m_comHeightInputOffset = data->CoMPositionWRTGlobal.z; okCoMHeight = true; } } - if(okCoMHeight) + if (okCoMHeight) return true; yarp::os::Time::delay(0.001); attempt++; } while (attempt < 100); - if(!okCoMHeight) - yError() << "[RetargetingClient::reset] The CoM height is not coming from the yarp port."; + if (!okCoMHeight) + yError() << "[RetargetingClient::reset] The CoM height is not coming from the yarp " + "port."; return false; } @@ -295,12 +375,11 @@ bool RetargetingClient::reset(WalkingFK& kinDynWrapper) bool RetargetingClient::getFeedback() { - if(m_useHandRetargeting) + if (m_useHandRetargeting) { - auto getHandFeedback = [this](HandRetargeting& hand) - { + auto getHandFeedback = [this](HandRetargeting& hand) { auto desiredHandPose = hand.port.read(false); - if(desiredHandPose != nullptr) + if (desiredHandPose != nullptr) { hand.smoother.smoother->computeNextValues(*desiredHandPose); } @@ -311,7 +390,7 @@ bool RetargetingClient::getFeedback() getHandFeedback(m_rightHand); } - if (m_useJointRetargeting || m_useCoMHeightRetargeting) + if (m_useJointRetargeting || m_useCoMHeightRetargeting || m_useCoMHorizontalRetargeting) { const auto HDEData = m_hdeRetargeting.port.read(false); if (HDEData != nullptr) @@ -320,16 +399,52 @@ bool RetargetingClient::getFeedback() { if (m_phase == Phase::Walking) { - m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_comConstantHeight; + m_hdeRetargeting.comHeight.smoother.yarpBuffer(0) = m_comConstantHeight; } else { const auto& desiredCoMHeight = HDEData->CoMPositionWRTGlobal.z; - m_hdeRetargeting.com.smoother.yarpBuffer(0) + m_hdeRetargeting.comHeight.smoother.yarpBuffer(0) = (desiredCoMHeight - m_comHeightInputOffset) * m_comHeightScalingFactor + m_comConstantHeight; } } + if (m_useCoMHorizontalRetargeting) + { + if (!m_comHorizontalRetargetingReady) + { + // the following line is valid only because in the human state provider the + // root link orientation is set constant + m_comHorizontalInputOffset[0] = HDEData->CoMPositionWRTGlobal.x; + m_comHorizontalInputOffset[1] = HDEData->CoMPositionWRTGlobal.y; + m_comHorizontalRetargetingReady = true; + } + + const auto& HDECoM = HDEData->CoMPositionWRTGlobal; + + if (m_phase == Phase::Walking) + { + m_hdeRetargeting.comHorizontal.smoother.yarpBuffer.zero(); + } else{ + // Phase::Approaching or Phase::Stance + m_hdeRetargeting.comHorizontal.smoother.yarpBuffer[0] + = std::max(m_comXLimits[0], + std::min(HDECoM.x - m_comHorizontalInputOffset[0], + m_comXLimits[1])); + + m_hdeRetargeting.comHorizontal.smoother.yarpBuffer[1] + = std::max(m_comYLimits[0], + std::min(HDECoM.y - m_comHorizontalInputOffset[1], + m_comYLimits[1])); + } + + for (const auto& [joint, index] : m_retargetedJointsToControlJoints) + { + m_hdeRetargeting.joints.smoother.yarpBuffer(index) + = HDEData->positions[m_retargetedJointsToHDEJoints[joint]]; + } + } + if (m_useJointRetargeting) { // the first time we should define the map between the retargeted joints and the @@ -339,7 +454,8 @@ bool RetargetingClient::getFeedback() const auto& HDEJointNames = HDEData->jointNames; for (const auto& [joint, index] : m_retargetedJointsToControlJoints) { - const auto element = std::find(HDEJointNames.begin(), HDEJointNames.end(), joint); + const auto element + = std::find(HDEJointNames.begin(), HDEJointNames.end(), joint); if (element == HDEJointNames.end()) { yError() << "[RetargetingClient::getFeedback] Unable to find the joint " @@ -347,14 +463,16 @@ bool RetargetingClient::getFeedback() << joint << " in the list of the HDE joints."; return false; } - m_retargetedJointsToHDEJoints[joint] = std::distance(HDEJointNames.begin(), element); + m_retargetedJointsToHDEJoints[joint] + = std::distance(HDEJointNames.begin(), element); } } const auto& HDEJoints = HDEData->positions; for (const auto& [joint, index] : m_retargetedJointsToControlJoints) { - m_hdeRetargeting.joints.smoother.yarpBuffer(index) = HDEData->positions[m_retargetedJointsToHDEJoints[joint]]; + m_hdeRetargeting.joints.smoother.yarpBuffer(index) + = HDEData->positions[m_retargetedJointsToHDEJoints[joint]]; } } } @@ -364,10 +482,22 @@ bool RetargetingClient::getFeedback() // generate a smoother trajectory if (m_useCoMHeightRetargeting) { - m_hdeRetargeting.com.smoother.smoother->computeNextValues( - m_hdeRetargeting.com.smoother.yarpBuffer); - m_hdeRetargeting.com.position = m_hdeRetargeting.com.smoother.smoother->getPos()(0); - m_hdeRetargeting.com.velocity = m_hdeRetargeting.com.smoother.smoother->getVel()(0); + m_hdeRetargeting.comHeight.smoother.smoother->computeNextValues( + m_hdeRetargeting.comHeight.smoother.yarpBuffer); + m_hdeRetargeting.comHeight.position + = m_hdeRetargeting.comHeight.smoother.smoother->getPos()(0); + m_hdeRetargeting.comHeight.velocity + = m_hdeRetargeting.comHeight.smoother.smoother->getVel()(0); + } + + if (m_useCoMHorizontalRetargeting) + { + m_hdeRetargeting.comHorizontal.smoother.smoother->computeNextValues( + m_hdeRetargeting.comHorizontal.smoother.yarpBuffer); + iDynTree::toEigen(m_hdeRetargeting.comHorizontal.position) + = iDynTree::toEigen(m_hdeRetargeting.comHorizontal.smoother.smoother->getPos()); + iDynTree::toEigen(m_hdeRetargeting.comHorizontal.velocity) + = iDynTree::toEigen(m_hdeRetargeting.comHorizontal.smoother.smoother->getVel()); } if (m_useJointRetargeting) @@ -381,10 +511,10 @@ bool RetargetingClient::getFeedback() } // check if the approaching phase is finished - if(m_phase == Phase::Approaching) + if (m_phase == Phase::Approaching) { double now = yarp::os::Time::now(); - if(now - m_startingApproachingPhaseTime > m_approachPhaseDuration) + if (now - m_startingApproachingPhaseTime > m_approachPhaseDuration) stopApproachingPhase(); } @@ -413,12 +543,17 @@ const iDynTree::VectorDynSize& RetargetingClient::jointVelocities() const double RetargetingClient::comHeight() const { - return m_hdeRetargeting.com.position; + return m_hdeRetargeting.comHeight.position; } double RetargetingClient::comHeightVelocity() const { - return m_hdeRetargeting.com.velocity; + return m_hdeRetargeting.comHeight.velocity; +} + +const iDynTree::Vector2& RetargetingClient::comHorizonal() const +{ + return m_hdeRetargeting.comHorizontal.position; } void RetargetingClient::close() @@ -429,7 +564,7 @@ void RetargetingClient::close() m_rightHand.port.close(); } - if (m_useJointRetargeting || m_useCoMHeightRetargeting) + if (m_useJointRetargeting || m_useCoMHeightRetargeting || m_useCoMHorizontalRetargeting) { m_hdeRetargeting.port.close(); } @@ -437,7 +572,7 @@ void RetargetingClient::close() void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotation) { - if(!m_useVirtualizer) + if (!m_useVirtualizer) return; yarp::sig::Vector& output = m_robotOrientationPort.prepare(); @@ -478,8 +613,14 @@ void RetargetingClient::stopApproachingPhase() if (m_useCoMHeightRetargeting) { - m_hdeRetargeting.com.smoother.smoother->setT( - m_hdeRetargeting.com.smoother.smoothingTimeInWalking); + m_hdeRetargeting.comHeight.smoother.smoother->setT( + m_hdeRetargeting.comHeight.smoother.smoothingTimeInWalking); + } + + if (m_useCoMHorizontalRetargeting) + { + m_hdeRetargeting.comHorizontal.smoother.smoother->setT( + m_hdeRetargeting.comHorizontal.smoother.smoothingTimeInWalking); } m_phase = Phase::Stance; @@ -488,12 +629,12 @@ void RetargetingClient::stopApproachingPhase() void RetargetingClient::startApproachingPhase() { // if the retargeting is not used the approaching phase is not required - if(!m_useHandRetargeting && !m_useJointRetargeting && !m_useCoMHeightRetargeting) + if (!m_useHandRetargeting && !m_useJointRetargeting && !m_useCoMHeightRetargeting) return; m_startingApproachingPhaseTime = yarp::os::Time::now(); - if(m_useHandRetargeting) + if (m_useHandRetargeting) { m_leftHand.smoother.smoother->setT(m_leftHand.smoother.smoothingTimeInApproaching); m_rightHand.smoother.smoother->setT(m_rightHand.smoother.smoothingTimeInApproaching); @@ -507,8 +648,14 @@ void RetargetingClient::startApproachingPhase() if (m_useCoMHeightRetargeting) { - m_hdeRetargeting.com.smoother.smoother->setT( - m_hdeRetargeting.com.smoother.smoothingTimeInApproaching); + m_hdeRetargeting.comHeight.smoother.smoother->setT( + m_hdeRetargeting.comHeight.smoother.smoothingTimeInApproaching); + } + + if (m_useCoMHorizontalRetargeting) + { + m_hdeRetargeting.comHorizontal.smoother.smoother->setT( + m_hdeRetargeting.comHorizontal.smoother.smoothingTimeInApproaching); } } From e2e1065d9ffaa6fd21873140003dc1cfaf8b2466 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 30 Sep 2022 13:02:35 +0200 Subject: [PATCH 2/4] Add the possibility to update the local ZMP delta in when update Trajectory is called --- .../TrajectoryPlanner/TrajectoryGenerator.h | 6 +++++- .../src/TrajectoryGenerator.cpp | 19 +++++++++++++------ 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 6ccd361c..3f0175a4 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -57,6 +57,9 @@ namespace WalkingControllers iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of the unicycle and the point that has to be reach the goal. */ + iDynTree::Vector2 m_leftZMPIntialDelta; + iDynTree::Vector2 m_rightZMPIntialDelta; + GeneratorState m_generatorState{GeneratorState::NotConfigured}; /**< Useful to track the generator state. */ std::thread m_generatorThread; /**< Main trajectory thread. */ @@ -135,7 +138,8 @@ namespace WalkingControllers */ bool updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft, - const iDynTree::Transform& measured, const iDynTree::VectorDynSize& plannerDesiredInput); + const iDynTree::Transform& measured, const iDynTree::VectorDynSize& plannerDesiredInput, + const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta); /** * @brief Set the free space ellipse in which the robot can walk. diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index 20da5de8..a888c8a6 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -12,7 +12,9 @@ // iDynTree #include +#include +// walking-controllers #include #include @@ -66,15 +68,13 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) } // get left and right ZMP delta - iDynTree::Vector2 leftZMPDelta; - if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", leftZMPDelta)) + if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", m_leftZMPIntialDelta)) { yError() << "[configurePlanner] Initialization failed while reading leftZMPDelta vector."; return false; } - iDynTree::Vector2 rightZMPDelta; - if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", rightZMPDelta)) + if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", m_rightZMPIntialDelta)) { yError() << "[configurePlanner] Initialization failed while reading rightZMPDelta vector."; return false; @@ -203,7 +203,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatios[0], mergePointRatios[1]); m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator(); - m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); + m_dcmGenerator->setFootOriginOffset(m_leftZMPIntialDelta, m_rightZMPIntialDelta); m_dcmGenerator->setOmega(sqrt(9.81/comHeight)); m_dcmGenerator->setFirstDCMTrajectoryMode(FirstDCMTrajectoryMode::FifthOrderPoly); ok = ok && m_dcmGenerator->setLastStepDCMOffsetPercentage(lastStepDCMOffset); @@ -565,7 +565,8 @@ bool TrajectoryGenerator::generateFirstTrajectories(const iDynTree::Transform &l bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft, - const iDynTree::Transform& measured, const iDynTree::VectorDynSize &plannerDesiredInput) + const iDynTree::Transform& measured, const iDynTree::VectorDynSize &plannerDesiredInput, + const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta) { { std::lock_guard guard(m_mutex); @@ -587,6 +588,12 @@ bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Ve m_personFollowingDesiredPoint.zero(); m_desiredDirectControl.zero(); + iDynTree::Vector2 leftZMPDelta = m_leftZMPIntialDelta; + iDynTree::Vector2 rightZMPDelta = m_rightZMPIntialDelta; + iDynTree::toEigen(leftZMPDelta) += iDynTree::toEigen(leftZMPOffsetDelta); + iDynTree::toEigen(rightZMPDelta) += iDynTree::toEigen(rightZMPOffsetDelta); + m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); + if (m_unicycleController == UnicycleController::PERSON_FOLLOWING) { if (plannerDesiredInput.size() < 2) From 9c34c9d764671cb0c948344d775b5cb053964d0f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 30 Sep 2022 13:09:51 +0200 Subject: [PATCH 3/4] Consider the CoM provided by the retrageting as offset of the walking controller --- .../WalkingControllers/WalkingModule/Module.h | 5 ++++- src/WalkingModule/src/Module.cpp | 14 ++++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 7f472d5e..3908570f 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -225,11 +225,14 @@ namespace WalkingControllers * @param measuredTransform transformation between the world and the (stance/swing??) foot; * @param mergePoint is the instant at which the old and the new trajectory will be merged; * @param plannerDesiredInput The desired input to the planner. + * @param leftZMPOffsetDelta the offset added to the delta ZMP on the left foot + * @param rightZMPOffsetDelta the offset added to the delta ZMP on the right foot * @return true/false in case of success/failure. */ bool askNewTrajectories(const double& initTime, const bool& isLeftSwinging, const iDynTree::Transform& measuredTransform, - const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput); + const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput, + const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta); /** * Update the old trajectory. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 085cbade..46582fa6 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -745,7 +745,6 @@ bool WalkingModule::updateModule() // when we are near to the merge point the new trajectory is evaluated if(m_newTrajectoryMergeCounter == m_plannerAdvanceTimeSteps) { - double initTimeTrajectory; initTimeTrajectory = m_time + m_newTrajectoryMergeCounter * m_dT; @@ -754,9 +753,13 @@ bool WalkingModule::updateModule() m_leftTrajectory[m_newTrajectoryMergeCounter]; // ask for a new trajectory + const iDynTree::Vector2& comHorizontal = m_retargetingClient->comHorizonal(); + iDynTree::Vector2 zmpOffset; + zmpOffset(0) = comHorizontal(0); + zmpOffset(1) = 0; if(!askNewTrajectories(initTimeTrajectory, !m_isLeftFixedFrame.front(), measuredTransform, m_newTrajectoryMergeCounter, - m_plannerInput)) + m_plannerInput, zmpOffset, zmpOffset)) { yError() << "[WalkingModule::updateModule] Unable to ask for a new trajectory."; return false; @@ -1071,6 +1074,7 @@ bool WalkingModule::updateModule() CoMPositionDesired[2] = m_retargetingClient->comHeight() + m_comHeightOffset; data.vectors["com::position::desired"].assign(CoMPositionDesired.begin(), CoMPositionDesired.begin() + CoMPositionDesired.size()); data.vectors["com::position::desired_macumba"].assign(desiredCoMPosition.data(), desiredCoMPosition.data() + desiredCoMPosition.size()); + data.vectors["com::position::desired_retargeting"].assign(m_retargetingClient->comHorizonal().data(), m_retargetingClient->comHorizonal().data() + m_retargetingClient->comHorizonal().size()); // Manual definition of this value to add also the planned CoM height velocity std::vector CoMVelocityDesired(3); @@ -1440,7 +1444,8 @@ bool WalkingModule::generateFirstTrajectories() bool WalkingModule::askNewTrajectories(const double& initTime, const bool& isLeftSwinging, const iDynTree::Transform& measuredTransform, - const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput) + const size_t& mergePoint, const iDynTree::VectorDynSize &plannerDesiredInput, + const iDynTree::Vector2& leftZMPOffsetDelta, const iDynTree::Vector2& rightZMPOffsetDelta) { if(m_trajectoryGenerator == nullptr) { @@ -1472,7 +1477,8 @@ bool WalkingModule::askNewTrajectories(const double& initTime, const bool& isLef if(!m_trajectoryGenerator->updateTrajectories(initTime, m_DCMPositionDesired[mergePoint], m_DCMVelocityDesired[mergePoint], isLeftSwinging, - measuredTransform, plannerDesiredInput)) + measuredTransform, plannerDesiredInput, + leftZMPOffsetDelta, rightZMPOffsetDelta)) { yError() << "[WalkingModule::askNewTrajectories] Unable to update the trajectory."; return false; From 4cf150d52e77252d5d564fd492ac5a1dd3ddb895 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 30 Sep 2022 13:11:54 +0200 Subject: [PATCH 4/4] =?UTF-8?q?=F0=9F=A4=96=20[iCubGenova09]=20Enable=20th?= =?UTF-8?q?e=20CoM=20horizontal=20retargeting=20when=20iFeel=20joint=20ret?= =?UTF-8?q?argeting=20is=20enable?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../iFeel_joint_retargeting/jointRetargeting.ini | 9 ++++++++- .../iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 4 +++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini index 810708c7..3bb0b60f 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini @@ -20,7 +20,14 @@ smoothing_time_walking 0.03 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o -[COM_RETARGETING] +[COM_HEIGHT_RETARGETING] smoothing_time_approaching 2.0 smoothing_time_walking 1.0 com_height_scaling_factor 0.5 + + +[COM_HORIZONTAL_RETARGETING] +smoothing_time_approaching 2.0 +smoothing_time_walking 0.5 +com_x_limits (-0.05, 0.05) +com_y_limits (0.0, 0.0) \ No newline at end of file diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini index 8eedaea4..7626ad60 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini @@ -52,7 +52,9 @@ sampling_time 0.01 use_joint_retargeting 1 # enable the virtualizer use_virtualizer 1 -use_com_retargeting 0 +use_com_height_retargeting 0 +use_com_horizontal_retargeting 1 + # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link height_reference_frame root_link