From 794c018b164d526997dcc5acd29264ffd6b03fc2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 4 Jun 2021 12:25:04 +0200 Subject: [PATCH 1/7] Added configuration files to use iFeel with iCubGenova09. --- .../inverseKinematics.ini | 27 +++++++ .../jointRetargeting.ini | 27 +++++++ .../qpInverseKinematics.ini | 71 ++++++++++++++++ .../iFeel_joint_retargeting/robotControl.ini | 33 ++++++++ .../dcm_walking_iFeel_joint_retargeting.ini | 80 +++++++++++++++++++ 5 files changed, 238 insertions(+) create mode 100644 src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini create mode 100644 src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini new file mode 100644 index 00000000..c235f13c --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini @@ -0,0 +1,27 @@ +#The left_foot_frame is supposed to have the same orientation of the Inertia frame, which have the z axis pointing upwards, +#the x axis pointing forward and and the y concludes a right-handed frame +left_foot_frame l_sole +right_foot_frame r_sole + +#Remove the following line in order not to consider the +#additional frame +additional_frame neck_2 + +#The additional rotation is defined (by rows) in the Inertia frame. +#Remove the following line to keep the identity as additional rotation +additional_rotation ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0)) + +# solver paramenters +solver-verbosity 0 +# solver_name ma27 +max-cpu-time 20 + +#DEGREES +jointRegularization (7, 0.12, -0.01, + -3.7, 25.0, -13.0, 40.769, 0, 0, 0, + -3.7, 25.0, -13.0, 40.769, 0, 0, 0, + 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, + 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) + +#jointRegularization (0, 15, 0, 0, -29.794, 29.97, 0.00, 44.977, -29.794, 29.97, 0.00, 44.977, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) +#jointRegularization (15, 0, 0, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351, 5.082, 0.406, -0.131, -45.249, -26.454, -0.351) 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 new file mode 100644 index 00000000..3aa28936 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini @@ -0,0 +1,27 @@ +approaching_phase_duration 4.0 + +[HAND_RETARGETING] + + +[JOINT_RETARGETING] +## List of the retargeting joint. This list must be the same or a subset of the +## "joints_list" in robotControl.ini. The order of the joints should be choseen +## accordingly to the order of the joints received in the +## "joint_retargeting_port_name" port +retargeting_joint_list ("torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") + + +joint_retargeting_port_name /jointPosition:i +smoothing_time_approaching 2.0 +smoothing_time_walking 0.2 + +[VIRTUALIZER] +robot_orientation_port_name /torsoYaw:o + +[COM_RETARGETING] +com_height_retargeting_port_name /CoM:i +smoothing_time_approaching 2.0 +smoothing_time_walking 1.0 +com_height_scaling_factor 0.5 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini new file mode 100644 index 00000000..25997a51 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini @@ -0,0 +1,71 @@ +# CoM +use_com_as_constraint 1 + +# torso +torso_weight_walking 5.0 +torso_weight_stance 0.0 +additional_rotation ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0)) + +# joint regularization values in degrees +joint_regularization (7, 0.12, -0.01, + -3.7, 25.0, -13.0, 40.769, 0, 0, 0, + -3.7, 25.0, -13.0, 40.769, 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) + +joint_regularization_gains (5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +joint_regularization_weight_stance (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.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) + +joint_regularization_weight_walking (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, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +## Retargeting +joint_retargeting_gains (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 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +joint_retargeting_weight_walking (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, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +joint_retargeting_weight_stance (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, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + + +#Gains +k_posCom 1.0 +k_posFoot 7.0 +k_attFoot 5.0 +k_neck 5.0 + +# Hand retargeting +k_posHand 2.5 +k_attHand 1.0 + +# use gain scheduling for hand retargeting +smoothing_time 0.5 + +# joint limits +k_joint_limit_lower_bound 1.0 +k_joint_limit_upper_bound 1.0 + +# use_joint_limits_constraint 1 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini new file mode 100644 index 00000000..8dfb696d --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini @@ -0,0 +1,33 @@ +robot icub + + +joints_list ("torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + +remote_control_boards ("torso", "left_arm", "right_arm", "left_leg", "right_leg") + +# filters +# if use_*_filter is equal to 0 the low pass filters are not used +use_joint_velocity_filter 0 +joint_velocity_cut_frequency 10.0 + +use_wrench_filter 0 +wrench_cut_frequency 10.0 + + +# if true the joint is in stiff mode if false the joint is in compliant mode +joint_is_stiff_mode (true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) + +# if true a good joint traking is considered mandatory +good_tracking_required (true, true, true, + true, true, true, true, false, false, false, + true, true, true, true, false, false, false, + true, true, true, true, true, true, + true, true, true, true, true, true) 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 new file mode 100644 index 00000000..26128b5b --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini @@ -0,0 +1,80 @@ +# Remove this line if you don't want to use the MPC +# use_mpc 1 + +# Remove this line if you don't want to use the QP-IK +use_QP-IK 1 + +# Remove this line if you don't want to use osqp to +# solve QP-IK. In this case qpOASES will be used +use_osqp 1 + +# remove this line if you don't want to save data of the experiment +#dump_data 1 + +# Limit on the maximum initial velocity. This avoids the robot to jump at startup +max_initial_com_vel 0.1 + +# Tolerance radius to consider the measured ZMP constant. +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 + +# Minimum force to consider the ZMP valid. +minimum_normal_force_ZMP 1.0 + +# Limit to consider the ZMP valid +# |x| |y| +maximum_local_zmp (0.3, 0.2) + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.53 +# sampling time +sampling_time 0.01 +# enable joint retargeting +use_joint_retargeting 1 +# enable the virtualizer +use_virtualizer 1 +use_com_retargeting 1 + +# include robot control parameters +[include ROBOT_CONTROL "./dcm_walking/iFeel_joint_retargeting/robotControl.ini"] + +# include trajectory planner parameters +[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.ini"] + +# include free space ellipsoid manager parameters +[include FREE_SPACE_ELLIPSE_MANAGER "./dcm_walking/common/freeSpaceEllipseParams.ini"] + +# include MPC parameters +[include DCM_MPC_CONTROLLER "./dcm_walking/common/controllerParams.ini"] + +# include MPC parameters +[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"] + +# include MPC parameters +[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"] + +# include inverse kinematcs parameters +[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini"] + +# include qp inverse kinematcs parameters +[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini"] + +# include inverse kinematcs parameters +[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] + +# include FT sensors parameters +[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"] + +# include Logger parameters +[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"] + +# include lower PID parameters +[include PID "./dcm_walking/common/pidParams.ini"] + +# retargeting +[include RETARGETING "./dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini"] From f11ab561182e9660a17a5b9daf5d74483d930ac3 Mon Sep 17 00:00:00 2001 From: icub Date: Mon, 7 Jun 2021 15:49:03 +0200 Subject: [PATCH 2/7] Avoid to retarget the torso when using iFeel on iCub3. --- .../iFeel_joint_retargeting/qpInverseKinematics.ini | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini index 25997a51..82ce4fad 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini @@ -3,7 +3,7 @@ use_com_as_constraint 1 # torso torso_weight_walking 5.0 -torso_weight_stance 0.0 +torso_weight_stance 5.0 additional_rotation ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0)) # joint regularization values in degrees @@ -19,7 +19,7 @@ joint_regularization_gains (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) -joint_regularization_weight_stance (0.0, 0.0, 0.0, +joint_regularization_weight_stance (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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -44,7 +44,7 @@ joint_retargeting_weight_walking (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) -joint_retargeting_weight_stance (1.0, 1.0, 1.0, +joint_retargeting_weight_stance (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, From db43b97e3e80c0f3fc982c3f953507e5dc9a2c2b Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Jun 2021 16:57:45 +0200 Subject: [PATCH 3/7] Using the feetYawDelta and the slowWhenBackwardFactor. --- cmake/WalkingControllersFindDependencies.cmake | 2 +- src/TrajectoryPlanner/src/TrajectoryGenerator.cpp | 11 ++++++++++- .../dcm_walking/common/plannerParams.ini | 5 +++++ .../iCubGazeboV3/dcm_walking/common/plannerParams.ini | 5 +++++ .../dcm_walking/hand_retargeting/plannerParams.ini | 7 ++++++- .../iFeel_joint_retargeting/plannerParams.ini | 5 +++++ .../dcm_walking/joint_retargeting/plannerParams.ini | 5 +++++ .../dcm_walking/joypad_control/plannerParams.ini | 5 +++++ .../iCubGenova09/dcm_walking/common/plannerParams.ini | 5 +++++ .../dcm_walking/common/plannerParams.ini | 5 +++++ 10 files changed, 52 insertions(+), 3 deletions(-) diff --git a/cmake/WalkingControllersFindDependencies.cmake b/cmake/WalkingControllersFindDependencies.cmake index 205d123b..a49735cf 100644 --- a/cmake/WalkingControllersFindDependencies.cmake +++ b/cmake/WalkingControllersFindDependencies.cmake @@ -134,7 +134,7 @@ checkandset_dependency(iDynTree) find_package(Eigen3 3.2.92 QUIET) checkandset_dependency(Eigen3) -find_package(UnicyclePlanner 0.3.0 QUIET) +find_package(UnicyclePlanner 0.3.101 QUIET) checkandset_dependency(UnicyclePlanner) find_package(osqp QUIET) diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index ef18c1b9..247eeeb6 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -83,6 +83,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) double timeWeight = config.check("timeWeight", yarp::os::Value(2.5)).asDouble(); double positionWeight = config.check("positionWeight", yarp::os::Value(1.0)).asDouble(); double slowWhenTurningGain = config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble(); + double slowWhenBackwardFactor = config.check("slowWhenBackwardFactor", yarp::os::Value(1.0)).asDouble(); double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asDouble(); double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asDouble(); double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble(); @@ -103,6 +104,8 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) yarp::os::Value(0.4)).asDouble(); double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asDouble(); + double leftYawDeltaInDeg = config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asDouble(); + double rightYawDeltaInDeg = config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asDouble(); m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); @@ -129,6 +132,9 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation); ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); + ok = ok && unicyclePlanner->setSlowWhenBackwardFactor(slowWhenBackwardFactor); + unicyclePlanner->setLeftFootYawOffsetInRadians(iDynTree::deg2rad(leftYawDeltaInDeg)); + unicyclePlanner->setRightFootYawOffsetInRadians(iDynTree::deg2rad(rightYawDeltaInDeg)); unicyclePlanner->addTerminalStep(true); unicyclePlanner->startWithLeft(m_swingLeft); unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); @@ -489,7 +495,10 @@ bool TrajectoryGenerator::generateFirstTrajectories(const iDynTree::Transform &l return false; } - m_generatorState = GeneratorState::Returned; + { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Returned; + } return true; } diff --git a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini index e32e45a3..457c4851 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking/common/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -39,6 +40,10 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + # Last Step DCM Offset # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini index a9da6869..b45f279d 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -39,6 +40,10 @@ switchOverSwingRatio 1.0 leftZMPDelta (-0.015 -0.01) rightZMPDelta (-0.015 0.01) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + # Last Step DCM Offset # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini index 64911aa3..9514f6cc 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/hand_retargeting/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -19,7 +20,7 @@ minWidth 0.15 maxAngleVariation 20.0 minAngleVariation 8.0 #Timings -maxStepDuration 0.95 +maxStepDuration 0.95 minStepDuration 0.80 ##Nominal Values @@ -39,6 +40,10 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + # Last Step DCM Offset # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/plannerParams.ini index 1d908ec3..fe60a21f 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/iFeel_joint_retargeting/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -39,6 +40,10 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + #MergePoint mergePointRatio 0.4 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini index 1d908ec3..fe60a21f 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joint_retargeting/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -39,6 +40,10 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + #MergePoint mergePointRatio 0.4 diff --git a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini index 61fb0535..d1eb5116 100644 --- a/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova04/dcm_walking/joypad_control/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -39,6 +40,10 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.03 -0.005) rightZMPDelta (0.03 0.005) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + # Last Step DCM Offset # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; 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 6aeadaab..d9636f2c 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -39,6 +40,10 @@ switchOverSwingRatio 0.7 leftZMPDelta (0.01 -0.01) rightZMPDelta (0.01 -0.0) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + # Last Step DCM Offset # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; diff --git a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/plannerParams.ini index 984f22aa..7dd2ebf2 100644 --- a/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/icubGazeboSim/dcm_walking/common/plannerParams.ini @@ -7,6 +7,7 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 0.1 slowWhenTurningGain 1.4 +slowWhenBackwardFactor 1.0 ##Bounds #Step length @@ -38,6 +39,10 @@ switchOverSwingRatio 0.4 leftZMPDelta (0.02 0.0) rightZMPDelta (0.02 0.0) +#Feet cartesian offset on the yaw +leftYawDeltaInDeg 0.0 +rightYawDeltaInDeg 0.0 + # Last Step DCM Offset # If it is 0.5 the final DCM will be in the middle of the two footsteps; # If it is 0 the DCM position coincides with the stance foot ZMP; From 9851bac9d2beb9138beef1d0420881514fdcda0b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 10 Jun 2021 09:44:11 +0200 Subject: [PATCH 4/7] Update the configuration files for iCubGenova09 --- .../dcm_walking/common/pidParams.ini | 4 ++-- .../dcm_walking/common/plannerParams.ini | 18 +++++++++--------- .../dcm_walking/common/zmpControllerParams.ini | 6 +++--- .../joypad_control/qpInverseKinematics.ini | 2 +- .../dcm_walking_iFeel_joint_retargeting.ini | 8 ++++---- .../iCubGenova09/dcm_walking_with_joypad.ini | 6 +++--- 6 files changed, 22 insertions(+), 22 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 fc7940b7..d2564294 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini @@ -25,5 +25,5 @@ l_hip_pitch -5000.0 -1500.0 0.0 r_hip_pitch 5000.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 5000.0 2000.0 0.0 -l_hip_yaw -5000.0 -2000.0 0.0 +r_hip_yaw 10000.0 2000.0 0.0 +l_hip_yaw -7000.0 -2000.0 0.0 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 d9636f2c..bfbb62f3 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/plannerParams.ini @@ -7,21 +7,21 @@ referencePosition (0.1 0.0) timeWeight 2.5 positionWeight 1.0 slowWhenTurningGain 5.0 -slowWhenBackwardFactor 1.0 +slowWhenBackwardFactor 0.5 ##Bounds #Step length -maxStepLength 0.25 +maxStepLength 0.22 minStepLength 0.05 #Width -minWidth 0.14 +minWidth 0.16 #Angle Variations in DEGREES #maxAngleVariation 12.0 -maxAngleVariation 22.0 +maxAngleVariation 20.0 minAngleVariation 8.0 #Timings -maxStepDuration 1.8 -minStepDuration 1.4 +maxStepDuration 1.9 +minStepDuration 1.6 ##Nominal Values #Width @@ -32,12 +32,12 @@ stepLandingVelocity 0.0 footApexTime 0.5 comHeightDelta 0.01 #Timings -nominalDuration 1.6 +nominalDuration 1.8 lastStepSwitchTime 0.8 -switchOverSwingRatio 0.7 +switchOverSwingRatio 1.0 #ZMP Delta -leftZMPDelta (0.01 -0.01) +leftZMPDelta (0.01 -0.0) rightZMPDelta (0.01 -0.0) #Feet cartesian offset on the yaw 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 2e2ed827..8b95326f 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini @@ -4,11 +4,11 @@ useGainScheduling 1 smoothingTime 0.05 # if the gain scheduling is off only k*_walking parameters are used -kZMP_walking 0.7 +kZMP_walking 1.5 # kZMP_stance 1.0 -kCoM_walking 4.0 +kCoM_walking 5.0 -kZMP_stance 0.5 +kZMP_stance 1.0 # kZMP_stance 1.0 kCoM_stance 4.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini index 3b6d9da7..7b906033 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematics.ini @@ -26,7 +26,7 @@ joint_regularization_gains (5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) # Gains -k_posCom 1.0 +k_posCom 2.0 k_posFoot 7.0 k_attFoot 5.0 k_neck 5.0 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 26128b5b..daec3a78 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 @@ -9,13 +9,13 @@ use_QP-IK 1 use_osqp 1 # remove this line if you don't want to save data of the experiment -#dump_data 1 +# dump_data 1 # Limit on the maximum initial velocity. This avoids the robot to jump at startup -max_initial_com_vel 0.1 +max_initial_com_vel 0.15 # Tolerance radius to consider the measured ZMP constant. -constant_ZMP_tolerance 0.00001 +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 @@ -25,7 +25,7 @@ minimum_normal_force_ZMP 1.0 # Limit to consider the ZMP valid # |x| |y| -maximum_local_zmp (0.3, 0.2) +maximum_local_zmp (0.35, 0.25) # 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 f225f010..275de93b 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -9,10 +9,10 @@ use_QP-IK 1 use_osqp 1 # remove this line if you don't want to save data of the experiment -# dump_data 1 +dump_data 1 # Limit on the maximum initial velocity. This avoids the robot to jump at startup -max_initial_com_vel 0.1 +max_initial_com_vel 0.15 # Tolerance radius to consider the measured ZMP constant. constant_ZMP_tolerance 0.00001 @@ -25,7 +25,7 @@ minimum_normal_force_ZMP 1.0 # Limit to consider the ZMP valid # |x| |y| -maximum_local_zmp (0.3, 0.2) +maximum_local_zmp (0.35, 0.25) # general parameters [GENERAL] From 85cdd95ad79c079e4443b714d417b28f62702dc3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 10 Jun 2021 10:16:05 +0200 Subject: [PATCH 5/7] Enable the logging for iFeel demo --- src/WalkingModule/src/Module.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 5ba0f73b..83710449 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -893,7 +893,8 @@ bool WalkingModule::updateModule() m_leftTrajectory.front().getPosition(), m_leftTrajectory.front().getRotation().asRPY(), m_rightTrajectory.front().getPosition(), m_rightTrajectory.front().getRotation().asRPY(), m_robotControlHelper->getJointPosition(), - m_qDesired); + m_qDesired, + m_retargetingClient->jointValues()); } // in the approaching phase the robot should not move and the trajectories should not advance @@ -1367,15 +1368,20 @@ bool WalkingModule::startWalking() "rf_des_x", "rf_des_y", "rf_des_z", "rf_des_roll", "rf_des_pitch", "rf_des_yaw", "torso_pitch", "torso_roll", "torso_yaw", - "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", - "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll", "torso_pitch_des", "torso_roll_des", "torso_yaw_des", - "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des", - "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des", + "l_shoulder_pitch_des", "l_shoulder_roll_des", "l_shoulder_yaw_des", "l_elbow_des", "l_wrist_prosup_des", "l_wrist_pitch_des", "l_wrist_yaw_des", + "r_shoulder_pitch_des", "r_shoulder_roll_des", "r_shoulder_yaw_des", "r_elbow_des", "r_wrist_prosup_des", "r_wrist_pitch_des", "r_wrist_yaw_des", "l_hip_pitch_des", "l_hip_roll_des", "l_hip_yaw_des", "l_knee_des", "l_ankle_pitch_des", "l_ankle_roll_des", - "r_hip_pitch_des", "r_hip_roll_des", "r_hip_yaw_des", "r_knee_des", "r_ankle_pitch_des", "r_ankle_roll_des"}); + "r_hip_pitch_des", "r_hip_roll_des", "r_hip_yaw_des", "r_knee_des", "r_ankle_pitch_des", "r_ankle_roll_des", + "torso_pitch_ret", "torso_roll_ret", "torso_yaw_ret", + "l_shoulder_pitch_ret", "l_shoulder_roll_ret", "l_shoulder_yaw_ret", "l_elbow_ret", "l_wrist_prosup_ret", "l_wrist_pitch_ret", "l_wrist_yaw_ret", + "r_shoulder_pitch_ret", "r_shoulder_roll_ret", "r_shoulder_yaw_ret", "r_elbow_ret", "r_wrist_prosup_ret", "r_wrist_pitch_ret", "r_wrist_yaw_ret", + "l_hip_pitch_ret", "l_hip_roll_ret", "l_hip_yaw_ret", "l_knee_ret", "l_ankle_pitch_ret", "l_ankle_roll_ret", + "r_hip_pitch_ret", "r_hip_roll_ret", "r_hip_yaw_ret", "r_knee_ret", "r_ankle_pitch_ret", "r_ankle_roll_ret"}); } // if the robot was only prepared the filters has to be reseted From 4faddbc471675507ee599e02f46a389fc31137bd Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 14 Jun 2021 18:12:03 +0200 Subject: [PATCH 6/7] Enable the compliant mode in the iFeel walking --- .../iCubGenova09/dcm_walking/common/zmpControllerParams.ini | 2 +- .../dcm_walking/iFeel_joint_retargeting/robotControl.ini | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 8b95326f..1c6c0faa 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/zmpControllerParams.ini @@ -8,7 +8,7 @@ kZMP_walking 1.5 # kZMP_stance 1.0 kCoM_walking 5.0 -kZMP_stance 1.0 +kZMP_stance 0.5 # kZMP_stance 1.0 kCoM_stance 4.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini index 8dfb696d..828af0c7 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/robotControl.ini @@ -20,8 +20,8 @@ wrench_cut_frequency 10.0 # if true the joint is in stiff mode if false the joint is in compliant mode joint_is_stiff_mode (true, true, true, - true, true, true, true, true, true, true, - true, true, true, true, true, true, true, + false, false, false, true, true, true, true, + false, false, false, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true) From 86792c8d570590593c9bd3223fcfa773170f3816 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 13 Jul 2021 18:49:34 +0200 Subject: [PATCH 7/7] Moved the logic to compute things in the unicycle frame in a single place. It also considers the feet yaw delta. --- .../TrajectoryPlanner/TrajectoryGenerator.h | 2 + .../src/TrajectoryGenerator.cpp | 124 ++++++++---------- 2 files changed, 56 insertions(+), 70 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index af137a25..1a6f6849 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -49,6 +49,8 @@ namespace WalkingControllers double m_plannerHorizon; /**< Horizon of the planner. */ std::size_t m_stancePhaseDelay; /**< Delay in ticks of the beginning of the stance phase. */ + double m_leftYawDeltaInRad; /**< Offset for the left foot rotation around the z axis. */ + double m_rightYawDeltaInRad; /**< Offset for the right foot rotation around the z axis. */ double m_nominalWidth; /**< Nominal width between two feet. */ double m_initTime; /**< Init time of the current trajectory. */ diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index 247eeeb6..0ed2ca5b 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -104,8 +104,8 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) yarp::os::Value(0.4)).asDouble(); double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); double lastStepDCMOffset = config.check("lastStepDCMOffset", yarp::os::Value(0.0)).asDouble(); - double leftYawDeltaInDeg = config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asDouble(); - double rightYawDeltaInDeg = config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asDouble(); + m_leftYawDeltaInRad = iDynTree::deg2rad(config.check("leftYawDeltaInDeg", yarp::os::Value(0.0)).asDouble()); + m_rightYawDeltaInRad = iDynTree::deg2rad(config.check("rightYawDeltaInDeg", yarp::os::Value(0.0)).asDouble()); m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); @@ -133,8 +133,8 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); ok = ok && unicyclePlanner->setSlowWhenBackwardFactor(slowWhenBackwardFactor); - unicyclePlanner->setLeftFootYawOffsetInRadians(iDynTree::deg2rad(leftYawDeltaInDeg)); - unicyclePlanner->setRightFootYawOffsetInRadians(iDynTree::deg2rad(rightYawDeltaInDeg)); + unicyclePlanner->setLeftFootYawOffsetInRadians(m_leftYawDeltaInRad); + unicyclePlanner->setRightFootYawOffsetInRadians(m_rightYawDeltaInRad); unicyclePlanner->addTerminalStep(true); unicyclePlanner->startWithLeft(m_swingLeft); unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); @@ -198,9 +198,10 @@ void TrajectoryGenerator::computeThread() bool correctLeft; - iDynTree::Vector2 desiredPoint; + iDynTree::Vector2 desiredPointInRelativeFrame, desiredPointInAbsoluteFrame; iDynTree::Vector2 measuredPositionLeft, measuredPositionRight; double measuredAngleLeft, measuredAngleRight; + double leftYawDeltaInRad, rightYawDeltaInRad; iDynTree::Vector2 DCMBoundaryConditionAtMergePointPosition; iDynTree::Vector2 DCMBoundaryConditionAtMergePointVelocity; @@ -223,7 +224,7 @@ void TrajectoryGenerator::computeThread() endTime = initTime + m_plannerHorizon; // set desired point - desiredPoint = m_desiredPoint; + desiredPointInRelativeFrame = m_desiredPoint; // dcm boundary conditions DCMBoundaryConditionAtMergePointPosition = m_DCMBoundaryConditionAtMergePointPosition; @@ -233,11 +234,13 @@ void TrajectoryGenerator::computeThread() measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0); measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1); measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2); + leftYawDeltaInRad = m_leftYawDeltaInRad; // right foot measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0); measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1); measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2); + rightYawDeltaInRad = m_rightYawDeltaInRad; correctLeft = m_correctLeft; @@ -252,12 +255,55 @@ void TrajectoryGenerator::computeThread() } } + iDynTree::Vector2 measuredPosition; + double measuredAngle; + measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight; + measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; + + DCMInitialState initialState; + initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition; + initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity; + + Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition; + unicyclePositionFromStanceFoot(0) = 0.0; + + Eigen::Matrix2d unicycleRotation; + double unicycleAngle; + + if (correctLeft) + { + unicyclePositionFromStanceFoot(1) = -nominalWidth/2; + unicycleAngle = measuredAngleLeft - leftYawDeltaInRad; + footPosition = iDynTree::toEigen(measuredPositionLeft); + } + else + { + unicyclePositionFromStanceFoot(1) = nominalWidth/2; + unicycleAngle = measuredAngleRight - rightYawDeltaInRad; + footPosition = iDynTree::toEigen(measuredPositionRight); + } + + double s_theta = std::sin(unicycleAngle); + double c_theta = std::cos(unicycleAngle); + + unicycleRotation(0,0) = c_theta; + unicycleRotation(0,1) = -s_theta; + unicycleRotation(1,0) = s_theta; + unicycleRotation(1,1) = c_theta; + + unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot + footPosition; + + // apply the homogeneous transformation w_H_{unicycle} + iDynTree::toEigen(desiredPointInAbsoluteFrame) = unicycleRotation * (iDynTree::toEigen(m_referencePointDistance) + + iDynTree::toEigen(desiredPointInRelativeFrame)) + + unicyclePosition; + // clear the old trajectory std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); unicyclePlanner->clearDesiredTrajectory(); // add new point - if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPoint)) + if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPointInAbsoluteFrame)) { // something goes wrong std::lock_guard guard(m_mutex); @@ -266,15 +312,6 @@ void TrajectoryGenerator::computeThread() break; } - iDynTree::Vector2 measuredPosition; - double measuredAngle; - measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight; - measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; - - DCMInitialState initialState; - initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition; - initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity; - if (!m_dcmGenerator->setDCMInitialState(initialState)) { // something goes wrong std::lock_guard guard(m_mutex); @@ -288,35 +325,6 @@ void TrajectoryGenerator::computeThread() iDynTree::MatrixFixSize<2,2> ellipseImage = freeSpaceEllipse.imageMatrix(), newEllipseImage; iDynTree::VectorFixSize<2> centerOffset = freeSpaceEllipse.centerOffset(), newCenterOffset; - Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition; - unicyclePositionFromStanceFoot(0) = 0.0; - - Eigen::Matrix2d unicycleRotation; - double theta; - - if (correctLeft) - { - unicyclePositionFromStanceFoot(1) = -nominalWidth/2; - theta = measuredAngleLeft; - footPosition = iDynTree::toEigen(measuredPositionLeft); - } - else - { - unicyclePositionFromStanceFoot(1) = nominalWidth/2; - theta = measuredAngleRight; - footPosition = iDynTree::toEigen(measuredPositionRight); - } - - double s_theta = std::sin(theta); - double c_theta = std::cos(theta); - - unicycleRotation(0,0) = c_theta; - unicycleRotation(0,1) = -s_theta; - unicycleRotation(1,0) = s_theta; - unicycleRotation(1,1) = c_theta; - - unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot + footPosition; - iDynTree::toEigen(newEllipseImage) = unicycleRotation * iDynTree::toEigen(ellipseImage); iDynTree::toEigen(newCenterOffset) = unicycleRotation * iDynTree::toEigen(centerOffset) + unicyclePosition; @@ -522,32 +530,8 @@ bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Ve << "Please call 'generateFirstTrajectories()' method."; return false; } - } - // if correctLeft is true the stance foot is the true. - // The vector (expressed in the unicycle reference frame from the left foot to the center of the - // unicycle is [0, width/2]') - iDynTree::Vector2 unicyclePositionFromStanceFoot; - unicyclePositionFromStanceFoot(0) = 0.0; - unicyclePositionFromStanceFoot(1) = correctLeft ? -m_nominalWidth/2 : m_nominalWidth/2; - iDynTree::Vector2 desredPositionFromStanceFoot; - iDynTree::toEigen(desredPositionFromStanceFoot) = iDynTree::toEigen(unicyclePositionFromStanceFoot) - + iDynTree::toEigen(m_referencePointDistance) + iDynTree::toEigen(desiredPosition); - - // prepare the rotation matrix w_R_{unicycle} - double theta = measured.getRotation().asRPY()(2); - double s_theta = std::sin(theta); - double c_theta = std::cos(theta); - - // save the data - { - std::lock_guard guard(m_mutex); - - // apply the homogeneous transformation w_H_{unicycle} - m_desiredPoint(0) = c_theta * desredPositionFromStanceFoot(0) - - s_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(0); - m_desiredPoint(1) = s_theta * desredPositionFromStanceFoot(0) - + c_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(1); + m_desiredPoint = desiredPosition; m_initTime = initTime;