Skip to content

Commit

Permalink
Add torso torque controlled. Remove torso task.
Browse files Browse the repository at this point in the history
  • Loading branch information
ergocub committed Oct 23, 2024
1 parent c522128 commit 4cdfc38
Show file tree
Hide file tree
Showing 4 changed files with 39 additions and 29 deletions.
2 changes: 1 addition & 1 deletion ...ancing-torque-control/config/robots/ergoCubSN001/blf-balancing-torque-control-options.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ contact_force_threshold 0.1 # in Newton


; com_knots_delta_x (0.0, 0.0, 0.03, 0.03, -0.03, -0.03, 0.0, 0.0) # in meter
; com_knots_delta_y (0.0, 0.05, 0.05, -0.05, -0.05, 0.05, 0.05, 0.0) # in meter
; com_knots_delta_y (0.0, -0.04, -0.04, 0.04, 0.04, -0.04, -0.04, 0.0) # in meter
; com_knots_delta_z (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter

com_knots_delta_x (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # in meter
Expand Down
18 changes: 10 additions & 8 deletions ...-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/robot_control.ini
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,18 @@
robot_name ergocub

joints_list ("l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
joints_list ("torso_pitch", "torso_roll", "torso_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")

joint_torque_lower_limits (-10.0, -15.0, -3.0, -5.0, -15.0, -15.0,
joint_torque_lower_limits (-50.0, -50.0, -50.0,
-10.0, -15.0, -3.0, -5.0, -15.0, -15.0,
-10.0, -15.0, -3.0, -5.0, -15.0, -15.0)

joint_torque_upper_limits ( 30.0, 15.0, 3.0, 30.0, 15.0, 15.0,
joint_torque_upper_limits ( 50.0, 50.0, 50.0,
30.0, 15.0, 3.0, 30.0, 15.0, 15.0,
30.0, 15.0, 3.0, 30.0, 15.0, 15.0)

remote_control_boards ("left_leg", "right_leg")
remote_control_boards ("torso", "left_leg", "right_leg")

positioning_duration 3.0 #in seconds
positioning_tolerance 0.05 #in radians
Expand All @@ -26,9 +29,8 @@ position_direct_max_admissible_error 0.1 #in radians
# fixed_joint_list_names ("torso_pitch") # List of the fixed joint names
# fixed_joint_list_values (10.0) # List of the fixed joint values (in degrees)

fixed_joint_list_names ("torso_pitch", "torso_roll", "torso_yaw",
"l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
fixed_joint_list_names ("l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
"r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow")
fixed_joint_list_values (7.955, 0.051, -0.008,
-3.778, 19.433, -12.942, 40.664,

fixed_joint_list_values (-3.778, 19.433, -12.942, 40.664,
-3.780, 19.428, -12.942, 40.665)
34 changes: 18 additions & 16 deletions ...balancing-torque-control/config/robots/ergoCubSN001/blf_balancing_torque_control/tsid.ini
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
tasks ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK",
"JOINT_REGULARIZATION_TASK", "TORSO_TASK",
"JOINT_REGULARIZATION_TASK",
"JOINT_DYNAMICS_TASK", "BASE_DYNAMICS_TASK",
"LF_WRENCH_TASK", "RF_WRENCH_TASK")

[VARIABLES]
variables_name ("robot_acceleration", "joint_torques", "lf_wrench", "rf_wrench")
variables_size (18, 12, 6, 6)
variables_size (21, 15, 6, 6)

[TSID]
robot_acceleration_variable_name "robot_acceleration"
Expand All @@ -16,8 +16,8 @@ contact_wrench_variables_name ("lf_wrench", "rf_wrench")
name com_task
type CoMTask
robot_acceleration_variable_name "robot_acceleration"
kp_linear 30.0
kd_linear 0.0
kp_linear 50.0
kd_linear 1.0
mask (true, true, true)
priority 0

Expand All @@ -28,8 +28,8 @@ robot_acceleration_variable_name "robot_acceleration"
frame_name "r_sole"
kp_linear 5.0
kp_angular 5.0
kd_linear 0.0
kd_angular 0.0
kd_linear 1.0
kd_angular 1.0
priority 0

[LEFT_FOOT_TASK]
Expand All @@ -39,8 +39,8 @@ robot_acceleration_variable_name "robot_acceleration"
frame_name "l_sole"
kp_linear 5.0
kp_angular 5.0
kd_linear 0.0
kd_angular 0.0
kd_linear 1.0
kd_angular 1.0
priority 0

[TORSO_TASK]
Expand All @@ -51,21 +51,23 @@ frame_name "chest"
kp_angular 20.0
kd_angular 0.0
priority 1
weight (3.0, 3.0, 3.0)
weight (5.0, 5.0, 5.0)

[JOINT_REGULARIZATION_TASK]
name joint_regularization_task
type JointTrackingTask
robot_acceleration_variable_name "robot_acceleration"
kp (100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
100.0, 100.0, 100.0, 100.0, 100.0, 100.0)

; kd (1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
; 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)
kp (70.0, 70.0, 70.0,
120.0, 120.0, 120.0, 120.0, 120.0, 120.0,
120.0, 120.0, 120.0, 120.0, 120.0, 120.0)
kd (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, 1.0)

priority 1
weight (5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
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)

[LF_WRENCH_TASK]
name "lf_feasibility_wrench_task"
Expand Down
14 changes: 10 additions & 4 deletions utilities/balancing-torque-control/script/blf-balancing-torque-control.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,10 +190,16 @@ def __init__(self, config_file: str):
"Impossible to set the set point for the joint regularization task."
)

if not self.tsid.tasks["torso_task"].set_set_point(
manif.SO3.Identity(), manif.SO3Tangent.Zero(), manif.SO3Tangent.Zero()
):
raise ValueError("Impossible to set the set point for the torso task.")
# torso_initial_orientation = self.kindyn_with_measured.getWorldTransform("chest")

# if not self.tsid.tasks["torso_task"].set_set_point(
# blf.conversions.to_manif_rot(
# torso_initial_orientation.getRotation().toNumPy()
# ),
# manif.SO3Tangent.Zero(),
# manif.SO3Tangent.Zero()
# ):
# raise ValueError("Impossible to set the set point for the torso task.")

self.desired_joint_positions = self.joint_positions.copy()
self.desired_joint_velocities = self.joint_velocities.copy()
Expand Down

0 comments on commit 4cdfc38

Please sign in to comment.