From 41547ea4f3a6f4e58bc9db057bee56bc28c8563a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 23 Sep 2024 10:35:05 +0200 Subject: [PATCH 1/2] [iRonCub-Mk1] Add the configuration files for the walking controller --- .../dcm_walking/common/controllerParams.ini | 10 +++ .../common/dcmReactiveControllerParams.ini | 1 + .../dcm_walking/common/forceTorqueSensors.ini | 6 ++ .../dcm_walking/common/forwardKinematics.ini | 18 ++++ .../common/freeSpaceEllipseParams.ini | 34 +++++++ .../dcm_walking/common/globalCoPEvaluator.ini | 14 +++ .../dcm_walking/common/pidParams.ini | 58 ++++++++++++ .../dcm_walking/common/plannerParams.ini | 88 +++++++++++++++++++ .../dcm_walking/common/walkingLogger.ini | 1 + .../common/zmpControllerParams.ini | 18 ++++ .../inverseKinematics.ini | 28 ++++++ .../jointRetargeting.ini | 31 +++++++ .../qpInverseKinematicsBlf.ini | 30 +++++++ .../iFeel_joint_retargeting/robotControl.ini | 36 ++++++++ .../tasks/regularization.ini | 30 +++++++ .../tasks/retargeting.ini | 30 +++++++ .../iFeel_joint_retargeting/tasks/torso.ini | 16 ++++ .../joypad_control/inverseKinematics.ini | 23 +++++ .../joypad_control/qpInverseKinematicsBlf.ini | 30 +++++++ .../joypad_control/robotControl.ini | 32 +++++++ .../joypad_control/tasks/regularization.ini | 26 ++++++ .../joypad_control/tasks/torso.ini | 16 ++++ .../dcm_walking_iFeel_joint_retargeting.ini | 83 +++++++++++++++++ .../iRonCub-Mk1/dcm_walking_with_joypad.ini | 75 ++++++++++++++++ 24 files changed, 734 insertions(+) create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/controllerParams.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/dcmReactiveControllerParams.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/freeSpaceEllipseParams.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/globalCoPEvaluator.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/walkingLogger.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/inverseKinematics.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/robotControl.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/regularization.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/torso.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini create mode 100644 src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/controllerParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/controllerParams.ini new file mode 100644 index 000000000..1359c17bb --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/controllerParams.ini @@ -0,0 +1,10 @@ +controllerHorizon 2 + +stateWeightTriplets ((0,0,750), (1,1,750)) +inputWeightTriplets ((0,0,90000000), (1,1,90000000)) + +#Foot Dimensions x_min x_max y_min y_max +foot_size ((-0.02 0.05), (-0.045 0.05)) +initial_zmp_position (0.0 0.0) + +convex_hull_tolerance 0.05 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/dcmReactiveControllerParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/dcmReactiveControllerParams.ini new file mode 100644 index 000000000..fd8eafba4 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/dcmReactiveControllerParams.ini @@ -0,0 +1 @@ +kDCM 1.1 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini new file mode 100644 index 000000000..5590bb48d --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini @@ -0,0 +1,6 @@ +# name of the left and right feet wrench ports +left_foot_wrench_input_port_name ("/left_foot/cartesianEndEffectorWrench:i") +left_foot_wrench_output_port_name ("/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o") + +right_foot_wrench_input_port_name ("/right_foot/cartesianEndEffectorWrench:i") +right_foot_wrench_output_port_name ("/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o") diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini new file mode 100644 index 000000000..c58f816fc --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini @@ -0,0 +1,18 @@ +#the x axis pointing forward the z axis pointing upward and the y concludes a right-handed frame +left_foot_frame l_sole +right_foot_frame r_sole + +# hand frames +left_hand_frame l_arm_jet_turbine +right_hand_frame r_arm_jet_turbine + +head_frame imu_frame + +root_frame root_link +torso_frame chest + +# filters +# if it is equal to 0 the low pass filters are not used +use_filters 0 + #Hz +cut_frequency 10.0 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/freeSpaceEllipseParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/freeSpaceEllipseParams.ini new file mode 100644 index 000000000..77b6e0d39 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/freeSpaceEllipseParams.ini @@ -0,0 +1,34 @@ +[ELLIPSE_METHOD_SETTINGS] +#This factor affects the reference of the unicycle. It allows switching between the specified ellipse and a smaller one to +#take into account the robot dimensions. If 0, it always considers only the specified ellipse. If greater than zero, the reference +#to the unicycle will be projected in a smaller ellipse as soon as the unicycle is no more perpendicular to the ellipse. +#Greater values make this behavior more sensitive. +conservative_factor 2.0 + +#This offset is subtracted from the semi_major_axis to obtain the inner ellipse +inner_offset_major 0.5 + +#This offset is subtracted from the semi_minor_axis to obtain the inner ellipse +inner_offset_minor 1.0 + +[ELLIPSE_MANAGER_SETTINGS] +port_name freeSpaceEllipse:in + +use_initial_ellipse 0 + +[INITIAL_ELLIPSE] + +#Axis along the x direction +semi_major_axis 1.0 + +#Axis along the y direction +semi_minor_axis 1.0 + +#Rotation around the z axis +angle 0.0 + +#x coordinate of the center +center_x 0.1 + +#y coordinate of the center +center_y 0.0 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/globalCoPEvaluator.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/globalCoPEvaluator.ini new file mode 100644 index 000000000..cb2fd51c4 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/globalCoPEvaluator.ini @@ -0,0 +1,14 @@ +# Minimum force to consider the CoP valid. +minimum_normal_force 1.0 + +# Limit to consider the CoP valid. +# |x| |y| +cop_admissible_limits (0.3, 0.2) + +# Tolerance radius to consider the measured CoP constant. +constant_cop_tolerance 0.0001 + +# Consecutive times in which the CoP remains constant. +# If this limit is reached, the module stops. +# Use a negative value to disable. +constant_cop_max_counter 125 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini new file mode 100644 index 000000000..bdc5b9124 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini @@ -0,0 +1,58 @@ +# PID handling file + +# All the following phases are those of the left foot. In a specific phase, a joint will have the PIDs specified in the corresponding section. If not, the default will be used (if specified, the original PID otherwise). + +# Possible phases: SWING_LEFT, SWING_RIGHT, SWITCH + +# Each group, except DEFAULT, contains: +# -the field activationPhase with one of the possible phases defined above, +# -the field activationOffset which specified the time advance/delay (if the sign is + it will be a delay) wrt the begin of the phase (OPTIONAL, default 0.0) +# -the field smoothingTime which defines the smoothing time with which the PIDs will be changed (OPTIONAL, default 1.0) + +# if 0 only the default group will be taken into consideration +useGainScheduling 0 +firmwareDelay 0.01 +smoothingTime 0.05 + +[DEFAULT] +#NAME P I D +torso_yaw -6000.0 -7111.0 0.0 +torso_roll -9000.0 -10666.0 0.0 +torso_pitch -9000.0 -14222.0 0.0 +l_shoulder_pitch 2000.0 7111.0 0.0 +#l_shoulder_roll 2166.0 10666.0 0.0 +l_shoulder_yaw 1711.1 7111.0 0.0 +r_shoulder_pitch -2000.0 -7111.0 0.0 +#r_shoulder_roll -2066.0 -10666.0 0.0 +r_shoulder_yaw -1711.1 -7111.0 0.0 +l_hip_pitch -4000.0 -7000.0 -100.0 +l_hip_roll 4500.0 8000.0 0.0 +l_hip_yaw -3011.0 -100.0 0.0 +l_knee -9000.0 -800.0 0.0 +l_ankle_pitch 4000.0 200.0 0.0 +l_ankle_roll 6000.0 200.0 0.0 +r_hip_pitch 4000.0 7000.0 100.0 +r_hip_roll -4500.0 -8000.0 0.0 +r_hip_yaw 3011.0 100.0 0.0 +r_knee 9000.0 800.0 0.0 +r_ankle_pitch -4000.0 -200.0 0.0 +r_ankle_roll -6000.0 -200.0 0.0 + +[PREIMPACT_LEFT] +activationPhase SWING_LEFT +activationOffset 0.3 +#NAME P I D +l_ankle_pitch 1000.0 100.0 0.0 +l_ankle_roll 2000.0 100.0 0.0 + +[PREIMPACT_RIGHT] +activationPhase SWING_RIGHT +activationOffset 0.3 +#NAME P I D +r_ankle_pitch -1000.0 -100.0 0.0 +r_ankle_roll -2000.0 -100.0 0.0 + +# The next reset the original gains. +[AFTER_IMPACT] +activationPhase SWITCH +activationOffset 0.2 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini new file mode 100644 index 000000000..5dc026c0c --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini @@ -0,0 +1,88 @@ +##Timings +plannerHorizon 5.0 + +##Unicycle controller. Available types: personFollowing, direct +controlType direct + +##Unicycle Related Quantities +unicycleGain 10.0 +referencePosition (0.1 0.0) +timeWeight 2.5 +positionWeight 1.0 +slowWhenTurningGain 2.0 +slowWhenBackwardFactor 0.4 +slowWhenSidewaysFactor 0.2 + +# Conservative factors that multiply the unicycle velocity saturations +# computed from the other parameters, like the minStepDuration. +# The first number multiplies the saturation for the linear and lateral +# velocity. The second number multiplies the angular velocity saturation. +saturationFactors (0.7, 0.7) + +##Bounds +#Step length +maxStepLength 0.21 +minStepLength 0.05 +maxLengthBackwardFactor 1.0 +#Width +minWidth 0.15 +#Angle Variations in DEGREES +#maxAngleVariation 12.0 +maxAngleVariation 20.0 +minAngleVariation 8.0 +#Timings +maxStepDuration 1.5 +minStepDuration 1.0 + +##Nominal Values +#Width +nominalWidth 0.16 +#Height +stepHeight 0.02 +stepLandingVelocity 0.00 +footApexTime 0.8 +comHeightDelta 0.0 +#Timings +nominalDuration 1.2 +lastStepSwitchTime 0.8 +switchOverSwingRatio 0.7 + +#ZMP Delta +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; +# If it is 1 the DCM position coincides with the next foot ZMP position. +lastStepDCMOffset 0.25 + +# Last Step DCM Stillness Percentage +# The percentage of the last step duration in which the DCM is still. +lastStepDCMStillPercentage 0.1 + + +#MergePoint +# The ratios of the double support in which it is present a merge point. +# The first number is the ratio at which the merge points begin, the second +# when they ends. +mergePointRatios (0.4, 0.4) + +# pitch delta +pitchDelta 0.0 + +##Should be the first step with the left foot? +swingLeft 0 +startAlwaysSameFoot 1 + +##Remove this line if you don't want to use the minimum jerk trajectory in feet interpolation +#useMinimumJerkFootTrajectory 1 + +##Remove this line if you want to enable the pause conditon +isPauseActive 1 + +additional_chest_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/walkingLogger.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/walkingLogger.ini new file mode 100644 index 000000000..11221de29 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/walkingLogger.ini @@ -0,0 +1 @@ +remote "/logger" diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini new file mode 100644 index 000000000..3d74ec1a9 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini @@ -0,0 +1,18 @@ +# remove the following line if you do not want the gain scheduling +useGainScheduling 1 + +smoothingTime 0.1 + +# if the gain scheduling is off only k*_walking parameters are used +kZMP_walking 3.5 +kCoM_walking 4.0 + +kZMP_stance 0.9 +kCoM_stance 4.0 + +# old parameters +#kZMP 2.0 low velocity reactive +#kZMP 1.9 low velocity MPC + +#kCoM 10.0 low velocity reactive +#kCoM 8.0 low velocity MPC diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini new file mode 100644 index 000000000..48739bd40 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini @@ -0,0 +1,28 @@ +#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 chest + +#The additional rotation is defined (by rows) in the Inertia frame. +#Remove the following line to keep the identity as additional rotation +additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) + +# solver paramenters +solver-verbosity 0 +solver_name mumps +max-cpu-time 20 + +#DEGREES +jointRegularization (0.0, 0.0, 0.0, + 15, 0, 0, + -7, 22, 11, 30, + -7, 22, 11, 30, + 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 (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/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini new file mode 100644 index 000000000..8b2718bec --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini @@ -0,0 +1,31 @@ +approaching_phase_duration 4.0 +hde_port_name /humanState:i +data_arrived_timeout 1.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 ("neck_pitch", "neck_roll", "neck_yaw", + "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 +variation_range (-0.05 0.0) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini new file mode 100644 index 000000000..22c23e297 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini @@ -0,0 +1,30 @@ +use_feedforward_term_for_joint_retargeting false + +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +mask (true, true, true) + +[RIGHT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 + +[ROOT_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +frame_name "root_link" + +[LEFT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 + +[include TORSO_TASK "./tasks/torso.ini"] +[include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] +[include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"] diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini new file mode 100644 index 000000000..11be4f090 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini @@ -0,0 +1,36 @@ +robot icub + + +joints_list ("neck_pitch", "neck_roll", "neck_yaw", + "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 ("head", "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) + +# if true a good joint traking is considered mandatory +good_tracking_required (false, false, false, + true, true, true, + true, true, true, true, + true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini new file mode 100644 index 000000000..b4d3f6761 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -0,0 +1,30 @@ +robot_velocity_variable_name "robot_velocity" +kp (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +states ("STANCE", "WALKING") +sampling_time 0.005 +settling_time 5.0 + +[STANCE] +name "stance" +weight (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) + +[WALKING] +name "walking" +weight (0.0, 0.0, 0.0, + 2.0, 2.0, 2.0, + 0.0, 2.0, 0.0, 0.0, + 0.0, 2.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/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini new file mode 100644 index 000000000..fc5ce6147 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -0,0 +1,30 @@ +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, 5.0, 5.0, 5.0) + +states ("STANCE", "WALKING") +sampling_time 0.005 +settling_time 5.0 + +[STANCE] +name "stance" +weight (2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +[WALKING] +name "walking" + +weight (2.0, 2.0, 2.0, + 0.0, 0.0, 0.0, + 2.0, 0.0, 2.0, 2.0, + 2.0, 0.0, 2.0, 2.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini new file mode 100644 index 000000000..f6941eb42 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -0,0 +1,16 @@ +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + + +states ("STANCE", "WALKING") +sampling_time 0.005 +settling_time 3.0 + +[STANCE] +name "stance" +weight (0.0, 0.0, 0.0) + +[WALKING] +name "walking" +weight (5.0, 5.0, 5.0) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/inverseKinematics.ini new file mode 100644 index 000000000..b9b18f563 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/inverseKinematics.ini @@ -0,0 +1,23 @@ +#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 chest + +#The additional rotation is defined (by rows) in the Inertia frame. +#Remove the following line to keep the identity as additional rotation +additional_rotation ((0.0 0.0 1.0),(1.0 0.0 0.0),(0.0 1.0 0.0)) + +# solver paramenters +solver-verbosity 0 +solver_name mumps +max-cpu-time 20 + +#DEGREES +jointRegularization (15, 0, 0, -7, 22, 11, 30, -7, 22, 11, 30, 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 (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/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini new file mode 100644 index 000000000..619d354e0 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -0,0 +1,30 @@ +use_feedforward_term_for_joint_retargeting false + +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +mask (true, true, true) + +[RIGHT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 + +[LEFT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 + +[ROOT_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +frame_name "root_link" + +[include TORSO_TASK "./tasks/torso.ini"] +[include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] +[include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"] diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/robotControl.ini new file mode 100644 index 000000000..68515325d --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/robotControl.ini @@ -0,0 +1,32 @@ +robot icub + +joints_list ("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_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) + +# if true a good joint traking is considered mandatory +good_tracking_required (true, true, true, + true, true, true, true, + true, true, true, true, + true, true, true, true, true, true, + true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/regularization.ini new file mode 100644 index 000000000..e50dfc6fb --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/regularization.ini @@ -0,0 +1,26 @@ +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) + +states ("STANCE", "WALKING") +sampling_time 0.005 +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, + 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, + 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/iRonCub-Mk1/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/torso.ini new file mode 100644 index 000000000..e88300702 --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/tasks/torso.ini @@ -0,0 +1,16 @@ +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + + +states ("STANCE", "WALKING") +sampling_time 0.005 +settling_time 0.5 + +[STANCE] +name "stance" +weight (5.0, 5.0, 5.0) + +[WALKING] +name "walking" +weight (5.0, 5.0, 5.0) diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini new file mode 100644 index 000000000..db206673f --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini @@ -0,0 +1,83 @@ +# 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 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.15 + +# If set to true, the desired ZMP is used directly in the COM/ZMP controller +skip_dcm_controller 1 + +# The goal port receives 2 or 3 doubles according to the selected planner controller +goal_port_suffix /goal:i + +# Scales the input coming from the goal port +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.18 + +# How much time (in seconds) before failing due to missing feedback +max_feedback_delay_in_s 1.0 + +# If set true, we remove the zmp-com offset at startworking and rotate in evaluateZMP +remove_zmp_offset 0 + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.56 +# sampling time +sampling_time 0.005 +# enable joint retargeting +use_joint_retargeting 1 +# enable the virtualizer +use_virtualizer 1 +use_com_retargeting 0 +# 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/iFeel_joint_retargeting/robotControl.ini"] + +# include trajectory planner parameters +[include TRAJECTORY_PLANNER "./dcm_walking/common/plannerParams.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/qpInverseKinematicsBlf.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 CoP evaluator parameters +[include COP_EVALUATOR "./dcm_walking/common/globalCoPEvaluator.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"] diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini new file mode 100644 index 000000000..0426c3f7d --- /dev/null +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini @@ -0,0 +1,75 @@ +# 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 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.15 + +# If set to true, the desired ZMP is used directly in the COM/ZMP controller +skip_dcm_controller 1 + +# The goal port receives 2 or 3 doubles according to the selected planner controller +goal_port_suffix /goal:i + +# Scales the input coming from the goal port +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 + +# If set true, we remve the zmp-com offset at startworking and rotate in evaluateZMP +remove_zmp_offset 1 + +# general parameters +[GENERAL] +name walking-coordinator +# height of the com +com_height 0.56 +# sampling time +sampling_time 0.005 +# 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"] + +# 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 Reactive parameters +[include DCM_REACTIVE_CONTROLLER "./dcm_walking/common/dcmReactiveControllerParams.ini"] + +# include ZMP-CoM controller parameters +[include ZMP_CONTROLLER "./dcm_walking/common/zmpControllerParams.ini"] + +# include inverse kinematics parameters +[include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joypad_control/inverseKinematics.ini"] + +# include qp inverse kinematics parameters +[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematicsBlf.ini"] + +# include inverse kinematics parameters +[include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] + +# include FT sensors parameters +[include FT_SENSORS "./dcm_walking/common/forceTorqueSensors.ini"] + +# include CoP evaluator parameters +[include COP_EVALUATOR "./dcm_walking/common/globalCoPEvaluator.ini"] + +# include Logger parameters +[include WALKING_LOGGER "./dcm_walking/common/walkingLogger.ini"] + +# include lower PID parameters +[include PID "./dcm_walking/common/pidParams.ini"] From d2b138156c9360705ef47cc81b454d7b986a359e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 24 Sep 2024 08:56:58 +0000 Subject: [PATCH 2/2] [iRonCub-Mk1] Tune the parameters --- .../app/dcmWalkingJoypad/joypad.ini | 6 ++--- .../dcm_walking/common/forceTorqueSensors.ini | 4 ++-- .../dcm_walking/common/forwardKinematics.ini | 6 ++--- .../dcm_walking/common/pidParams.ini | 14 +++++------ .../dcm_walking/common/plannerParams.ini | 24 +++++++++---------- .../common/zmpControllerParams.ini | 2 +- .../jointRetargeting.ini | 4 ++-- .../qpInverseKinematicsBlf.ini | 3 ++- .../iFeel_joint_retargeting/robotControl.ini | 4 ++-- .../joypad_control/qpInverseKinematicsBlf.ini | 3 ++- .../dcm_walking_iFeel_joint_retargeting.ini | 4 ++-- .../iRonCub-Mk1/dcm_walking_with_joypad.ini | 4 ++-- 12 files changed, 40 insertions(+), 38 deletions(-) diff --git a/src/JoypadModule/app/dcmWalkingJoypad/joypad.ini b/src/JoypadModule/app/dcmWalkingJoypad/joypad.ini index 12a55b502..daefb3057 100644 --- a/src/JoypadModule/app/dcmWalkingJoypad/joypad.ini +++ b/src/JoypadModule/app/dcmWalkingJoypad/joypad.ini @@ -21,15 +21,15 @@ disconnectButton 11 # Menu button connectPortsSeparately true # Port names -rpcServerPort_name /walking-coordinator/rpc -rpcClientPort_name /rpc +rpcServerPort_name /walking-coordinatori-ironCub/rpc +rpcClientPort_name /ironcub-joypad/rpc # The goal is the desired position of the robot CoM. The desired position is a # vector containing the x and the y coordinate expressed in a frame rigidly # attached to the robot. # The x-axis points forward # The y-axis points on the left -robotGoalInputPort_name /walking-coordinator/goal:i +robotGoalInputPort_name /walking-coordinator-ironCub/goal:i robotGoalOutputPort_name /goal:o [JOYPAD_DEVICE] diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini index 5590bb48d..fda4110f2 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forceTorqueSensors.ini @@ -1,6 +1,6 @@ # name of the left and right feet wrench ports left_foot_wrench_input_port_name ("/left_foot/cartesianEndEffectorWrench:i") -left_foot_wrench_output_port_name ("/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o") +left_foot_wrench_output_port_name ("/iRonCub-Mk1/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o") right_foot_wrench_input_port_name ("/right_foot/cartesianEndEffectorWrench:i") -right_foot_wrench_output_port_name ("/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o") +right_foot_wrench_output_port_name ("/iRonCub-Mk1/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o") diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini index c58f816fc..59dc7047c 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/forwardKinematics.ini @@ -3,13 +3,13 @@ left_foot_frame l_sole right_foot_frame r_sole # hand frames -left_hand_frame l_arm_jet_turbine -right_hand_frame r_arm_jet_turbine +left_hand_frame l_hand_dh_frame +right_hand_frame r_hand_dh_frame head_frame imu_frame root_frame root_link -torso_frame chest +torso_frame neck_2 # filters # if it is equal to 0 the low pass filters are not used diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini index bdc5b9124..60f9c1c18 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/pidParams.ini @@ -17,25 +17,25 @@ smoothingTime 0.05 [DEFAULT] #NAME P I D torso_yaw -6000.0 -7111.0 0.0 -torso_roll -9000.0 -10666.0 0.0 -torso_pitch -9000.0 -14222.0 0.0 +torso_roll -9000.0 -5666.0 0.0 +torso_pitch -6000.0 -5222.0 0.0 l_shoulder_pitch 2000.0 7111.0 0.0 #l_shoulder_roll 2166.0 10666.0 0.0 l_shoulder_yaw 1711.1 7111.0 0.0 r_shoulder_pitch -2000.0 -7111.0 0.0 #r_shoulder_roll -2066.0 -10666.0 0.0 r_shoulder_yaw -1711.1 -7111.0 0.0 -l_hip_pitch -4000.0 -7000.0 -100.0 +l_hip_pitch -5000.0 -7000.0 -100.0 l_hip_roll 4500.0 8000.0 0.0 l_hip_yaw -3011.0 -100.0 0.0 -l_knee -9000.0 -800.0 0.0 +l_knee -10000.0 -800.0 0.0 l_ankle_pitch 4000.0 200.0 0.0 l_ankle_roll 6000.0 200.0 0.0 -r_hip_pitch 4000.0 7000.0 100.0 +r_hip_pitch 5000.0 7000.0 100.0 r_hip_roll -4500.0 -8000.0 0.0 r_hip_yaw 3011.0 100.0 0.0 -r_knee 9000.0 800.0 0.0 -r_ankle_pitch -4000.0 -200.0 0.0 +r_knee 10000.0 800.0 0.0 +r_ankle_pitch -5000.0 -200.0 0.0 r_ankle_roll -6000.0 -200.0 0.0 [PREIMPACT_LEFT] diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini index 5dc026c0c..aebae3fc4 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/plannerParams.ini @@ -21,35 +21,35 @@ saturationFactors (0.7, 0.7) ##Bounds #Step length -maxStepLength 0.21 +maxStepLength 0.19 minStepLength 0.05 maxLengthBackwardFactor 1.0 #Width -minWidth 0.15 +minWidth 0.14 #Angle Variations in DEGREES #maxAngleVariation 12.0 maxAngleVariation 20.0 minAngleVariation 8.0 #Timings -maxStepDuration 1.5 -minStepDuration 1.0 +maxStepDuration 1.2 +minStepDuration 0.9 ##Nominal Values #Width -nominalWidth 0.16 +nominalWidth 0.15 #Height -stepHeight 0.02 -stepLandingVelocity 0.00 -footApexTime 0.8 +stepHeight 0.04 +stepLandingVelocity -0.1 +footApexTime 0.5 comHeightDelta 0.0 #Timings -nominalDuration 1.2 +nominalDuration 1.0 lastStepSwitchTime 0.8 -switchOverSwingRatio 0.7 +switchOverSwingRatio 0.5 #ZMP Delta -leftZMPDelta (0.03 -0.005) -rightZMPDelta (0.03 0.005) +leftZMPDelta (0.03 0.02) +rightZMPDelta (0.03 -0.0) #Feet cartesian offset on the yaw leftYawDeltaInDeg 0.0 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini index 3d74ec1a9..fa148e5e3 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/common/zmpControllerParams.ini @@ -4,7 +4,7 @@ useGainScheduling 1 smoothingTime 0.1 # if the gain scheduling is off only k*_walking parameters are used -kZMP_walking 3.5 +kZMP_walking 2.5 kCoM_walking 4.0 kZMP_stance 0.9 diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini index 8b2718bec..dba7d4e1f 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini @@ -12,8 +12,8 @@ data_arrived_timeout 1.0 ## "joint_retargeting_port_name" port retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "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_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow") joint_retargeting_port_name /jointPosition:i diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini index 22c23e297..6bec72318 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini @@ -6,7 +6,7 @@ robot_velocity_variable_name "robot_velocity" [COM_TASK] robot_velocity_variable_name "robot_velocity" kp_linear 2.0 -mask (true, true, true) +mask (true, true, false) [RIGHT_FOOT_TASK] robot_velocity_variable_name "robot_velocity" @@ -18,6 +18,7 @@ kp_angular 5.0 robot_velocity_variable_name "robot_velocity" kp_linear 2.0 frame_name "root_link" +mask (false, false, true) [LEFT_FOOT_TASK] robot_velocity_variable_name "robot_velocity" diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini index 11be4f090..b7f390957 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/iFeel_joint_retargeting/robotControl.ini @@ -3,8 +3,8 @@ robot icub joints_list ("neck_pitch", "neck_roll", "neck_yaw", "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_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "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") diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini index 619d354e0..0ed177e89 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -6,7 +6,7 @@ robot_velocity_variable_name "robot_velocity" [COM_TASK] robot_velocity_variable_name "robot_velocity" kp_linear 2.0 -mask (true, true, true) +mask (true, true, false) [RIGHT_FOOT_TASK] robot_velocity_variable_name "robot_velocity" @@ -24,6 +24,7 @@ kp_angular 5.0 robot_velocity_variable_name "robot_velocity" kp_linear 2.0 frame_name "root_link" +mask (false, false, true) [include TORSO_TASK "./tasks/torso.ini"] [include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini index db206673f..24ce200e3 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_iFeel_joint_retargeting.ini @@ -30,9 +30,9 @@ remove_zmp_offset 0 # general parameters [GENERAL] -name walking-coordinator +name walking-coordinator-ironCub # height of the com -com_height 0.56 +com_height 0.53 # sampling time sampling_time 0.005 # enable joint retargeting diff --git a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini index 0426c3f7d..9e294d25e 100644 --- a/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iRonCub-Mk1/dcm_walking_with_joypad.ini @@ -27,9 +27,9 @@ remove_zmp_offset 1 # general parameters [GENERAL] -name walking-coordinator +name walking-coordinator-ironCub # height of the com -com_height 0.56 +com_height 0.53 # sampling time sampling_time 0.005 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link