Add walking files for ergoCubSN003 and bump version to 0.10.0

CMakeLists.txt

@@ -6,7 +6,7 @@ cmake_minimum_required(VERSION 3.5)
 
 ## MAIN project
 project(WalkingControllers
-  VERSION 0.9.0)
+  VERSION 0.10.0)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/autonomous/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, 0.0,
+                         0.0, 0.0, 0.0,
+                         12.0, 7.0, -12.0, 41.0, 0.0, 0.0, 0.0,
+                         12.0, 7.0, -12.0, 41.0, 0.0, 0.0, 0.0,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52,
+                         5.76, 1.61, -0.31, -31.64, -20.52, -1.52)
+
+#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/ergoCubSN003/dcm_walking/autonomous/jointRetargeting.ini

@@ -0,0 +1,28 @@
+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          ("camera_tilt", "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_yaw", "l_wrist_roll", "l_wrist_pitch",
+				  "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_yaw", "r_wrist_roll", "r_wrist_pitch")
+
+smoothing_time_approaching       2.0
+smoothing_time_walking           0.03
+
+[VIRTUALIZER]
+robot_orientation_port_name     /torsoYaw:o
+
+[COM_RETARGETING]
+smoothing_time_approaching       2.0
+smoothing_time_walking           1.0
+com_height_scaling_factor        0.5
+variation_range                  (-0.05 0.0)

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/autonomous/qpInverseKinematicsBlf.ini

@@ -0,0 +1,31 @@
+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, false)
+
+[ROOT_TASK]
+robot_velocity_variable_name    "robot_velocity"
+frame_name                      "root_link"
+kp_linear                       0.5
+mask                            (false, false, 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
+
+[include TORSO_TASK "./tasks/torso.ini"]
+[include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"]
+[include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"]

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/autonomous/robotControl.ini

@@ -0,0 +1,36 @@
+robot                   ergocub
+
+
+joints_list             ("camera_tilt", "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_yaw", "l_wrist_roll", "l_wrist_pitch",
+                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_yaw", "r_wrist_roll", "r_wrist_pitch",
+                         "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, true,
+                         true, true, true, true, true, true)
+
+# if true a good joint traking is considered mandatory
+good_tracking_required  (false, false, false, false
+                         true, true, true,
+                         false, false, true, true, false, false, false,
+                         false, false, true, true, false, false, false,
+                         true, true, true, true, true, true,
+                         true, true, true, true, true, true)

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/autonomous/tasks/regularization.ini

@@ -0,0 +1,36 @@
+robot_velocity_variable_name    "robot_velocity"
+kp                              (5.0, 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, 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.001
+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, 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,
+#          2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0,
+#          2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0,
+#          1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+#          1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+weight  (0.0, 0.0, 0.0, 0.0,
+         2.0, 2.0, 2.0,
+         0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+         0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+         1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+         1.0, 1.0, 1.0, 1.0, 1.0, 1.0)

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/autonomous/tasks/retargeting.ini

@@ -0,0 +1,37 @@
+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, 5.0,
+                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
+
+states          ("STANCE", "WALKING")
+sampling_time   0.001
+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,
+         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,
+#          0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0,
+#          0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0,
+#          1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+#          1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
+
+
+weight  (2.0, 2.0, 2.0, 2.0,
+         0.0, 0.0, 0.0,
+         2.0, 0.0, 2.0, 2.0, 2.0, 2.0, 2.0,
+         2.0, 0.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)

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/autonomous/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.001
+settling_time   3.0
+
+[STANCE]
+name    "stance"
+weight  (0.0, 0.0, 0.0)
+
+[WALKING]
+name    "walking"
+weight  (5.0, 5.0, 5.0)

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

@@ -0,0 +1,10 @@
+controllerHorizon       2
+
+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.12   0.12), (-0.05   0.05))
+initial_zmp_position    (0.0 0.0)
+
+convex_hull_tolerance   0.05

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/common/dcmReactiveControllerParams.ini

@@ -0,0 +1 @@
+kDCM                    1.2

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/common/forceTorqueSensors.ini

@@ -0,0 +1,10 @@
+# name of the left and right feet wrench ports
+left_foot_wrench_input_port_name     ("/left_foot_front/cartesianEndEffectorWrench:i",
+                                      "/left_foot_rear/cartesianEndEffectorWrench:i")
+left_foot_wrench_output_port_name    ("/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o",
+                                      "/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o")
+
+right_foot_wrench_input_port_name     ("/right_foot_front/cartesianEndEffectorWrench:i",
+                                      "/right_foot_rear/cartesianEndEffectorWrench:i")
+right_foot_wrench_output_port_name    ("/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o",
+                                      "/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o")

src/WalkingModule/app/robots/ergoCubSN003/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_hand_palm
+right_hand_frame        r_hand_palm
+
+head_frame              head
+
+root_frame              root_link
+torso_frame             neck_2
+
+# filters
+# if it is equal to 0 the low pass filters are not used
+use_filters             0
+                        #Hz
+cut_frequency           10.0

src/WalkingModule/app/robots/ergoCubSN003/dcm_walking/common/freeSpaceEllipseParams.ini

@@ -0,0 +1,31 @@
+[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_minor_axis and semi_major_axis to obtain the inner ellipse
+inner_ellipse_offset   0.5
+
+[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

src/WalkingModule/app/robots/ergoCubSN003/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.35, 0.25)
+
+# Tolerance radius to consider the measured CoP constant.
+constant_cop_tolerance              0.000001
+
+# 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

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

@@ -0,0 +1,37 @@
+# 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
+
+[DEFAULT]
+#NAME                P     I      D
+#torso_pitch         5.0   1.0    0.5
+l_knee             -18000.0  -1000.0   0.0
+r_knee              18000.0   1000.0   0.0
+l_ankle_pitch      -18000.0  -800.0   0.0
+r_ankle_pitch       18000.0   800.0   0.0
+l_ankle_roll        -12000.0  -1000.0   0.0
+r_ankle_roll         12000.0   1000.0   0.0
+l_hip_pitch         -15000.0  -1500.0   0.0
+r_hip_pitch          15000.0   1500.0   0.0
+l_hip_roll          18000.0   1000.0   0.0
+r_hip_roll         -18000.0  -1000.0   0.0
+r_hip_yaw            15000.0    2000.0   0.0
+l_hip_yaw           -15000.0    -2000.0   0.0
+# #r_shoulder_pitch   -10000.0 -10000.0   0.0
+# #r_shoulder_roll    -10000.0 -10000.0   0.0
+# #r_shoulder_yaw      -3000.0  -1500.0   0.0
+# #r_elbow              4000.0   4000.0   0.0
+# #l_shoulder_pitch    10000.0  10000.0   0.0
+# #l_shoulder_roll     10000.0  10000.0   0.0
+# #l_shoulder_yaw       3000.0   1500.0   0.0
+# #l_elbow             -4000.0  -4000.0   0.0