Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve JaxSim backend #24

Open
wants to merge 13 commits into
base: main
Choose a base branch
from
+165 −115
Open

Improve JaxSim backend #24

Show file tree
Hide file tree
Changes from 12 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
962b2a9
Sort imports
diegoferigo Oct 22, 2024
1ec9653
Update JaxsimSimulator class
diegoferigo Oct 22, 2024
d1012d6
Regularize the joints with the desired position
diegoferigo Oct 22, 2024
5468ad8
Regularize the TSID wrenches with those planned by the MPC
diegoferigo Oct 22, 2024
e227947
Allow scaling the foot shape considered in the centroidal mpc
diegoferigo Oct 22, 2024
0017d38
Sort imports
diegoferigo Oct 22, 2024
dffd6e8
Provide working configuration with soft-like contact models
diegoferigo Oct 22, 2024
02171c0
Step using the dt passed to JaxsimSimulator
diegoferigo Nov 5, 2024
3c49ec0
Update how the integrator is stored in JaxsimSimulator
diegoferigo Nov 5, 2024
7508ad6
Remove to specify custom parameter in RelaxedRigidContactsParams
diegoferigo Nov 5, 2024
4495950
Make planners more conservative on the considered friction
diegoferigo Nov 5, 2024
54cdbce
Make step function compatible with base class
diegoferigo Nov 5, 2024
dfffeab
Adjust TSID and MPC parameters in `jaxsim_walking` notebook to improv…
xela-95 Nov 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
71 changes: 58 additions & 13 deletions src/comodo/TSIDController/TSIDController.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,14 +51,16 @@ def define_tasks(self, tsid_parameters: TSIDParameterTuning):
value=self.robot_acceleration_variable_name,
)
self.controller_variable_handler.set_parameter_string(
name="joint_torques_variable_name", value=self.joint_torques_variable_name
name="joint_torques_variable_name",
value=self.joint_torques_variable_name,
)
vector_string_contact = [
self.variable_name_left_contact,
self.variable_name_right_contact,
]
self.controller_variable_handler.set_parameter_vector_string(
name="contact_wrench_variables_name", value=vector_string_contact
name="contact_wrench_variables_name",
value=vector_string_contact,
)
self.controller.initialize(self.controller_variable_handler)
self.define_varible_handler()
Expand Down Expand Up @@ -136,7 +138,7 @@ def define_tasks(self, tsid_parameters: TSIDParameterTuning):
)
self.left_foot_param_handler.set_parameter_int(name="number_of_slices", value=2)
self.left_foot_param_handler.set_parameter_float(
name="static_friction_coefficient", value=1.0
name="static_friction_coefficient", value=0.33
)
self.left_foot_param_handler.set_parameter_vector_float(
name="foot_limits_x", value=[-0.12, 0.12]
Expand Down Expand Up @@ -177,6 +179,38 @@ def define_tasks(self, tsid_parameters: TSIDParameterTuning):
param_handler=self.right_foot_param_handler
)

## Left foot wrench regularization task --> Providing reference from MPC.
self.left_foot_wrench_reg_task = blf.tsid.VariableRegularizationTask()
self.left_foot_wrench_reg_task_name = "left_foot_wrench_reg_task"
self.left_foot_wrench_reg_priority = 1
self.left_foot_wrench_reg_weight = np.array([10.0] * 3 + [0] * 3)
self.left_foot_wrench_reg_param_handler = (
blf.parameters_handler.StdParametersHandler()
)
self.left_foot_wrench_reg_param_handler.set_parameter_string(
"variable_name", self.variable_name_left_contact
)
self.left_foot_wrench_reg_param_handler.set_parameter_int("variable_size", 6)
self.left_foot_wrench_reg_task.initialize(
param_handler=self.left_foot_wrench_reg_param_handler
)

## Right foot wrench regularization task --> Providing reference from MPC.
self.right_foot_wrench_reg_task = blf.tsid.VariableRegularizationTask()
self.right_foot_wrench_reg_task_name = "right_foot_wrench_reg_task"
self.right_foot_wrench_reg_priority = 1
self.right_foot_wrench_reg_weight = np.array([10.0] * 3 + [0] * 3)
self.right_foot_wrench_reg_param_handler = (
blf.parameters_handler.StdParametersHandler()
)
self.right_foot_wrench_reg_param_handler.set_parameter_string(
"variable_name", self.variable_name_right_contact
)
self.right_foot_wrench_reg_param_handler.set_parameter_int("variable_size", 6)
self.right_foot_wrench_reg_task.initialize(
param_handler=self.right_foot_wrench_reg_param_handler
)

## Base"" Dynamics Task --> Base dynamic constraint
self.base_dynamic_task = blf.tsid.BaseDynamicsTask()
self.base_dynamic_task_name = "base_dynamic_task"
Expand Down Expand Up @@ -322,6 +356,18 @@ def define_tasks(self, tsid_parameters: TSIDParameterTuning):
self.right_foot_wrench_task_name,
self.right_foot_wrench_priority,
)
self.controller.add_task(
self.left_foot_wrench_reg_task,
self.left_foot_wrench_reg_task_name,
self.left_foot_wrench_reg_priority,
self.left_foot_wrench_reg_weight,
)
self.controller.add_task(
self.right_foot_wrench_reg_task,
self.right_foot_wrench_reg_task_name,
self.right_foot_wrench_reg_priority,
self.right_foot_wrench_reg_weight,
)
self.controller.add_task(
self.joint_dynamic_task,
self.joint_dynamic_task_name,
Expand Down Expand Up @@ -444,17 +490,16 @@ def update_task_references_mpc(
left_contact=left_foot_desired.is_in_contact,
right_contact=right_foot_desired.is_in_contact,
)

# Regularize the joints with the desired positions
self.joint_regularization_task.set_set_point(s_desired)
# self.angular_momentum_task.set_set_point(np.zeros(3), np.zeros(3))
wrench_desired_left = np.zeros(6)
wrench_desired_right = np.zeros(6)
# mass = self.robot_model.get_total_mass()
# wrench_desired[2] = -(mass*9.81/2)
# wrench_desiredp[]
wrench_desired_left[:3] = wrenches_left
wrench_desired_right[:3] = wrenches_right
# self.left_foot_regularization_task.set_set_point(wrench_desired_left)
# self.right_foot_regularization_task.set_set_point(wrench_desired_right)

# Regularize the feet wrenches with the planned ones
mass = self.kindyn.model().getTotalMass()
wrench_desired_left = mass * wrenches_left
wrench_desired_right = mass * wrenches_right
self.left_foot_wrench_reg_task.set_set_point(wrench_desired_left)
self.right_foot_wrench_reg_task.set_set_point(wrench_desired_right)

def update_com_task(self):

Expand Down
42 changes: 28 additions & 14 deletions src/comodo/centroidalMPC/centroidalMPC.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,12 @@
from comodo.abstractClasses.planner import Planner
from comodo.centroidalMPC.footPositionPlanner import FootPositionPlanner
import bipedal_locomotion_framework as blf
from datetime import timedelta

import bipedal_locomotion_framework as blf
import idyntree.bindings as iDynTree
import numpy as np
import matplotlib.pyplot as plt
import numpy as np

from comodo.abstractClasses.planner import Planner
from comodo.centroidalMPC.footPositionPlanner import FootPositionPlanner
from comodo.centroidalMPC.mpcParameterTuning import MPCParameterTuning


Expand Down Expand Up @@ -67,7 +69,7 @@ def set_state_with_base(self, s, s_dot, H_b, w_b, t):
self.H_b = H_b
self.kindyn.setRobotState(self.H_b, self.s, self.w_b, self.s_dot, self.gravity)

def intialize_mpc(self, mpc_parameters: MPCParameterTuning):
def intialize_mpc(self, mpc_parameters: MPCParameterTuning, scale: float = 1.0):
time_horizon = timedelta(seconds=1.2)

## MPC Param Hanlder
Expand All @@ -81,7 +83,7 @@ def intialize_mpc(self, mpc_parameters: MPCParameterTuning):
self.mpc_param_handler.set_parameter_int("solver_verbosity", 0)
self.mpc_param_handler.set_parameter_int("number_of_maximum_contacts", 2)
self.mpc_param_handler.set_parameter_int("number_of_slices", 1)
self.mpc_param_handler.set_parameter_float("static_friction_coefficient", 0.33)
self.mpc_param_handler.set_parameter_float("static_friction_coefficient", 0.50)
self.mpc_param_handler.set_parameter_string("linear_solver", "mumps")
self.mpc_param_handler.set_parameter_string("solver_name", "ipopt")

Expand All @@ -90,12 +92,18 @@ def intialize_mpc(self, mpc_parameters: MPCParameterTuning):
self.contact_0_handler = blf.parameters_handler.StdParametersHandler()
self.contact_0_handler.set_parameter_int("number_of_corners", 4)
self.contact_0_handler.set_parameter_string("contact_name", "left_foot")
self.contact_0_handler.set_parameter_vector_float("corner_0",self.robot_model.corner_0)
self.contact_0_handler.set_parameter_vector_float("corner_1", self.robot_model.corner_1)
self.contact_0_handler.set_parameter_vector_float(
"corner_2", self.robot_model.corner_2
"corner_0", scale * self.robot_model.corner_0
)
self.contact_0_handler.set_parameter_vector_float(
"corner_1", scale * self.robot_model.corner_1
)
self.contact_0_handler.set_parameter_vector_float(
"corner_2", scale * self.robot_model.corner_2
)
self.contact_0_handler.set_parameter_vector_float(
"corner_3", scale * self.robot_model.corner_3
)
self.contact_0_handler.set_parameter_vector_float("corner_3", self.robot_model.corner_3)
self.contact_0_handler.set_parameter_vector_float(
"bounding_box_lower_limit", [0.0, 0.0, 0.0]
)
Expand All @@ -106,12 +114,18 @@ def intialize_mpc(self, mpc_parameters: MPCParameterTuning):
self.contact_1_handler = blf.parameters_handler.StdParametersHandler()
self.contact_1_handler.set_parameter_int("number_of_corners", 4)
self.contact_1_handler.set_parameter_string("contact_name", "right_foot")
self.contact_1_handler.set_parameter_vector_float("corner_0", self.robot_model.corner_0)
self.contact_1_handler.set_parameter_vector_float("corner_1", self.robot_model.corner_1)
self.contact_1_handler.set_parameter_vector_float(
"corner_2", self.robot_model.corner_2
"corner_0", scale * self.robot_model.corner_0
)
self.contact_1_handler.set_parameter_vector_float(
"corner_1", scale * self.robot_model.corner_1
)
self.contact_1_handler.set_parameter_vector_float(
"corner_2", scale * self.robot_model.corner_2
)
self.contact_1_handler.set_parameter_vector_float(
"corner_3", scale * self.robot_model.corner_3
)
self.contact_1_handler.set_parameter_vector_float("corner_3", self.robot_model.corner_3)
self.contact_1_handler.set_parameter_vector_float(
"bounding_box_lower_limit", [0.0, 0.0, 0.0]
)
Expand Down
Loading