From 9b9490c4e6e59d11ae59ad6b70e22bde9b979cc8 Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Wed, 9 Oct 2024 11:22:55 +0200 Subject: [PATCH 1/2] [WIP] Removed unused links and modified ET root fetching --- src/comodo/mujocoSimulator/mujocoSimulator.py | 60 +++++---- src/comodo/robotModel/robotModel.py | 120 +++++++++++++----- 2 files changed, 120 insertions(+), 60 deletions(-) diff --git a/src/comodo/mujocoSimulator/mujocoSimulator.py b/src/comodo/mujocoSimulator/mujocoSimulator.py index 8814108..2fd25b6 100644 --- a/src/comodo/mujocoSimulator/mujocoSimulator.py +++ b/src/comodo/mujocoSimulator/mujocoSimulator.py @@ -16,7 +16,7 @@ def __init__(self) -> None: def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): self.robot_model = robot_model - + mujoco_xml = robot_model.get_mujoco_model() self.model = mujoco.MjModel.from_xml_string(mujoco_xml) self.data = mujoco.MjData(self.model) @@ -32,17 +32,16 @@ def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): self.kv_motors = ( kv_motors if not (kv_motors is None) else np.zeros(self.robot_model.NDoF) ) - self.H_left_foot = copy.deepcopy(self.robot_model.H_left_foot) - self.H_right_foot = copy.deepcopy(self.robot_model.H_right_foot) - self.H_left_foot_num = None - self.H_right_foot_num = None + # self.H_left_foot = copy.deepcopy(self.robot_model.H_left_foot) + # self.H_right_foot = copy.deepcopy(self.robot_model.H_right_foot) + # self.H_left_foot_num = None + # self.H_right_foot_num = None self.mass = self.robot_model.get_total_mass() - def get_contact_status(self): left_wrench, rigth_wrench = self.get_feet_wrench() - left_foot_contact = left_wrench[2] > 0.1*self.mass - right_foot_contact = rigth_wrench[2] > 0.1*self.mass + left_foot_contact = left_wrench[2] > 0.1 * self.mass + right_foot_contact = rigth_wrench[2] > 0.1 * self.mass return left_foot_contact, right_foot_contact def set_visualize_robot_flag(self, visualize_robot): @@ -81,8 +80,10 @@ def create_mapping_vector_to_mujoco(self): index = self.robot_model.joint_name_list.index(mujoco_joint) self.to_mujoco.append(index) except ValueError: - raise ValueError(f"Mujoco joint '{mujoco_joint}' not found in joint list.") - + raise ValueError( + f"Mujoco joint '{mujoco_joint}' not found in joint list." + ) + def create_mapping_vector_from_mujoco(self): # This function creates the to_mujoco map self.from_mujoco = [] @@ -91,26 +92,33 @@ def create_mapping_vector_from_mujoco(self): index = self.robot_model.mujoco_joint_order.index(joint) self.from_mujoco.append(index) except ValueError: - raise ValueError(f"Joint name list joint '{joint}' not found in mujoco list.") - - def convert_vector_to_mujoco (self, array_in): - out_muj = np.asarray([array_in[self.to_mujoco[item]] for item in range(self.robot_model.NDoF)]) + raise ValueError( + f"Joint name list joint '{joint}' not found in mujoco list." + ) + + def convert_vector_to_mujoco(self, array_in): + out_muj = np.asarray( + [array_in[self.to_mujoco[item]] for item in range(self.robot_model.NDoF)] + ) return out_muj - + def convert_from_mujoco(self, array_muj): - out_classic = np.asarray([array_muj[self.from_mujoco[item]] for item in range(self.robot_model.NDoF)]) + out_classic = np.asarray( + [array_muj[self.from_mujoco[item]] for item in range(self.robot_model.NDoF)] + ) return out_classic def step(self, n_step=1, visualize=True): if self.postion_control: for _ in range(n_step): s, s_dot, tau = self.get_state(use_mujoco_convention=True) - kp_muj = self.convert_vector_to_mujoco(self.robot_model.kp_position_control) - kd_muj = self.convert_vector_to_mujoco(self.robot_model.kd_position_control) - ctrl = ( - kp_muj * (self.desired_pos - s) - - kd_muj * s_dot + kp_muj = self.convert_vector_to_mujoco( + self.robot_model.kp_position_control ) + kd_muj = self.convert_vector_to_mujoco( + self.robot_model.kd_position_control + ) + ctrl = kp_muj * (self.desired_pos - s) - kd_muj * s_dot self.data.ctrl = ctrl np.copyto(self.data.ctrl, ctrl) mujoco.mj_step(self.model, self.data) @@ -175,7 +183,7 @@ def check_feet_status(self, s, H_b): def get_feet_wrench(self): left_foot_wrench = np.zeros(6) rigth_foot_wrench = np.zeros(6) - s,s_dot, tau = self.get_state() + s, s_dot, tau = self.get_state() H_b = self.get_base() self.H_left_foot_num = np.array(self.H_left_foot(H_b, s)) self.H_right_foot_num = np.array(self.H_right_foot(H_b, s)) @@ -204,7 +212,7 @@ def get_feet_wrench(self): wrench_LF = self.compute_resulting_wrench(LF_H_contact, c_array) left_foot_wrench[:] += wrench_LF.reshape(6) return (left_foot_wrench, rigth_foot_wrench) - + def compute_resulting_wrench(self, b_H_a, force_torque_a): p = b_H_a[:3, 3] R = b_H_a[:3, :3] @@ -257,14 +265,14 @@ def get_base_velocity(self): indexes_joint_velocities = self.model.jnt_dofadr[1:] return self.data.qvel[: indexes_joint_velocities[0]] - def get_state(self, use_mujoco_convention = False): + def get_state(self, use_mujoco_convention=False): indexes_joint = self.model.jnt_qposadr[1:] indexes_joint_velocities = self.model.jnt_dofadr[1:] s = self.data.qpos[indexes_joint[0] :] s_dot = self.data.qvel[indexes_joint_velocities[0] :] tau = self.data.ctrl - if(use_mujoco_convention): - return s,s_dot,tau + if use_mujoco_convention: + return s, s_dot, tau s_out = self.convert_from_mujoco(s) s_dot_out = self.convert_from_mujoco(s_dot) tau_out = self.convert_from_mujoco(tau) diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index 77cc373..e6ea68d 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -16,35 +16,79 @@ class RobotModel(KinDynComputations): def __init__( self, - urdfstring: str, + urdf_path: str, robot_name: str, joint_name_list: list, - base_link: str = "root_link", - left_foot: str = "l_sole", - right_foot: str = "r_sole", - torso: str = "chest", - right_foot_rear_link_name: str = "r_foot_rear", - right_foot_front_link_name: str = "r_foot_front", - left_foot_rear_link_name: str = "l_foot_rear", - left_foot_front_link_name: str = "l_foot_front", + base_link: str = "root", + left_foot: str = None, + right_foot: str = None, + torso: str = None, + right_foot_rear_link_name: str = None, + right_foot_front_link_name: str = None, + left_foot_rear_link_name: str = None, + left_foot_front_link_name: str = None, kp_pos_control: np.float32 = np.array( - [35 * 70.0, 35 * 70.0, 35 * 40.0, 35 * 100.0, 35 * 100.0, 35 * 100.0,35 * 70.0, 35 * 70.0, 35 * 40.0, 35 * 100.0, 35 * 100.0, 35 * 100.0,20 * 5.745, 20 * 5.745, 20 * 5.745, 20 * 1.745,20 * 5.745, 20 * 5.745, 20 * 5.745, 20 * 1.745] + [ + 35 * 70.0, + 35 * 70.0, + 35 * 40.0, + 35 * 100.0, + 35 * 100.0, + 35 * 100.0, + 35 * 70.0, + 35 * 70.0, + 35 * 40.0, + 35 * 100.0, + 35 * 100.0, + 35 * 100.0, + 20 * 5.745, + 20 * 5.745, + 20 * 5.745, + 20 * 1.745, + 20 * 5.745, + 20 * 5.745, + 20 * 5.745, + 20 * 1.745, + ] + ), + kd_pos_control: np.float32 = np.array( + [ + 15 * 0.15, + 15 * 0.15, + 15 * 0.35, + 15 * 0.15, + 15 * 0.15, + 15 * 0.15, + 15 * 0.15, + 15 * 0.15, + 15 * 0.35, + 15 * 0.15, + 15 * 0.15, + 15 * 0.15, + 4 * 5.745, + 4 * 5.745, + 4 * 5.745, + 4 * 1.745, + 4 * 5.745, + 4 * 5.745, + 4 * 5.745, + 4 * 1.745, + ] ), - kd_pos_control: np.float32= np.array([15 * 0.15, 15 * 0.15, 15 * 0.35, 15 * 0.15, 15 * 0.15, 15 * 0.15,15 * 0.15, 15 * 0.15, 15 * 0.35, 15 * 0.15, 15 * 0.15, 15 * 0.15,4 * 5.745, 4 * 5.745, 4 * 5.745, 4 * 1.745,4 * 5.745, 4 * 5.745, 4 * 5.745, 4 * 1.745]) ) -> None: self.collision_keyword = "_collision" self.visual_keyword = "_visual" - self.urdf_string = urdfstring + self.urdf_path = urdf_path self.robot_name = robot_name self.joint_name_list = joint_name_list self.base_link = base_link self.left_foot_frame = left_foot self.right_foot_frame = right_foot - self.torso_link = torso - self.right_foot_rear_ct = right_foot_rear_link_name + self.collision_keyword - self.right_foot_front_ct = right_foot_front_link_name + self.collision_keyword - self.left_foot_rear_ct = left_foot_rear_link_name + self.collision_keyword - self.left_foot_front_ct = left_foot_front_link_name + self.collision_keyword + # self.torso_link = torso + # self.right_foot_rear_ct = right_foot_rear_link_name + self.collision_keyword + # self.right_foot_front_ct = right_foot_front_link_name + self.collision_keyword + # self.left_foot_rear_ct = left_foot_rear_link_name + self.collision_keyword + # self.left_foot_front_ct = left_foot_front_link_name + self.collision_keyword self.remote_control_board_list = [ "/" + self.robot_name + "/torso", @@ -55,15 +99,15 @@ def __init__( ] self.kp_position_control = kp_pos_control - self.kd_position_control = kd_pos_control + self.kd_position_control = kd_pos_control self.ki_position_control = 10 * self.kd_position_control self.gravity = iDynTree.Vector3() self.gravity.zero() self.gravity.setVal(2, -9.81) self.H_b = iDynTree.Transform() - super().__init__(urdfstring, self.joint_name_list, self.base_link) - self.H_left_foot = self.forward_kinematics_fun(self.left_foot_frame) - self.H_right_foot = self.forward_kinematics_fun(self.right_foot_frame) + super().__init__(urdf_path, self.joint_name_list, self.base_link) + # self.H_left_foot = self.forward_kinematics_fun(self.left_foot_frame) + # self.H_right_foot = self.forward_kinematics_fun(self.right_foot_frame) def override_control_boar_list(self, remote_control_board_list: list): self.remote_control_board_list = remote_control_board_list @@ -91,7 +135,7 @@ def set_limbs_indexes( def get_idyntree_kyndyn(self): model_loader = iDynTree.ModelLoader() model_loader.loadReducedModelFromString( - copy.deepcopy(self.urdf_string), self.joint_name_list + copy.deepcopy(self.urdf_path), self.joint_name_list ) kindyn = iDynTree.KinDynComputations() kindyn.loadRobotModel(model_loader.model()) @@ -128,11 +172,11 @@ def compute_desired_position_walking(self): for index, joint_name in enumerate(self.joint_name_list): if "knee" in joint_name: self.solver.subject_to(self.s[index] == desired_knee) - if "shoulder_roll" in joint_name: + if "shoulder_roll" in joint_name: self.solver.subject_to(self.s[index] == shoulder_roll) - if "elbow" in joint_name: - self.solver.subject_to(self.s[index] == elbow) - + if "elbow" in joint_name: + self.solver.subject_to(self.s[index] == elbow) + self.solver.subject_to(H_left_foot[2, 3] == 0.0) self.solver.subject_to(H_right_foot[2, 3] == 0.0) self.solver.subject_to(quat_left_foot == reference_rotation) @@ -183,15 +227,15 @@ def compute_base_pose_left_foot_in_contact(self, s): w_H_init = np.linalg.inv(w_H_lefFoot_num) @ w_H_torso_num return w_H_init - def compute_com_init(self): + def compute_com_init(self): com = self.CoM_position_fun() - return np.array(com(self.w_H_b_init,self.s_init)) + return np.array(com(self.w_H_b_init, self.s_init)) - def set_initial_position(self, s_init, w_H_b_init, xyz_rpy_init): + def set_initial_position(self, s_init, w_H_b_init, xyz_rpy_init): self.s_init = s_init self.w_H_b_init = w_H_b_init self.xyz_rpy_init = xyz_rpy_init - + def rotation_matrix_to_quaternion(self, R): # Ensure the matrix is a valid rotation matrix (orthogonal with determinant 1) trace = cs.trace(R) @@ -210,11 +254,17 @@ def rotation_matrix_to_quaternion(self, R): return cs.vertcat(w, x, y, z) - def get_mujoco_urdf_string(self): + def get_mujoco_urdf_string(self) -> str: ## We first start by ET tempFileOut = tempfile.NamedTemporaryFile(mode="w+") - tempFileOut.write(copy.deepcopy(self.urdf_string)) - root = ET.fromstring(self.urdf_string) + tempFileOut.write(copy.deepcopy(self.urdf_path)) + + with open(tempFileOut.name, "r") as file: + data = file.read() + + parser = ET.XMLParser(encoding="utf-8") + tree = ET.parse(self.urdf_path, parser=parser) + root = tree.getroot() self.mujoco_joint_order = [] # Declaring as fixed the not controlled joints for joint in root.findall(".//joint"): @@ -276,11 +326,13 @@ def get_mujoco_urdf_string(self): mujoco_el.append(compiler_el) robot_el.append(mujoco_el) # Convert the XML tree to a string - robot_urdf_string_original = ET.tostring(root) + robot_urdf_string_original = ET.tostring(root, encoding="unicode") return robot_urdf_string_original def get_mujoco_model(self): urdf_string = self.get_mujoco_urdf_string() + with open("temp.urdf", "w+") as f: + f.write(urdf_string) mujoco_model = mujoco.MjModel.from_xml_string(urdf_string) path_temp_xml = tempfile.NamedTemporaryFile(mode="w+") From 5a8fcad2e83e4f0c3b689691a54dc672a6cdf404 Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Wed, 11 Dec 2024 18:04:22 +0100 Subject: [PATCH 2/2] Raising exception if root link is missing in urdf to avoid misleading xml exception --- src/comodo/robotModel/robotModel.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index e6ea68d..c0c0ffc 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -265,6 +265,9 @@ def get_mujoco_urdf_string(self) -> str: parser = ET.XMLParser(encoding="utf-8") tree = ET.parse(self.urdf_path, parser=parser) root = tree.getroot() + has_root_link = "root" in [l.attrib['name'] for l in root.findall(".//link")] + if not has_root_link: + raise ValueError("The URDF file must have a link named 'root'") self.mujoco_joint_order = [] # Declaring as fixed the not controlled joints for joint in root.findall(".//joint"):