From d23d8edd5f99f2a0512ab756b9549fd2b6ab53fe Mon Sep 17 00:00:00 2001 From: carlos cardenas Date: Tue, 24 Sep 2024 12:57:25 +0200 Subject: [PATCH 1/3] Adding files for walking autonomous --- .../autonomous/inverseKinematics.ini | 28 ++++++ .../autonomous/jointRetargeting.ini | 27 ++++++ .../autonomous/qpInverseKinematicsBlf.ini | 31 +++++++ .../dcm_walking/autonomous/robotControl.ini | 36 ++++++++ .../autonomous/tasks/regularization.ini | 36 ++++++++ .../autonomous/tasks/retargeting.ini | 37 ++++++++ .../dcm_walking/autonomous/tasks/torso.ini | 16 ++++ .../ergoCubSN000/dcm_walking_autonomous.ini | 86 +++++++++++++++++++ 8 files changed, 297 insertions(+) create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/inverseKinematics.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/jointRetargeting.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/qpInverseKinematicsBlf.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/robotControl.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/regularization.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/retargeting.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/torso.ini create mode 100644 src/WalkingModule/app/robots/ergoCubSN000/dcm_walking_autonomous.ini diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/inverseKinematics.ini new file mode 100644 index 000000000..558e9e63f --- /dev/null +++ b/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) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/jointRetargeting.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/jointRetargeting.ini new file mode 100644 index 000000000..ad9d37a8e --- /dev/null +++ b/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 diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/qpInverseKinematicsBlf.ini new file mode 100644 index 000000000..b16519e48 --- /dev/null +++ b/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"] diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/robotControl.ini new file mode 100644 index 000000000..fbcd01641 --- /dev/null +++ b/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) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/regularization.ini new file mode 100644 index 000000000..182b72247 --- /dev/null +++ b/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) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/retargeting.ini new file mode 100644 index 000000000..3d8f48e52 --- /dev/null +++ b/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) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/torso.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/autonomous/tasks/torso.ini new file mode 100644 index 000000000..01d7def3d --- /dev/null +++ b/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) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking_autonomous.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking_autonomous.ini new file mode 100644 index 000000000..e806af557 --- /dev/null +++ b/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"] From f9c8e93cf39bb0c679fe2d2819ea5dc4ee584ce9 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Oct 2024 19:23:59 +0200 Subject: [PATCH 2/3] Add a logic to stop the robot if the goal is not set for more than a given ammount of time --- .../WalkingControllers/WalkingModule/Module.h | 2 ++ src/WalkingModule/src/Module.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 638b262c5..a33018eee 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/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. */ diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index d74bb0fc1..dd2d5162e 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/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,17 @@ 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)) + { + yWarning() << "[WalkingModule::updateModule] The goal has not been set for more than " << m_maxTimeToWaitForGoal << " seconds."; + 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 From bc0b6cf9bbdc1ca71960f175dafb4fa1ac69778e Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 14 Oct 2024 11:32:56 +0200 Subject: [PATCH 3/3] Remove useless print --- src/WalkingModule/src/Module.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index dd2d5162e..dfb14dbd5 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -680,7 +680,6 @@ bool WalkingModule::updateModule() } else if (!m_firstRun && ((m_time - m_lastSetGoalTime) > m_maxTimeToWaitForGoal)) { - yWarning() << "[WalkingModule::updateModule] The goal has not been set for more than " << m_maxTimeToWaitForGoal << " seconds."; yarp::sig::Vector dummy(3, 0.0); if (!setPlannerInput(dummy)) {