Fix/icub3 sim

README.md

@@ -87,6 +87,7 @@ export YARP_DATA_DIRS=$YARP_DATA_DIRS:$WalkingControllers_INSTALL_DIR/share/yarp
 3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5):
 
     ``` sh
+    export YARP_CLOCK=/clock
     gazebo -slibgazebo_yarp_clock.so
     ```
 4. Run `yarprobotinterface`

src/RobotInterface/src/Helper.cpp

@@ -163,8 +163,8 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
             for(;leftWrenchIt != m_leftFootMeasuredWrench.end(); std::advance(leftWrenchIt, 1))
             {
                 // the idyntree wrench raw data  is not a consecutive array
-                iDynTree::toEigen(m_leftWrench.getLinearVec3()) = yarp::eigen::toEigen(leftWrenchIt->wrenchInput).head<3>();
-                iDynTree::toEigen(m_leftWrench.getAngularVec3()) = yarp::eigen::toEigen(leftWrenchIt->wrenchInput).tail<3>();
+                iDynTree::toEigen(m_leftWrench.getLinearVec3()) += yarp::eigen::toEigen(leftWrenchIt->wrenchInput).head<3>();
+                iDynTree::toEigen(m_leftWrench.getAngularVec3()) += yarp::eigen::toEigen(leftWrenchIt->wrenchInput).tail<3>();
             }
 
             // right foot
@@ -181,8 +181,8 @@ bool RobotInterface::getFeedbacksRaw(unsigned int maxAttempts)
             for(;rightWrenchIt != m_rightFootMeasuredWrench.end(); std::advance(rightWrenchIt, 1))
             {
                 // the idyntree wrench raw data  is not a consecutive array
-                iDynTree::toEigen(m_rightWrench.getLinearVec3()) = yarp::eigen::toEigen(rightWrenchIt->wrenchInput).head<3>();
-                iDynTree::toEigen(m_rightWrench.getAngularVec3()) = yarp::eigen::toEigen(rightWrenchIt->wrenchInput).tail<3>();
+                iDynTree::toEigen(m_rightWrench.getLinearVec3()) += yarp::eigen::toEigen(rightWrenchIt->wrenchInput).head<3>();
+                iDynTree::toEigen(m_rightWrench.getAngularVec3()) += yarp::eigen::toEigen(rightWrenchIt->wrenchInput).tail<3>();
             }
 
             return true;

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_hand_retargeting.ini

@@ -11,6 +11,22 @@ 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.02
+
+# Tolerance radius to consider the measured ZMP constant.
+constant_ZMP_tolerance              0.0001
+
+# 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

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_joint_retargeting.ini

@@ -13,6 +13,22 @@ use_QP-IK                          1
 
 stance_phase_time_out                1
 
+# Limit on the maximum initial velocity. This avoids the robot to jump at startup
+max_initial_com_vel                 0.02
+
+# Tolerance radius to consider the measured ZMP constant.
+constant_ZMP_tolerance              0.0001
+
+# 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

src/WalkingModule/app/robots/iCubGazeboV2_5/dcm_walking_with_joypad.ini

@@ -11,6 +11,22 @@ 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.02
+
+# Tolerance radius to consider the measured ZMP constant.
+constant_ZMP_tolerance              0.0001
+
+# 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

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/controllerParams.ini

@@ -4,7 +4,7 @@ stateWeightTriplets     ((0,0,7500), (1,1,7500))
 inputWeightTriplets     ((0,0,9000000), (1,1,9000000))
 
 #Foot Dimensions          x_min   x_max  y_min   y_max
-foot_size               ((-0.02   0.05), (-0.025   0.025))
+foot_size               ((-0.12   0.12), (-0.05   0.05))
 initial_zmp_position    (0.0 0.0)
 
 convex_hull_tolerance   0.05

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/forwardKinematics.ini

@@ -3,10 +3,10 @@ left_foot_frame         l_sole
 right_foot_frame        r_sole
 
 # hand frames
-left_hand_frame         l_sole
-right_hand_frame        r_sole
+left_hand_frame         l_hand
+right_hand_frame        r_hand
 
-head_frame              l_sole
+head_frame              head
 
 root_frame              root_link
 torso_frame             neck_2

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/pidParams.ini

@@ -14,14 +14,16 @@ useGainScheduling           0
 
 [DEFAULT]
 #NAME                P     I      D
-#torso_pitch         5.0   1.0    0.5
-#l_knee             30.0  0.30   0.30
-#r_knee             30.0  0.30   0.30
-#l_ankle_pitch      35.0  0.35   0.35
-#r_ankle_pitch      35.0  0.35   0.35
-#l_ankle_roll       35.0  0.35   0.35
-#r_ankle_roll       35.0  0.35   0.35
-#l_hip_pitch        35.0  0.35   0.35
-#r_hip_pitch        35.0  0.35   0.35
-#l_hip_roll         35.0  0.35   0.35
-#r_hip_roll         35.0  0.35   0.35
+torso_pitch        40.0  0.35   0.35
+torso_roll         40.0  0.35   0.35
+torso_yaw          40.0  0.35   0.35
+l_knee             40.0  0.35   0.35
+r_knee             40.0  0.35   0.35
+l_ankle_pitch      40.0  0.35   0.35
+r_ankle_pitch      40.0  0.35   0.35
+l_ankle_roll       40.0  0.35   0.35
+r_ankle_roll       40.0  0.35   0.35
+l_hip_pitch        40.0  0.35   0.35
+r_hip_pitch        40.0  0.35   0.35
+l_hip_roll         40.0  0.35   0.35
+r_hip_roll         40.0  0.35   0.35

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/plannerParams.ini

@@ -10,34 +10,34 @@ slowWhenTurningGain     5.0
 
 ##Bounds
 #Step length
-maxStepLength           0.22
+maxStepLength           0.21
 minStepLength           0.05
 #Width
-minWidth                0.18
+minWidth                0.12
 #Angle Variations in DEGREES
 #maxAngleVariation       12.0
-maxAngleVariation       10.0
+maxAngleVariation       22.0
 minAngleVariation       8.0
 #Timings
-maxStepDuration         5.0
-minStepDuration         4.0
+maxStepDuration         1.3
+minStepDuration         1.1
 
 ##Nominal Values
 #Width
-nominalWidth            0.20
+nominalWidth            0.14
 #Height
-stepHeight              0.03
+stepHeight              0.02
 stepLandingVelocity     0.0
-footApexTime            0.8
-comHeightDelta          0.01
+footApexTime            0.7
+comHeightDelta          0.0
 #Timings
-nominalDuration         4.9
+nominalDuration         1.2
 lastStepSwitchTime      0.8
-switchOverSwingRatio    0.7
+switchOverSwingRatio    1.0
 
 #ZMP Delta
-leftZMPDelta           (0.0 -0.0)
-rightZMPDelta          (0.0 0.0)
+leftZMPDelta           (-0.015 -0.01)
+rightZMPDelta          (-0.015  0.01)
 
 # Last Step DCM Offset
 # If it is 0.5 the final DCM will be in the middle of the two footsteps;

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/common/zmpControllerParams.ini

@@ -4,13 +4,13 @@ useGainScheduling      1
 smoothingTime          0.05
 
 # if the gain scheduling is off only k*_walking parameters are used
-kZMP_walking           0.0
+kZMP_walking           1.0
 # kZMP_stance            1.0
-kCoM_walking           5.0
+kCoM_walking           3.0
 
-kZMP_stance            0.0
+kZMP_stance            1.0
 # kZMP_stance            1.0
-kCoM_stance            5.0
+kCoM_stance            3.0
 
 # old parameters
 #kZMP                    2.0 low velocity reactive

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/hand_retargeting/handRetargeting.ini

@@ -0,0 +1,12 @@
+approaching_phase_duration      5.0
+
+[HAND_RETARGETING]
+
+left_hand_transform_port_name     /leftHandDesiredPose:i
+right_hand_transform_port_name    /rightHandDesiredPose:i
+
+smoothing_time_approaching       3.0
+smoothing_time_walking           0.1
+
+[VIRTUALIZER]
+robot_orientation_port_name     /torsoYaw:o

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/hand_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        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)

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/hand_retargeting/qpInverseKinematics.ini

@@ -0,0 +1,50 @@
+# CoM
+use_com_as_constraint             1
+# comWeightTriplets              ((0,0,100), (1,1,100), (2,2,100))
+
+# weight matrices related to the neck
+neck_weight                     5.0
+additional_rotation             ((1.0 0.0 0.0),(0.0 1.0 0.0),(0.0 0.0 1.0))
+
+## Regularization 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_weights   (1.0, 1.0, 1.0,
+                                0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25,
+                                0.15, 0.25, 0.25, 0.15, 0.25, 0.25, 0.25,
+                                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_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)
+
+#Gains
+k_posCom                        1.0
+k_posFoot                       10.0
+k_attFoot                       5.0
+k_neck                          1.0
+
+# Hand retargeting
+k_posHand                       1.0
+k_attHand                       1.0
+# Upperbounds for the hand velocity defined by the controller and used as reference in the QP-IK. Negative value to disable the check
+max_hand_linear_vel_modulus     0.3
+max_hand_angular_vel_modulus    -1.0
+
+# use gain scheduling for hand retargeting
+smoothing_time                  0.5
+hand_weight_stance               (10, 10, 10, 2.0, 2.0, 2.0)
+hand_weight_walking              (10, 10, 10, 2.0, 2.0, 2.0)
+
+# joint limits
+k_joint_limit_lower_bound       1.0
+k_joint_limit_upper_bound       1.0
+
+# use_joint_limits_constraint   1

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/hand_retargeting/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", "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)

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/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        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     (0.0, 0.0, 0.0,
+                         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)

src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joint_retargeting/jointRetargeting.ini

@@ -0,0 +1,28 @@
+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          ("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