Adding files for walking autonomous on SN000 and stop the robot if go…

…al data are not available(#193)

src/WalkingModule/app/robots/ergoCubSN000/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/ergoCubSN000/dcm_walking/autonomous/jointRetargeting.ini

@@ -0,0 +1,27 @@
+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

src/WalkingModule/app/robots/ergoCubSN000/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/ergoCubSN000/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/ergoCubSN000/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.002
+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/ergoCubSN000/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.002
+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/ergoCubSN000/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.002
+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/ergoCubSN000/dcm_walking_autonomous.ini

@@ -0,0 +1,86 @@
+# 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.03
+
+# 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                   1
+
+# general parameters
+[GENERAL]
+name                    walking-coordinator
+# height of the com
+com_height              0.70
+# sampling time
+sampling_time           0.002
+# 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/autonomous/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/autonomous/inverseKinematics.ini"]
+
+# include qp inverse kinematcs parameters
+[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/autonomous/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/autonomous/jointRetargeting.ini"]

src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h

@@ -61,6 +61,8 @@ namespace WalkingControllers
 
         double m_dT; /**< RFModule period. */
         double m_time; /**< Current time. */
+        double m_lastSetGoalTime; /**< Time of the last set goal. */
+        double m_maxTimeToWaitForGoal; /**< Maximum time to wait for a goal. */
         std::string m_robot; /**< Robot name. */
 
         bool m_useMPC; /**< True if the MPC controller is used. */

src/WalkingModule/src/Module.cpp

@@ -138,6 +138,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf)
     std::string goalSuffix = rf.check("goal_port_suffix", yarp::os::Value("/goal:i")).asString();
     m_skipDCMController = rf.check("skip_dcm_controller", yarp::os::Value(false)).asBool();
     m_removeZMPOffset = rf.check("remove_zmp_offset", yarp::os::Value(false)).asBool();
+    m_maxTimeToWaitForGoal = rf.check("max_time_to_wait_for_goal", yarp::os::Value(1.0)).asFloat64();
 
     m_goalScaling.resize(3);
     if (!YarpUtilities::getVectorFromSearchable(rf, "goal_port_scaling", m_goalScaling))
@@ -675,6 +676,16 @@ bool WalkingModule::updateModule()
                 yError() << "[WalkingModule::updateModule] Unable to set the planner input";
                 return false;
             }
+            m_lastSetGoalTime = m_time;
+        }
+        else if (!m_firstRun && ((m_time - m_lastSetGoalTime) > m_maxTimeToWaitForGoal))
+        {
+            yarp::sig::Vector dummy(3, 0.0);
+            if (!setPlannerInput(dummy))
+            {
+                yError() << "[WalkingModule::updateModule] Unable to set the planner input";
+                return false;
+            }
         }
 
         // if a new trajectory is required check if its the time to evaluate the new trajectory or