From dcc03f6833507931e188233a9e2b22cd0ff673a2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 28 Sep 2022 17:21:51 +0000 Subject: [PATCH 01/15] Increase the frequency of the walking-controller to 500Hz --- .../dcm_walking/joypad_control/tasks/regularization.ini | 2 +- .../iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini | 2 +- .../iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 2 +- .../app/robots/iCubGenova09/dcm_walking_with_joypad.ini | 5 +++-- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini index 87c2081f..4b75c831 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini @@ -6,7 +6,7 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("stance", "walking") -sampling_time 0.01 +sampling_time 0.002 settling_time 0.5 [stance] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini index 8258c0af..89f8654e 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini @@ -4,7 +4,7 @@ kp_angular 5.0 states ("stance", "walking") -sampling_time 0.01 +sampling_time 0.002 settling_time 0.5 [stance] 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..7e24d798 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 @@ -39,7 +39,7 @@ goal_port_suffix /goal:i goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.08 +planner_advance_time_in_s 0.02 # general parameters [GENERAL] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini index e13120db..8e8260c2 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -39,7 +39,7 @@ goal_port_suffix /goal:i goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.08 +planner_advance_time_in_s 0.02 # general parameters [GENERAL] @@ -47,10 +47,11 @@ name walking-coordinator # height of the com com_height 0.565 # sampling time -sampling_time 0.01 +sampling_time 0.002 # 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 + # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] From 6d6132fe1d1ec3520b60a8a749ad0fbda97280e8 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 4 Oct 2022 09:37:34 +0000 Subject: [PATCH 02/15] Increased ZMP counter since the frequency is higher --- .../robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 2 +- .../app/robots/iCubGenova09/dcm_walking_with_joypad.ini | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 7e24d798..788dc698 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 @@ -20,7 +20,7 @@ max_initial_com_vel 0.15 constant_ZMP_tolerance 0.000001 # Consecutive times in which the ZMP remains constant. If this limit is reached, the module stops. Use a negative value to disable. -constant_ZMP_counter 25 +constant_ZMP_counter 125 # Minimum force to consider the ZMP valid. minimum_normal_force_ZMP 1.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini index 8e8260c2..e627a179 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -20,7 +20,7 @@ max_initial_com_vel 0.15 constant_ZMP_tolerance 0.00001 # Consecutive times in which the ZMP remains constant. If this limit is reached, the module stops. Use a negative value to disable. -constant_ZMP_counter 25 +constant_ZMP_counter 125 # Minimum force to consider the ZMP valid. minimum_normal_force_ZMP 1.0 From 504a168e8faeb764d850f1ddf96508e6d1c0bc2d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 5 Oct 2022 12:49:15 +0200 Subject: [PATCH 03/15] Avoid to force the connection to the logger when the dump_data is enabled --- src/WalkingModule/src/Module.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 085cbade..ead2de7f 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -396,7 +396,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) { yarp::os::Bottle& loggerOptions = rf.findGroup("WALKING_LOGGER"); // open and connect the data logger port - std::string portInput, portOutput; + std::string portOutput; // open the connect the data logger port if(!YarpUtilities::getStringFromSearchable(loggerOptions, "dataLoggerOutputPort_name", @@ -405,21 +405,8 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yError() << "[WalkingModule::configure] Unable to get the string from searchable."; return false; } - if(!YarpUtilities::getStringFromSearchable(loggerOptions, - "dataLoggerInputPort_name", - portInput)) - { - yError() << "[WalkingModule::configure] Unable to get the string from searchable."; - return false; - } m_loggerPort.open("/" + name + portOutput); - - if(!yarp::os::Network::connect("/" + name + portOutput, portInput)) - { - yError() << "Unable to connect the ports " << "/" + name + portOutput << "and" << portInput; - return false; - } } // time profiler From 2706da69be1b225377748de762f5c6ffdc8431d3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 8 Oct 2022 18:06:42 +0200 Subject: [PATCH 04/15] Make the walking running at 500Hz while iFeel retargeting is enable --- .../iFeel_joint_retargeting/tasks/regularization.ini | 4 ++-- .../iFeel_joint_retargeting/tasks/retargeting.ini | 6 +++--- .../dcm_walking/iFeel_joint_retargeting/tasks/torso.ini | 4 ++-- .../iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index d6fffcac..6512f658 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -7,8 +7,8 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("STANCE", "WALKING") -sampling_time 0.01 -settling_time 4.5 +sampling_time 0.002 +settling_time 3.0 [STANCE] name "stance" diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 903728e5..1c4bd716 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -7,8 +7,8 @@ kp (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) states ("STANCE", "WALKING") -sampling_time 0.01 -settling_time 4.5 +sampling_time 0.002 +settling_time 3.0 [STANCE] name "stance" @@ -22,7 +22,7 @@ weight (2.0, 2.0, 2.0, [WALKING] name "walking" weight (2.0, 2.0, 2.0, - 1.0, 1.0, 1.0, + 0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini index e69638dd..01d7def3 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -4,8 +4,8 @@ kp_angular 5.0 states ("STANCE", "WALKING") -sampling_time 0.01 -settling_time 4.5 +sampling_time 0.002 +settling_time 3.0 [STANCE] name "stance" 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 788dc698..2ead6b99 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 @@ -47,7 +47,7 @@ name walking-coordinator # height of the com com_height 0.565 # sampling time -sampling_time 0.01 +sampling_time 0.002 # enable joint retargeting use_joint_retargeting 1 # enable the virtualizer From 401fc0682d1f61797f5cc4db7ed5597af49c582a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 6 Oct 2022 14:50:50 +0200 Subject: [PATCH 05/15] Stream the rotated FT --- .../WalkingControllers/WalkingModule/Module.h | 7 ++ src/WalkingModule/src/Module.cpp | 77 +++++++++++++++++++ 2 files changed, 84 insertions(+) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 7f472d5e..a8f810f8 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -153,6 +153,13 @@ namespace WalkingControllers yarp::os::BufferedPort m_loggerPort; /**< Logger port. */ + yarp::os::BufferedPort m_leftFTArmRotatedPort; /**< Desired robot position port. */ + yarp::os::BufferedPort m_rightFTArmRotatedPort; /**< Desired robot position port. */ + + yarp::os::BufferedPort m_leftFTArmPort; /**< Desired robot position port. */ + yarp::os::BufferedPort m_rightFTArmPort; /**< Desired robot position port. */ + + /** * Get the robot model from the resource finder and set it. * @param rf is the reference to a resource finder object. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index ead2de7f..4bd4ca7e 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -12,6 +12,8 @@ #include // YARP +#include +#include #include #include #include @@ -247,6 +249,41 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + ///////////////// TODO remove this code. DONOT merge + std::string leftFTArmRoatedPort = "/" + getName() + "/left_arm_inertial_ft"; + if(!m_leftFTArmRotatedPort.open(leftFTArmRoatedPort)) + { + yError() << "[WalkingModule::configure] Could not open" << leftFTArmRoatedPort << " port."; + return false; + } + + std::string rightFTArmRoatedPort = "/" + getName() + "/right_arm_inertial_ft"; + if(!m_rightFTArmRotatedPort.open(rightFTArmRoatedPort)) + { + yError() << "[WalkingModule::configure] Could not open" << rightFTArmRoatedPort << " port."; + return false; + } + + std::string leftFTArmPort = "/" + getName() + "/left_arm_filtered_ft"; + if(!m_leftFTArmPort.open(leftFTArmPort)) + { + yError() << "[WalkingModule::configure] Could not open" << leftFTArmPort << " port."; + return false; + } + + + std::string rightFTArmPort = "/" + getName() + "/right_arm_filtered_ft"; + if(!m_rightFTArmPort.open(rightFTArmPort)) + { + yError() << "[WalkingModule::configure] Could not open" << rightFTArmPort << " port."; + return false; + } + + + yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/l_arm_ft_sensor", leftFTArmPort); + yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/r_arm_ft_sensor", rightFTArmPort); + ///////////////// + // initialize the trajectory planner m_trajectoryGenerator = std::make_unique(); yarp::os::Bottle& trajectoryPlannerOptions = rf.findGroup("TRAJECTORY_PLANNER"); @@ -712,6 +749,46 @@ bool WalkingModule::updateModule() m_profiler->setInitTime("Total"); + ///////////////////////// TODO this part of the code should be removed. DONOT merge this feature + yarp::sig::Vector* leftFTData = m_leftFTArmPort.read(false); + yarp::sig::Vector& leftArmRotated = m_leftFTArmRotatedPort.prepare(); + leftArmRotated.resize(3); + if (leftFTData == nullptr) + { + yarp::eigen::toEigen(leftArmRotated).setZero(); + } + else { + iDynTree::Vector3 temp; + temp(0) = (*leftFTData)[0]; + temp(1) = (*leftFTData)[1]; + temp(2) = (*leftFTData)[2]; + + yarp::eigen::toEigen(leftArmRotated) = iDynTree::toEigen(m_FKSolver->getKinDyn()->getWorldTransform("l_arm_ft_sensor").getRotation()) * + iDynTree::toEigen(temp); + } + m_leftFTArmRotatedPort.write(); + + + yarp::sig::Vector* rightFTData = m_rightFTArmPort.read(false); + yarp::sig::Vector& rightArmRotated = m_rightFTArmRotatedPort.prepare(); + rightArmRotated.resize(3); + if (rightFTData == nullptr) + { + yarp::eigen::toEigen(rightArmRotated).setZero(); + } + else { + iDynTree::Vector3 temp; + temp(0) = (*rightFTData)[0]; + temp(1) = (*rightFTData)[1]; + temp(2) = (*rightFTData)[2]; + + yarp::eigen::toEigen(rightArmRotated) = iDynTree::toEigen(m_FKSolver->getKinDyn()->getWorldTransform("r_arm_ft_sensor").getRotation()) * + iDynTree::toEigen(temp); + } + m_rightFTArmRotatedPort.write(); + + ////////////////////////////////// + // check desired planner input yarp::sig::Vector* desiredUnicyclePosition = nullptr; desiredUnicyclePosition = m_desiredUnyciclePositionPort.read(false); From 049420b518064b3feb5ca3763bc1d226f42b2560 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 8 Oct 2022 11:03:39 +0200 Subject: [PATCH 06/15] Streams the left and right hand pose every 100ms --- .../WalkingControllers/WalkingModule/Module.h | 5 + src/WalkingModule/src/Module.cpp | 119 +++++++++++++----- 2 files changed, 91 insertions(+), 33 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index a8f810f8..17661c47 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -159,6 +159,11 @@ namespace WalkingControllers yarp::os::BufferedPort m_leftFTArmPort; /**< Desired robot position port. */ yarp::os::BufferedPort m_rightFTArmPort; /**< Desired robot position port. */ + yarp::os::BufferedPort m_rightHandPort; + yarp::os::BufferedPort m_leftHandPort; + + int m_sendInfoIndex{0}; + int m_sendInfoMaxCounter{0}; /** * Get the robot model from the resource finder and set it. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 4bd4ca7e..c9b4b0c4 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -196,6 +197,9 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + // we will send the data every 100ms + m_sendInfoMaxCounter = std::floor(0.01 / m_dT); + double plannerAdvanceTimeInS = rf.check("planner_advance_time_in_s", yarp::os::Value(0.18)).asFloat64(); m_plannerAdvanceTimeSteps = std::round(plannerAdvanceTimeInS / m_dT) + 2; //The additional 2 steps are because the trajectory from the planner is requested two steps in advance wrt the merge point @@ -282,6 +286,21 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/l_arm_ft_sensor", leftFTArmPort); yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/r_arm_ft_sensor", rightFTArmPort); + + + std::string leftHandPort = "/" + getName() + "/left_hand:o"; + if(!m_leftHandPort.open(leftHandPort)) + { + yError() << "[WalkingModule::configure] Could not open" << leftHandPort << " port."; + return false; + } + + std::string rightHandPort = "/" + getName() + "/right_hand:o"; + if(!m_rightHandPort.open(rightHandPort)) + { + yError() << "[WalkingModule::configure] Could not open" << rightHandPort << " port."; + return false; + } ///////////////// // initialize the trajectory planner @@ -749,43 +768,77 @@ bool WalkingModule::updateModule() m_profiler->setInitTime("Total"); - ///////////////////////// TODO this part of the code should be removed. DONOT merge this feature - yarp::sig::Vector* leftFTData = m_leftFTArmPort.read(false); - yarp::sig::Vector& leftArmRotated = m_leftFTArmRotatedPort.prepare(); - leftArmRotated.resize(3); - if (leftFTData == nullptr) + ///////////////////////// TODO this part of the code should be removed. DONOT merge this + ///feature + if (m_sendInfoIndex == 0) { - yarp::eigen::toEigen(leftArmRotated).setZero(); - } - else { - iDynTree::Vector3 temp; - temp(0) = (*leftFTData)[0]; - temp(1) = (*leftFTData)[1]; - temp(2) = (*leftFTData)[2]; - - yarp::eigen::toEigen(leftArmRotated) = iDynTree::toEigen(m_FKSolver->getKinDyn()->getWorldTransform("l_arm_ft_sensor").getRotation()) * - iDynTree::toEigen(temp); - } - m_leftFTArmRotatedPort.write(); + yarp::sig::Vector* leftFTData = m_leftFTArmPort.read(false); + yarp::sig::Vector& leftArmRotated = m_leftFTArmRotatedPort.prepare(); + leftArmRotated.resize(3); + if (leftFTData == nullptr) + { + yarp::eigen::toEigen(leftArmRotated).setZero(); + } else + { + iDynTree::Vector3 temp; + temp(0) = (*leftFTData)[0]; + temp(1) = (*leftFTData)[1]; + temp(2) = (*leftFTData)[2]; + + yarp::eigen::toEigen(leftArmRotated) + = iDynTree::toEigen(m_FKSolver->getKinDyn() + ->getWorldTransform("l_arm_ft_sensor") + .getRotation()) + * iDynTree::toEigen(temp); + } + m_leftFTArmRotatedPort.write(); + yarp::sig::Vector* rightFTData = m_rightFTArmPort.read(false); + yarp::sig::Vector& rightArmRotated = m_rightFTArmRotatedPort.prepare(); + rightArmRotated.resize(3); + if (rightFTData == nullptr) + { + yarp::eigen::toEigen(rightArmRotated).setZero(); + } else + { + iDynTree::Vector3 temp; + temp(0) = (*rightFTData)[0]; + temp(1) = (*rightFTData)[1]; + temp(2) = (*rightFTData)[2]; + + yarp::eigen::toEigen(rightArmRotated) + = iDynTree::toEigen(m_FKSolver->getKinDyn() + ->getWorldTransform("r_arm_ft_sensor") + .getRotation()) + * iDynTree::toEigen(temp); + } + m_rightFTArmRotatedPort.write(); - yarp::sig::Vector* rightFTData = m_rightFTArmPort.read(false); - yarp::sig::Vector& rightArmRotated = m_rightFTArmRotatedPort.prepare(); - rightArmRotated.resize(3); - if (rightFTData == nullptr) - { - yarp::eigen::toEigen(rightArmRotated).setZero(); - } - else { - iDynTree::Vector3 temp; - temp(0) = (*rightFTData)[0]; - temp(1) = (*rightFTData)[1]; - temp(2) = (*rightFTData)[2]; - - yarp::eigen::toEigen(rightArmRotated) = iDynTree::toEigen(m_FKSolver->getKinDyn()->getWorldTransform("r_arm_ft_sensor").getRotation()) * - iDynTree::toEigen(temp); + const iDynTree::Transform head_T_left_hand + = m_FKSolver->getHeadToWorldTransform().inverse() + * m_FKSolver->getLeftHandToWorldTransform(); + + const iDynTree::Transform head_T_right_hand + = m_FKSolver->getHeadToWorldTransform().inverse() + * m_FKSolver->getRightHandToWorldTransform(); + + yarp::sig::Matrix& matrixLeft = m_leftHandPort.prepare(); + matrixLeft.resize(4, 4); + iDynTree::toYarp(head_T_left_hand, matrixLeft); + m_leftHandPort.write(); + + yarp::sig::Matrix& matrixRight = m_rightHandPort.prepare(); + matrixRight.resize(4, 4); + iDynTree::toYarp(head_T_right_hand, matrixRight); + m_rightHandPort.write(); + + // send the data every m_sendInfoMaxCounter + m_sendInfoIndex++; + if (m_sendInfoIndex > m_sendInfoMaxCounter) + { + m_sendInfoIndex = 0; + } } - m_rightFTArmRotatedPort.write(); ////////////////////////////////// From 3f0fcc94e44e10ee7143dd5bee6cb18c9e574e72 Mon Sep 17 00:00:00 2001 From: icub Date: Sat, 8 Oct 2022 21:19:49 +0000 Subject: [PATCH 07/15] Fix bug --- src/WalkingModule/src/Module.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index c9b4b0c4..9bc5663e 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -831,15 +831,15 @@ bool WalkingModule::updateModule() matrixRight.resize(4, 4); iDynTree::toYarp(head_T_right_hand, matrixRight); m_rightHandPort.write(); - - // send the data every m_sendInfoMaxCounter - m_sendInfoIndex++; - if (m_sendInfoIndex > m_sendInfoMaxCounter) - { - m_sendInfoIndex = 0; - } } + // send the data every m_sendInfoMaxCounter + m_sendInfoIndex++; + if (m_sendInfoIndex > m_sendInfoMaxCounter) + { + m_sendInfoIndex = 0; + } + ////////////////////////////////// // check desired planner input From 452a7f44aa3a1bf9af1bd307b43870b7690bf3ef Mon Sep 17 00:00:00 2001 From: icub Date: Sat, 8 Oct 2022 21:21:02 +0000 Subject: [PATCH 08/15] Avoid to use tcp in the connection of the walking controller and wbd --- src/RobotInterface/src/Helper.cpp | 2 +- src/WalkingModule/src/Module.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index ce145155..1784227f 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -494,7 +494,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, "fast_tcp")) { yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port " << wholeBodyDynamicsPortName << " to " << "/" + portPrefix + portInputName; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 9bc5663e..b08291a1 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -284,8 +284,8 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) } - yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/l_arm_ft_sensor", leftFTArmPort); - yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/r_arm_ft_sensor", rightFTArmPort); + yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/l_arm_ft_sensor", leftFTArmPort, "fast_tcp"); + yarp::os::Network::connect("/wholeBodyDynamics/filteredFT/r_arm_ft_sensor", rightFTArmPort, "fast_tcp"); std::string leftHandPort = "/" + getName() + "/left_hand:o"; From d0a6fc0f28ba45bf3b53e012fdad0a570f8e537f Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 9 Oct 2022 15:21:16 +0200 Subject: [PATCH 09/15] Added possibility to change the timeout behaviors to get feedback --- .../RobotInterface/Helper.h | 6 +++--- src/RobotInterface/src/Helper.cpp | 12 +++++------ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../dcm_walking_with_joypad.ini | 3 +++ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGazeboV3/dcm_walking_retargeting.ini | 3 +++ .../iCubGazeboV3/dcm_walking_with_joypad.ini | 3 +++ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_iFeel_joint_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGenova04/dcm_walking_with_joypad.ini | 3 +++ .../dcm_walking_hand_retargeting.ini | 3 +++ .../dcm_walking_iFeel_joint_retargeting.ini | 3 +++ .../dcm_walking_joint_retargeting.ini | 3 +++ .../iCubGenova09/dcm_walking_with_joypad.ini | 3 +++ .../icubGazeboSim/dcm_walking_with_joypad.ini | 3 +++ .../WalkingControllers/WalkingModule/Module.h | 3 +++ src/WalkingModule/src/Module.cpp | 20 +++++++++++-------- 20 files changed, 72 insertions(+), 17 deletions(-) diff --git a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h index ece43cd5..e9de8089 100644 --- a/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h +++ b/src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h @@ -156,9 +156,9 @@ namespace WalkingControllers * Get all the feedback signal from the interfaces * @return true in case of success and false otherwise. */ - bool getFeedbacks(unsigned int maxAttempts = 1); + bool getFeedbacks(unsigned int maxAttempts, double attemptDelay); - bool getFeedbacksRaw(unsigned int maxAttempts = 1); + bool getFeedbacksRaw(unsigned int maxAttempts, double attemptDelay); /** * Set the desired position reference. (The position will be sent using PositionControl mode) @@ -190,7 +190,7 @@ namespace WalkingControllers * Reset filters. * @return true in case of success and false otherwise. */ - bool resetFilters(); + bool resetFilters(unsigned int maxAttempts, double attemptDelay); /** * Close the polydrives. diff --git a/src/RobotInterface/src/Helper.cpp b/src/RobotInterface/src/Helper.cpp index 1784227f..670b6652 100644 --- a/src/RobotInterface/src/Helper.cpp +++ b/src/RobotInterface/src/Helper.cpp @@ -47,7 +47,7 @@ bool RobotInterface::getWorstError(const iDynTree::VectorDynSize& desiredJointPo return true; } -bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts) +bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts, double attemptDelay) { if(!m_encodersInterface) { @@ -187,7 +187,7 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts) return true; } - yarp::os::Time::delay(0.001); + yarp::os::Time::delay(attemptDelay); attempt++; } while (attempt < maxAttempts); @@ -665,9 +665,9 @@ bool RobotInterface::configurePIDHandler(const yarp::os::Bottle& config) } -bool RobotInterface::resetFilters() +bool RobotInterface::resetFilters(unsigned int maxAttempts, double attemptDelay) { - if(!getFeedbacksRaw(100)) + if(!getFeedbacksRaw(maxAttempts, attemptDelay)) { yError() << "[RobotInterface::resetFilters] Unable to get the feedback from the robot"; return false; @@ -710,9 +710,9 @@ bool RobotInterface::resetFilters() return true; } -bool RobotInterface::getFeedbacks(unsigned int maxAttempts) +bool RobotInterface::getFeedbacks(unsigned int maxAttempts, double attemptDelay) { - if(!getFeedbacksRaw(maxAttempts)) + if(!getFeedbacksRaw(maxAttempts, attemptDelay)) { yError() << "[RobotInterface::getFeedbacks] Unable to get the feedback from the robot"; return false; diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini index e11360fd..c5866054 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini index 57325148..068d7291 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini @@ -41,6 +41,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini index 33fb3781..df5315c3 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini index 71585596..81d155d2 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini index 0d83596d..3469cfb9 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_joint_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini index 1339e82f..ecd35678 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini index 625b0257..a1fb750d 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini @@ -41,6 +41,9 @@ goal_port_scaling (1.0, 1.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini index cd34af55..40694056 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini index 49d9c55d..1c578204 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_iFeel_joint_retargeting.ini @@ -23,6 +23,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini index 52157eac..0d94a993 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_joint_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini index d71c289d..0405e8b3 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking_with_joypad.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini index 1228cfb9..abf20497 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_hand_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator 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 2ead6b99..74989199 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 @@ -41,6 +41,9 @@ goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.02 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini index 42949a07..32a4ed6e 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_joint_retargeting.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini index e627a179..86edcbe0 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -41,6 +41,9 @@ goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.02 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini index 98d01c2f..8d657207 100644 --- a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking_with_joypad.ini @@ -39,6 +39,9 @@ goal_port_scaling (10.0, 10.0, 1.0) # How much in advance the planner should be called. The time is in seconds planner_advance_time_in_s 0.18 +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + # general parameters [GENERAL] name walking-coordinator diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 17661c47..42ff77c1 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -148,6 +148,9 @@ namespace WalkingControllers size_t m_plannerAdvanceTimeSteps; /** How many steps in advance the planner should be called. */ + size_t m_feedbackAttempts; + double m_feedbackAttemptDelay; + // debug std::unique_ptr m_velocityIntegral{nullptr}; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index b08291a1..60da3cb3 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -197,6 +197,10 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + double maxFBDelay = rf.check("max_feedback_delay_in_s", yarp::os::Value(1.0)).asFloat64(); + m_feedbackAttemptDelay = m_dT / 10; + m_feedbackAttempts = maxFBDelay / m_feedbackAttemptDelay; + // we will send the data every 100ms m_sendInfoMaxCounter = std::floor(0.01 / m_dT); @@ -661,7 +665,7 @@ bool WalkingModule::updateModule() if(m_robotState == WalkingFSM::Preparing) { - if(!m_robotControlHelper->getFeedbacksRaw(100)) + if(!m_robotControlHelper->getFeedbacksRaw(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[updateModule] Unable to get the feedback."; return false; @@ -706,7 +710,7 @@ bool WalkingModule::updateModule() m_stableDCMModel->reset(m_DCMPositionDesired.front()); // reset the retargeting - if(!m_robotControlHelper->getFeedbacks(100)) + if(!m_robotControlHelper->getFeedbacks(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[WalkingModule::updateModule] Unable to get the feedback."; return false; @@ -833,13 +837,13 @@ bool WalkingModule::updateModule() m_rightHandPort.write(); } - // send the data every m_sendInfoMaxCounter - m_sendInfoIndex++; + // send the data every m_sendInfoMaxCounter + m_sendInfoIndex++; if (m_sendInfoIndex > m_sendInfoMaxCounter) { m_sendInfoIndex = 0; } - + ////////////////////////////////// // check desired planner input @@ -904,7 +908,7 @@ bool WalkingModule::updateModule() } // get feedbacks and evaluate useful quantities - if(!m_robotControlHelper->getFeedbacks(100)) + if(!m_robotControlHelper->getFeedbacks(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[WalkingModule::updateModule] Unable to get the feedback."; return false; @@ -1386,7 +1390,7 @@ bool WalkingModule::prepareRobot(bool onTheFly) // get the current state of the robot // this is necessary because the trajectories for the joints, CoM height and neck orientation // depend on the current state of the robot - if(!m_robotControlHelper->getFeedbacksRaw(100)) + if(!m_robotControlHelper->getFeedbacksRaw(m_feedbackAttempts, m_feedbackAttemptDelay)) { yError() << "[WalkingModule::prepareRobot] Unable to get the feedback."; return false; @@ -1712,7 +1716,7 @@ bool WalkingModule::startWalking() // if the robot was only prepared the filters has to be reseted if(m_robotState == WalkingFSM::Prepared) { - m_robotControlHelper->resetFilters(); + m_robotControlHelper->resetFilters(m_feedbackAttempts, m_feedbackAttemptDelay); updateFKSolver(); From cefa9a0b15b379a3bccb5ba8f04a549ef63078e3 Mon Sep 17 00:00:00 2001 From: icub Date: Thu, 3 Nov 2022 19:04:55 +0000 Subject: [PATCH 10/15] Latest before qualification --- .../iCubGenova09/dcm_walking/common/plannerParams.ini | 4 ++-- .../iFeel_joint_retargeting/tasks/regularization.ini | 6 +++--- .../iFeel_joint_retargeting/tasks/retargeting.ini | 8 ++++++-- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 3e1104fd..3422110c 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -10,7 +10,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 2.0 -slowWhenBackwardFactor 1.0 +slowWhenBackwardFactor 0.8 slowWhenSidewaysFactor 0.2 # Conservative factors that multiply the unicycle velocity saturations @@ -21,7 +21,7 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length -maxStepLength 0.27 +maxStepLength 0.25 minStepLength 0.05 #Width minWidth 0.12 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 6512f658..570cf280 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -22,8 +22,8 @@ weight (0.0, 0.0, 0.0, [WALKING] name "walking" weight (0.0, 0.0, 0.0, - 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 2.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, 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/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 1c4bd716..d9ac8224 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -23,7 +23,11 @@ weight (2.0, 2.0, 2.0, name "walking" weight (2.0, 2.0, 2.0, 0.0, 0.0, 0.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, - 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, + 0.0, 0.0, 0.0, 2.0, 2.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) + + +# 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, +# 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, \ No newline at end of file From 304c6b67b738eacfa51e2b4a1c49363313ebf1e3 Mon Sep 17 00:00:00 2001 From: icub Date: Thu, 3 Nov 2022 23:51:12 +0000 Subject: [PATCH 11/15] Increase low level pid gains --- .../dcm_walking/common/pidParams.ini | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini index 8891b89d..ae37d599 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini @@ -15,18 +15,18 @@ useGainScheduling 0 [DEFAULT] #NAME P I D #torso_pitch 5.0 1.0 0.5 -l_knee -30000.0 -1000.0 0.0 -r_knee 30000.0 1000.0 0.0 +l_knee -33000.0 -1000.0 0.0 +r_knee 33000.0 1000.0 0.0 l_ankle_pitch -15000.0 -1000.0 0.0 r_ankle_pitch 15000.0 1000.0 0.0 -l_ankle_roll -8000.0 -1000.0 0.0 -r_ankle_roll 8000.0 1000.0 0.0 -l_hip_pitch -10000.0 -1500.0 0.0 -r_hip_pitch 10000.0 1500.0 0.0 -l_hip_roll 25000.0 1000.0 0.0 -r_hip_roll -25000.0 -1000.0 0.0 -r_hip_yaw 10000.0 2000.0 0.0 -l_hip_yaw -7000.0 -2000.0 0.0 +l_ankle_roll -10000.0 -1000.0 0.0 +r_ankle_roll 10000.0 1000.0 0.0 +l_hip_pitch -11000.0 -1500.0 0.0 +r_hip_pitch 11000.0 1500.0 0.0 +l_hip_roll 27500.0 1000.0 0.0 +r_hip_roll -27500.0 -1000.0 0.0 +r_hip_yaw 11000.0 2000.0 0.0 +l_hip_yaw -11000.0 -2000.0 0.0 #r_shoulder_pitch -10000.0 -10000.0 0.0 #r_shoulder_roll -10000.0 -10000.0 0.0 r_shoulder_yaw -3000.0 -1500.0 0.0 From cde85257414585baae477721c986de316f63d0d5 Mon Sep 17 00:00:00 2001 From: icub Date: Thu, 3 Nov 2022 23:52:58 +0000 Subject: [PATCH 12/15] decrease zmp gain --- .../iCubGenova09/dcm_walking/common/zmpControllerParams.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini index 4477491d..1153d6fc 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini @@ -4,7 +4,7 @@ useGainScheduling 1 smoothingTime 0.05 # if the gain scheduling is off only k*_walking parameters are used -kZMP_walking 2.0 +kZMP_walking 1.8 # kZMP_stance 1.0 kCoM_walking 5.0 From b33ec27777ad0bab0c11a880c027cd384e533ed9 Mon Sep 17 00:00:00 2001 From: icub Date: Thu, 3 Nov 2022 23:53:18 +0000 Subject: [PATCH 13/15] play with the planner params --- .../iCubGenova09/dcm_walking/common/plannerParams.ini | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 3422110c..009b56c1 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -47,8 +47,8 @@ lastStepSwitchTime 0.8 switchOverSwingRatio 0.3 #ZMP Delta -leftZMPDelta (0.01 -0.0) -rightZMPDelta (0.01 0.01) +leftZMPDelta (0.005 -0.0) +rightZMPDelta (0.005 0.0) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 @@ -77,4 +77,4 @@ startAlwaysSameFoot 1 # useMinimumJerkFootTrajectory 1 ##Remove this line if you want to enable the pause conditon -isPauseActive 0 +isPauseActive 1 From 67d814b4e39e76f020c77c91d47438a0be0f5dd5 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 5 Nov 2022 22:03:00 +0000 Subject: [PATCH 14/15] Tune the planner parameters --- .../robots/iCubGenova09/dcm_walking/common/plannerParams.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini index 009b56c1..d3bf9f1a 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -22,7 +22,7 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length maxStepLength 0.25 -minStepLength 0.05 +minStepLength 0.01 #Width minWidth 0.12 #Angle Variations in DEGREES From 18370c904727fe5cc6d1dd0f8963daa460c4d04b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 5 Nov 2022 22:04:10 +0000 Subject: [PATCH 15/15] Set the rgularization of the IK while walking --- .../tasks/regularization.ini | 14 +++++++++++--- .../tasks/retargeting.ini | 18 ++++++++++++------ 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 570cf280..ea2f5860 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -8,7 +8,7 @@ kp (5.0, 5.0, 5.0, states ("STANCE", "WALKING") sampling_time 0.002 -settling_time 3.0 +settling_time 5.0 [STANCE] name "stance" @@ -21,9 +21,17 @@ weight (0.0, 0.0, 0.0, [WALKING] name "walking" +# weight (0.0, 0.0, 0.0, +# 2.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, +# 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, +# 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + weight (0.0, 0.0, 0.0, 2.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, + 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, 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/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index d9ac8224..d12adc66 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -8,7 +8,7 @@ kp (5.0, 5.0, 5.0, states ("STANCE", "WALKING") sampling_time 0.002 -settling_time 3.0 +settling_time 5.0 [STANCE] name "stance" @@ -21,13 +21,19 @@ weight (2.0, 2.0, 2.0, [WALKING] name "walking" +# weight (2.0, 2.0, 2.0, +# 0.0, 0.0, 0.0, +# 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, +# 0.0, 0.0, 0.0, 2.0, 2.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) + + weight (2.0, 2.0, 2.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, - 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 2.0, 0.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 2.0, 0.0, 2.0, 2.0, 2.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) -# 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, -# 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, \ No newline at end of file