Skip to content

Commit

Permalink
Merge pull request #140 from robotology/ergoCubSN000_retargeting
Browse files Browse the repository at this point in the history 
🤖 [ergoCubSN000] [ergoCubGazeboV1] Add configuration files for the iFeel retargeting
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi authored Mar 7, 2023
2 parents 4cb53a1 + d8615af commit 9b6fa8d
Show file tree
Hide file tree
Showing 31 changed files with 662 additions and 23 deletions.
2 changes: 1 addition & 1 deletion cmake/WalkingControllersDependencies.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ find_package(ICUB REQUIRED)
find_package(ICUBcontrib REQUIRED)
find_package(iDynTree REQUIRED)
find_package(Eigen3 3.2.92 REQUIRED)
find_package(UnicyclePlanner 0.5.2 REQUIRED)
find_package(UnicyclePlanner 0.6.0 REQUIRED)
find_package(OsqpEigen 0.4.0 REQUIRED)
find_package(qpOASES REQUIRED)
find_package(BipedalLocomotionFramework 0.9.0
Expand Down
3 changes: 2 additions & 1 deletion src/TrajectoryPlanner/src/TrajectoryGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
double slowWhenBackwardFactor = config.check("slowWhenBackwardFactor", yarp::os::Value(1.0)).asFloat64();
double slowWhenSidewaysFactor = config.check("slowWhenSidewaysFactor", yarp::os::Value(1.0)).asFloat64();
double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asFloat64();
double maxStepLengthBackwardMultiplier = config.check("maxLengthBackwardFactor", yarp::os::Value(1.0)).asFloat64();
double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asFloat64();
double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asFloat64();
double maxAngleVariation = iDynTree::deg2rad(config.check("maxAngleVariation",
Expand Down Expand Up @@ -161,7 +162,7 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config)
m_referencePointDistance(1));
ok = ok && unicyclePlanner->setPersonFollowingControllerGain(unicycleGain);
ok = ok && unicyclePlanner->setMaximumIntegratorStepSize(m_dT);
ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength);
ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength, maxStepLengthBackwardMultiplier);
ok = ok && unicyclePlanner->setWidthSetting(minWidth, m_nominalWidth);
ok = ok && unicyclePlanner->setMaxAngleVariation(maxAngleVariation);
ok = ok && unicyclePlanner->setCostWeights(positionWeight, timeWeight);
Expand Down
26 changes: 13 additions & 13 deletions src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/pidParams.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,16 @@ useGainScheduling 0

[DEFAULT]
#NAME P I D
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
#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
1 change: 1 addition & 0 deletions src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/plannerParams.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ saturationFactors (0.7, 0.7)
#Step length
maxStepLength 0.25
minStepLength 0.01
maxLengthBackwardFactor 1.0
#Width
minWidth 0.12
#Angle Variations in DEGREES
Expand Down
28 changes: 28 additions & 0 deletions ...dule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini
View file
Original file line number Diff line number Diff line change
@@ -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,
12.0, 7.0, -12.0, 41.0, -28.3, 0, 0,
12.0, 7.0, -12.0, 41.0, -28.3, 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)
26 changes: 26 additions & 0 deletions ...odule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
approaching_phase_duration 4.0
hde_port_name /humanState:i

[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_roll", "l_wrist_pitch", "l_wrist_yaw",
"r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch", "r_wrist_yaw")

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
31 changes: 31 additions & 0 deletions ...app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini
View file
Original file line number Diff line number Diff line change
@@ -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"]
36 changes: 36 additions & 0 deletions ...ingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/robotControl.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
robot ergocubSim


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_roll", "l_wrist_pitch", "l_wrist_yaw",
"r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "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,
true, true, true, true, true, true)

# if true a good joint traking is considered mandatory
good_tracking_required (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)
37 changes: 37 additions & 0 deletions ...e/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
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, 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)

states ("STANCE", "WALKING")
sampling_time 0.01
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)

[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,
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)

37 changes: 37 additions & 0 deletions ...dule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini
View file
Original file line number Diff line number Diff line change
@@ -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)

states ("STANCE", "WALKING")
sampling_time 0.01
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, 5.0, 5.0,
2.0, 2.0, 2.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)

[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,
0.0, 0.0, 0.0,
2.0, 0.0, 2.0, 2.0, 2.0, 5.0, 5.0,
2.0, 0.0, 2.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)
16 changes: 16 additions & 0 deletions ...kingModule/app/robots/ergoCubGazeboV1/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
robot_velocity_variable_name "robot_velocity"
frame_name "chest"
kp_angular 5.0


states ("STANCE", "WALKING")
sampling_time 0.01
settling_time 3.0

[STANCE]
name "stance"
weight (0.0, 0.0, 0.0)

[WALKING]
name "walking"
weight (5.0, 5.0, 5.0)
102 changes: 102 additions & 0 deletions src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_iFeel_joint_retargeting.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@
# 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 use osqp to
# solve QP-IK. In this case qpOASES will be used
use_osqp 1

use_BLF-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

# Tolerance radius to consider the measured ZMP constant.
constant_ZMP_tolerance 0.000001

# 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 125

# 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.35, 0.25)

# 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.02

# 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.70
# sampling time
sampling_time 0.01
# 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 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/iFeel_joint_retargeting/inverseKinematics.ini"]

# include qp inverse kinematcs parameters
[include INVERSE_KINEMATICS_BLF_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 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"]
Loading

0 comments on commit 9b6fa8d

Please sign in to comment.