From e3a4b2de59adefa441328f75a708b5282de9ee7c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 5 Feb 2024 17:41:02 +0100 Subject: [PATCH 1/4] Attempt to minimize the angular momentum on the z component --- .../dcm_walking/common/plannerParams.ini | 10 ++++----- .../joypad_control/inverseKinematics.ini | 4 ++-- .../joypad_control/qpInverseKinematicsBlf.ini | 5 +++++ .../joypad_control/tasks/regularization.ini | 22 +++++++++---------- .../joypad_control/tasks/torso.ini | 6 ++--- .../dcm_walking_with_joypad.ini | 4 ++-- .../WholeBodyControllers/BLFIK.h | 2 ++ src/WholeBodyControllers/src/BLFIK.cpp | 21 ++++++++++++++++++ 8 files changed, 51 insertions(+), 23 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini index d051cabad..6cd61f953 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini @@ -21,7 +21,7 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length -maxStepLength 0.25 +maxStepLength 0.34 minStepLength 0.01 maxLengthBackwardFactor 1.0 #Width @@ -32,7 +32,7 @@ maxAngleVariation 18.0 minAngleVariation 5.0 #Timings maxStepDuration 1.31 -minStepDuration 0.7 +minStepDuration 0.5 ##Nominal Values #Width @@ -45,11 +45,11 @@ comHeightDelta 0.01 #Timings nominalDuration 1.3 lastStepSwitchTime 0.8 -switchOverSwingRatio 0.3 +switchOverSwingRatio 0.1 #ZMP Delta -leftZMPDelta (0.005 -0.0) -rightZMPDelta (0.005 0.0) +leftZMPDelta (0.0 -0.0) +rightZMPDelta (0.0 0.0) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini index be0243b00..4f74c63cd 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/inverseKinematics.ini @@ -26,8 +26,8 @@ max-cpu-time 20 #DEGREES jointRegularization (7, 0.12, -0.01, - -3.7, 20.0, -13.0, 40.769, - -3.7, 20.0, -13.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) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 70c48fa81..f843d25da 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -24,5 +24,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 (2.0) + [include TORSO_TASK "./tasks/torso.ini"] [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..73efbf070 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,9 +1,9 @@ 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, - 5.0, 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, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) states ("stance", "walking") sampling_time 0.002 @@ -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/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..59047e651 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/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 50.0 states ("stance", "walking") @@ -9,8 +9,8 @@ settling_time 0.5 [stance] name "stance" -weight (5.0, 5.0, 5.0) +weight (15.0, 15.0, 1.0) [walking] name "walking" -weight (5.0, 5.0, 5.0) +weight (15.0, 15.0, 1.0) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini index b6fe27b7b..07e521441 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking_with_joypad.ini @@ -17,7 +17,7 @@ skip_dcm_controller 1 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 +planner_advance_time_in_s 0.08 # How much time (in seconds) before failing due to missing feedback max_feedback_delay_in_s 1.0 @@ -29,7 +29,7 @@ remove_zmp_offset 0 [GENERAL] name walking-coordinator # height of the com -com_height 0.7 +com_height 0.715 # sampling time sampling_time 0.01 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index 1d19809ba..1b2215512 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ 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}; diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index 93dfcaf65..86b971c35 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -102,6 +102,24 @@ bool BLFIK::initialize( lowPriority, m_jointRegularizationWeight); + 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_jointRetargetingTask = std::make_shared(); @@ -124,6 +142,9 @@ bool BLFIK::initialize( ok = ok && m_qpIK.finalize(m_variableHandler); + m_angularMomentumTask->setSetPoint(Eigen::Vector3d::Zero()); + + BipedalLocomotion::log()->info("[BLFIK::initialize] {}", m_qpIK.toString()); m_jointVelocity.resize(kinDyn->getNrOfDegreesOfFreedom()); From 5b01ea6e290f47a385db7fc21b43ae61545f84e3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Feb 2024 11:50:19 +0100 Subject: [PATCH 2/4] Use teh torso orientation velocity to generate a desired angular momentum --- .../dcm_walking/common/plannerParams.ini | 4 ++-- .../WalkingControllers/WalkingModule/Module.h | 1 + src/WalkingModule/src/Module.cpp | 16 ++++++++++++++++ .../WholeBodyControllers/BLFIK.h | 3 +++ src/WholeBodyControllers/src/BLFIK.cpp | 16 +++++++++++++--- 5 files changed, 35 insertions(+), 5 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini index 6cd61f953..f3633cb06 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1_1/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1_1/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 1.0 #Width @@ -32,7 +32,7 @@ maxAngleVariation 18.0 minAngleVariation 5.0 #Timings maxStepDuration 1.31 -minStepDuration 0.5 +minStepDuration 0.7 ##Nominal Values #Width diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 16d4ea0e8..d881c9d77 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -184,6 +184,7 @@ namespace WalkingControllers bool solveBLFIK(const iDynTree::Position& desiredCoMPosition, const iDynTree::Vector3& desiredCoMVelocity, const iDynTree::Rotation& desiredNeckOrientation, + const iDynTree::SpatialMomentum& desiredCentroidalMomentum, iDynTree::VectorDynSize &output); /** diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 929424fea..56d3a78f0 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -371,6 +371,9 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf) return false; } + m_vectorsCollectionServer.populateMetadata("angular_momentum::measured", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("angular_momentum::desired", {"x", "y", "z"}); + m_vectorsCollectionServer.populateMetadata("dcm::position::measured", {"x", "y"}); m_vectorsCollectionServer.populateMetadata("dcm::position::desired", {"x", "y"}); m_vectorsCollectionServer.populateMetadata("dcm::velocity::desired", {"x", "y"}); @@ -502,6 +505,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"; @@ -516,6 +520,8 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position &desiredCoMPosition, 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); @@ -902,6 +908,10 @@ bool WalkingModule::updateModule() yawRotation = yawRotation.inverse(); modifiedInertial = yawRotation * m_inertial_R_worldFrame; + // compute the desired torso velocity + auto torsoVelocity = m_BLFIKSolver->getDesiredTorsoVelocity(); + auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * torsoVelocity; + if (m_useQPIK) { // integrate dq because velocity control mode seems not available @@ -919,6 +929,7 @@ bool WalkingModule::updateModule() if (!solveBLFIK(desiredCoMPosition, desiredCoMVelocity, yawRotation, + centroidalMomentumDesired, m_dqDesired)) { yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with " @@ -1023,6 +1034,8 @@ bool WalkingModule::updateModule() CoMVelocityDesired[1] = m_stableDCMModel->getCoMVelocity().data()[1]; CoMVelocityDesired[2] = m_retargetingClient->comHeightVelocity(); + m_vectorsCollectionServer.prepareData(); + m_vectorsCollectionServer.clearData(); m_vectorsCollectionServer.populateData("com::velocity::desired", CoMVelocityDesired); // Left foot position @@ -1083,6 +1096,9 @@ bool WalkingModule::updateModule() const double isLeftFootFixed = m_isLeftFixedFrame.front() ? 1.0 : 0.0; m_vectorsCollectionServer.populateData("stance_foot::is_left", std::array{isLeftFootFixed}); + m_vectorsCollectionServer.populateData("angular_momentum::measured", m_FKSolver->getKinDyn()->getCentroidalTotalMomentum().getAngularVec3()); + m_vectorsCollectionServer.populateData("angular_momentum::desired", centroidalMomentumDesired.getAngularVec3()); + m_vectorsCollectionServer.sendData(); } diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index 1b2215512..605e75cae 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -49,8 +49,11 @@ class BLFIK bool setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity); bool setRootSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity); bool setTorsoSetPoint(const iDynTree::Rotation& rotation); + bool setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum); const iDynTree::VectorDynSize& getDesiredJointVelocity() const; + iDynTree::Twist getDesiredTorsoVelocity() const; + private: std::shared_ptr m_torsoWeight; diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index 86b971c35..9577d8fc7 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -142,9 +142,6 @@ bool BLFIK::initialize( ok = ok && m_qpIK.finalize(m_variableHandler); - m_angularMomentumTask->setSetPoint(Eigen::Vector3d::Zero()); - - BipedalLocomotion::log()->info("[BLFIK::initialize] {}", m_qpIK.toString()); m_jointVelocity.resize(kinDyn->getNrOfDegreesOfFreedom()); @@ -221,6 +218,11 @@ bool BLFIK::setRegularizationJointSetPoint(const iDynTree::VectorDynSize& jointP return m_jointRegularizationTask->setSetPoint(iDynTree::toEigen(jointPosition)); } +bool BLFIK::setAngularMomentumSetPoint(const iDynTree::Vector3& angularMomentum) +{ + return m_angularMomentumTask->setSetPoint(iDynTree::toEigen(angularMomentum)); +} + bool BLFIK::setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity) { return m_comTask->setSetPoint(iDynTree::toEigen(position), iDynTree::toEigen(velocity)); @@ -244,3 +246,11 @@ 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; +} \ No newline at end of file From 778e76f9d39dc0cfed8a2516faf1874f6d88c9e2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sun, 11 Feb 2024 16:49:27 +0100 Subject: [PATCH 3/4] Use the average of the feet velocity as desired centroidal locked velocity --- .../WalkingControllers/WalkingModule/Module.h | 2 ++ src/WalkingModule/src/Module.cpp | 19 +++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index d881c9d77..19fa78af1 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -200,6 +200,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 56d3a78f0..8ba27a950 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -909,8 +909,8 @@ bool WalkingModule::updateModule() modifiedInertial = yawRotation * m_inertial_R_worldFrame; // compute the desired torso velocity - auto torsoVelocity = m_BLFIKSolver->getDesiredTorsoVelocity(); - auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * torsoVelocity; + const iDynTree::Twist desiredTorsoVelocity = this->computeAverageTwistFromPlannedFeet(); + auto centroidalMomentumDesired = m_FKSolver->getKinDyn()->getCentroidalRobotLockedInertia() * desiredTorsoVelocity; if (m_useQPIK) { @@ -1140,6 +1140,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) From cde8c90c2fd6283517487b43b70c77cab40ce386 Mon Sep 17 00:00:00 2001 From: ergocub Date: Fri, 15 Mar 2024 11:47:33 +0100 Subject: [PATCH 4/4] [ergoCubSN001] Add configuration to add the angular momentum control --- .../joypad_control/inverseKinematics.ini | 4 ++-- .../joypad_control/qpInverseKinematicsBlf.ini | 5 +++++ .../joypad_control/tasks/regularization.ini | 18 +++++++++--------- .../dcm_walking/joypad_control/tasks/torso.ini | 6 +++--- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini index ef948ede3..3f55907cd 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/inverseKinematics.ini @@ -32,8 +32,8 @@ max-cpu-time 20 # 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) diff --git a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 82acb692e..f874bfed6 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -24,5 +24,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/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c8316..05aa894ca 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/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/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini index 89f8654e2..040298f3c 100644 --- a/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking/joypad_control/tasks/torso.ini +++ b/src/WalkingModule/app/robots/ergoCubSN001/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)