From 9b9490c4e6e59d11ae59ad6b70e22bde9b979cc8 Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Wed, 9 Oct 2024 11:22:55 +0200 Subject: [PATCH 1/6] [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 f47e355691bd5356785452f2f14796fc1b7ec40f Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Thu, 10 Oct 2024 11:50:52 +0200 Subject: [PATCH 2/6] Added doc-strings and return type hinting in mujocoSimulator --- src/comodo/mujocoSimulator/mujocoSimulator.py | 304 ++++++++++++++++-- 1 file changed, 279 insertions(+), 25 deletions(-) diff --git a/src/comodo/mujocoSimulator/mujocoSimulator.py b/src/comodo/mujocoSimulator/mujocoSimulator.py index 2fd25b6..95bcc70 100644 --- a/src/comodo/mujocoSimulator/mujocoSimulator.py +++ b/src/comodo/mujocoSimulator/mujocoSimulator.py @@ -1,4 +1,5 @@ from comodo.abstractClasses.simulator import Simulator +from comodo.robotModel.robotModel import RobotModel import mujoco import math import numpy as np @@ -14,7 +15,20 @@ def __init__(self) -> None: self.compute_misalignment_gravity_fun() super().__init__() - def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): + def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=None) -> None: + """ + Loads the robot model into the MuJoCo simulator. + + Args: + robot_model (RobotModel): The robot model to be loaded. + s (array-like): The joint state vector. + xyz_rpy (array-like): The base pose in terms of position (xyz) and orientation (rpy). + kv_motors (array-like?): Motor velocity constants. Defaults to zeros if not provided. + Im (array-like?): Motor inertia values. Defaults to zeros if not provided. + Returns: + None + """ + self.robot_model = robot_model mujoco_xml = robot_model.get_mujoco_model() @@ -38,41 +52,116 @@ def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): # self.H_right_foot_num = None self.mass = self.robot_model.get_total_mass() - def get_contact_status(self): + def get_contact_status(self) -> tuple: + """ + Determines the contact status of the left and right feet. + + Returns: + tuple: A tuple containing two boolean values: + - left_foot_contact (bool): True if the left foot is in contact, False otherwise. + - right_foot_contact (bool): True if the right foot is in contact, False otherwise. + """ + 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 return left_foot_contact, right_foot_contact - def set_visualize_robot_flag(self, visualize_robot): + def set_visualize_robot_flag(self, visualize_robot) -> None: + """ + Sets the flag to visualize the robot and initializes the viewer if the flag is set to True. + + Args: + visualize_robot (bool): A flag indicating whether to visualize the robot. + """ + self.visualize_robot_flag = visualize_robot if self.visualize_robot_flag: self.viewer = mujoco_viewer.MujocoViewer(self.model, self.data) - def set_base_pose_in_mujoco(self, xyz_rpy): + def set_base_pose_in_mujoco(self, xyz_rpy) -> None: + """ + Set the base pose in the MuJoCo simulator. + + Args: + xyz_rpy (array-like): A 6-element array where the first three elements represent the + position (x, y, z) and the last three elements represent the + orientation in roll, pitch, and yaw (rpy) angles. + Returns: + None + Notes: + - This function converts the roll, pitch, and yaw angles to a quaternion and sets the + base position and orientation in the MuJoCo simulator's data structure. + """ + base_xyz_quat = np.zeros(7) base_xyz_quat[:3] = xyz_rpy[:3] base_xyz_quat[3:] = self.RPY_to_quat(xyz_rpy[3], xyz_rpy[4], xyz_rpy[5]) base_xyz_quat[2] = base_xyz_quat[2] self.data.qpos[:7] = base_xyz_quat - def set_joint_vector_in_mujoco(self, pos): + def set_joint_vector_in_mujoco(self, pos) -> None: + """ + Sets the joint positions in the MuJoCo simulation. + This method converts the given joint positions from the robot's coordinate + system to MuJoCo's coordinate system and updates the joint positions in the + MuJoCo simulation accordingly. + + Args: + pos (list or array-like): A list or array of joint positions in the robot's + coordinate system. + Returns: + None + """ + pos_muj = self.convert_vector_to_mujoco(pos) indexes_joint = self.model.jnt_qposadr[1:] for i in range(self.robot_model.NDoF): self.data.qpos[indexes_joint[i]] = pos_muj[i] - def set_input(self, input): + def set_input(self, input) -> None: + """ + Sets the input for the MuJoCo simulator. + This method converts the provided input vector to a format compatible + with MuJoCo and assigns it to the simulator's control data. + + Args: + input (array-like): The input vector to be set for the simulator. + Returns: + None + """ + input_muj = self.convert_vector_to_mujoco(input) self.data.ctrl = input_muj np.copyto(self.data.ctrl, input_muj) - def set_position_input(self, pos): + def set_position_input(self, pos) -> None: + """ + Sets the desired position input for the simulator. + This method converts the given position vector to the MuJoCo format and + sets it as the desired position. It also enables position control. + + Args: + pos (list or np.ndarray): The position vector to be set as the desired position. + """ + pos_muj = self.convert_vector_to_mujoco(pos) self.desired_pos = pos_muj self.postion_control = True - def create_mapping_vector_to_mujoco(self): + def create_mapping_vector_to_mujoco(self) -> None: + """ + Creates a mapping vector from the robot model's joint names to the Mujoco joint order. + This function initializes the `to_mujoco` attribute as an empty list and populates it with + indices that map each Mujoco joint to its corresponding index in the robot model's joint + name list. If a Mujoco joint is not found in the joint name list, a ValueError is raised. + + Returns: + None + Raises: + ValueError: If a Mujoco joint is not found in the robot model's joint name list. + """ + # This function creates the to_mujoco map self.to_mujoco = [] for mujoco_joint in self.robot_model.mujoco_joint_order: @@ -84,7 +173,20 @@ def create_mapping_vector_to_mujoco(self): f"Mujoco joint '{mujoco_joint}' not found in joint list." ) - def create_mapping_vector_from_mujoco(self): + def create_mapping_vector_from_mujoco(self) -> None: + """ + Creates a mapping vector from the MuJoCo joint order to the robot model's joint order. + This function initializes the `from_mujoco` attribute as an empty list and then populates it + with indices that map each joint in the robot model's joint name list to its corresponding + index in the MuJoCo joint order list. + + Returns: + None + Raises: + ValueError: If a joint name in the robot model's joint name list is not found in the + MuJoCo joint order list. + """ + # This function creates the to_mujoco map self.from_mujoco = [] for joint in self.robot_model.joint_name_list: @@ -96,19 +198,53 @@ def create_mapping_vector_from_mujoco(self): f"Joint name list joint '{joint}' not found in mujoco list." ) - def convert_vector_to_mujoco(self, array_in): + def convert_vector_to_mujoco(self, array_in) -> np.ndarray: + """ + Converts a given vector to the MuJoCo format. + This function takes an input array and reorders its elements according to the + mapping defined in `self.to_mujoco` for the degrees of freedom (DoF) of the robot model. + + Args: + array_in (array-like): The input array to be converted. + Returns: + np.ndarray: The converted array in MuJoCo format. + """ + 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): + """ + Converts an array from MuJoCo format to a classic format. + + Args: + array_muj (np.ndarray): The input array in MuJoCo format. + Returns: + np.ndarray: The converted array in classic format. + """ + 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): + def step(self, n_step=1, visualize=True) -> None: + """ + Advances the simulation by a specified number of steps. + + Args: + n_step (int?): The number of simulation steps to advance. Default is 1. + visualize (bool?): If True, renders the simulation after stepping. Default is True. + Notes: + - If position control is enabled, the control input is computed using + proportional-derivative (PD) control based on the desired position. + - The control input is applied to the simulation, and the simulation + is advanced by the specified number of steps. + - If visualization is enabled, the simulation is rendered after stepping. + """ + if self.postion_control: for _ in range(n_step): s, s_dot, tau = self.get_state(use_mujoco_convention=True) @@ -132,7 +268,17 @@ def step(self, n_step=1, visualize=True): if self.visualize_robot_flag: self.viewer.render() - def step_with_motors(self, n_step, torque): + def step_with_motors(self, n_step, torque) -> None: + """ + Advances the simulation by a specified number of steps while applying motor torques. + + Args: + n_step (int): The number of simulation steps to advance. + torque (array-like): An array of torques to be applied to the motors. The length of the array should match the number of degrees of freedom (NDoF) of the robot. + Returns: + None + """ + indexes_joint_acceleration = self.model.jnt_dofadr[1:] s_dot_dot = self.data.qacc[indexes_joint_acceleration[0] :] for _ in range(n_step): @@ -153,14 +299,37 @@ def step_with_motors(self, n_step, torque): if self.visualize_robot_flag: self.viewer.render() - def compute_misalignment_gravity_fun(self): + def compute_misalignment_gravity_fun(self) -> None: + """ + Computes the misalignment gravity function and assigns it to the instance variable `error_mis`. + This function creates a symbolic 4x4 matrix `H` and a symbolic variable `theta`. It calculates + `theta` as the dot product of the vector [0, 0, 1] and the first three elements of the third + column of `H`, minus 1. It then defines a CasADi function `error` that takes `H` as input and + returns `theta` as output. This function is assigned to the instance variable `error_mis`. + + Returns: + None + """ + H = cs.SX.sym("H", 4, 4) theta = cs.SX.sym("theta") theta = cs.dot([0, 0, 1], H[:3, 2]) - 1 error = cs.Function("error", [H], [theta]) self.error_mis = error - def check_feet_status(self, s, H_b): + def check_feet_status(self, s, H_b) -> tuple: + """ + Checks the status of the robot's feet to determine if they are in contact with the ground and aligned properly. + + Args: + s (np.ndarray): The state vector of the robot. + H_b (np.ndarray): The homogeneous transformation matrix representing the base pose of the robot. + Returns: + tuple: + bool: True if both feet are in contact with the ground and properly aligned, False otherwise. + float: The total misalignment error of both feet. + """ + left_foot_pose = self.robot_model.H_left_foot(H_b, s) rigth_foot_pose = self.robot_model.H_right_foot(H_b, s) left_foot_z = left_foot_pose[2, 3] @@ -180,7 +349,19 @@ def check_feet_status(self, s, H_b): return True, misalignment_error - def get_feet_wrench(self): + def get_feet_wrench(self) -> tuple: + """ + Computes the wrenches (forces and torques) applied to the left and right feet of the robot. + This method calculates the resulting wrenches on the robot's feet based on the current state + and contact forces. It iterates through all contacts, determines if the contact is with the + left or right foot, and accumulates the wrench for each foot. + + Returns: + tuple: A tuple containing two numpy arrays: + - left_foot_wrench (numpy.ndarray): A 6-element array representing the wrench on the left foot. + - right_foot_wrench (numpy.ndarray): A 6-element array representing the wrench on the right foot. + """ + left_foot_wrench = np.zeros(6) rigth_foot_wrench = np.zeros(6) s, s_dot, tau = self.get_state() @@ -213,7 +394,17 @@ def get_feet_wrench(self): 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): + def compute_resulting_wrench(self, b_H_a, force_torque_a) -> np.ndarray: + """ + Compute the resulting wrench (force and torque) in frame b given the wrench in frame a. + + Args: + b_H_a (np.ndarray): A 4x4 homogeneous transformation matrix representing the pose of frame a relative to frame b. + force_torque_a (np.ndarray): A 6-element array representing the force and torque in frame a. + Returns: + np.ndarray: A 6x1 array representing the force and torque in frame b. + """ + p = b_H_a[:3, 3] R = b_H_a[:3, :3] adjoint_matrix = np.zeros([6, 6]) @@ -224,8 +415,19 @@ def compute_resulting_wrench(self, b_H_a, force_torque_a): return force_torque_b # note that for mujoco the ordering is w,x,y,z - def get_base(self): + def get_base(self) -> np.ndarray: + """ + Computes the transformation matrix representing the base position and orientation + of the model in the simulation. + + Returns: + np.ndarray: A 4x4 transformation matrix where the upper-left 3x3 submatrix + represents the rotation matrix derived from the quaternion, and + the upper-right 3x1 submatrix represents the translation vector + (position) of the base. + """ indexes_joint = self.model.jnt_qposadr[1:] + # Extract quaternion components w, x, y, z = self.data.qpos[3 : indexes_joint[0]] @@ -261,13 +463,35 @@ def get_base(self): # Return transformation matrix return trans_mat - def get_base_velocity(self): + def get_base_velocity(self) -> np.ndarray: + """ + Retrieve the base velocity of the model. + This method extracts the base velocity from the simulation data. It uses the joint degrees of freedom + addresses to determine the relevant indices and returns the velocity of the base. + + Returns: + numpy.ndarray: The base velocity of the model. + """ + 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) -> tuple: + """ + Returns the state of the robot either in mujoco_convention or classic one. + If the model has no joints, an empty state is returned either way. + + Args: + use_mujoco_convention (bool): If True, the state is returned in mujoco_convention. If False, it is returned in classic convention. + Returns: + s_out (np.array): joint positions + s_dot_out (np.array): joint velocities + tau_out (np.array): joint torques + """ indexes_joint = self.model.jnt_qposadr[1:] indexes_joint_velocities = self.model.jnt_dofadr[1:] + if len(indexes_joint) == 0: + return np.array([]), np.array([]), np.array([]) s = self.data.qpos[indexes_joint[0] :] s_dot = self.data.qvel[indexes_joint_velocities[0] :] tau = self.data.ctrl @@ -278,20 +502,45 @@ def get_state(self, use_mujoco_convention=False): tau_out = self.convert_from_mujoco(tau) return s_out, s_dot_out, tau_out - def close(self): + def close(self) -> None: + """ + Closes the simulator viewer if the visualization flag is set. + This method checks if the `visualize_robot_flag` is True. If it is, it closes the viewer associated with the simulator. + """ + if self.visualize_robot_flag: self.viewer.close() - def visualize_robot(self): + def visualize_robot(self) -> None: self.viewer.render() - def get_simulation_time(self): + def get_simulation_time(self) -> float: + """ + Retrieve the current simulation time. + + Returns: + float: The current time of the simulation. + """ + return self.data.time - def get_simulation_frequency(self): + def get_simulation_frequency(self) -> float: return self.model.opt.timestep - def RPY_to_quat(self, roll, pitch, yaw): + def RPY_to_quat(self, roll, pitch, yaw) -> list: + """ + Convert roll, pitch, and yaw angles to a quaternion. + The quaternion is returned as a list of four elements [qw, qx, qy, qz]. + + Args: + roll (float): The roll angle in radians. + pitch (float): The pitch angle in radians. + yaw (float): The yaw angle in radians. + Returns: + list: A list containing the quaternion [qw, qx, qy, qz]. + """ + + cr = math.cos(roll / 2) cp = math.cos(pitch / 2) cy = math.cos(yaw / 2) @@ -306,6 +555,11 @@ def RPY_to_quat(self, roll, pitch, yaw): return [qw, qx, qy, qz] - def close_visualization(self): + def close_visualization(self) -> None: + """ + Closes the visualization window if it is open. + This method checks if the `visualize_robot_flag` is True. If it is, it closes the viewer associated with the simulator. + """ + if self.visualize_robot_flag: self.viewer.close() From be0ff886c74b71eaa30ac95f09627b08453ed773 Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Thu, 10 Oct 2024 12:02:17 +0200 Subject: [PATCH 3/6] Defined inclined plane passed in degrees to mujocoSimulator.load_model --- src/comodo/mujocoSimulator/mujocoSimulator.py | 14 +++++++++++--- src/comodo/robotModel/robotModel.py | 7 ++++++- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/comodo/mujocoSimulator/mujocoSimulator.py b/src/comodo/mujocoSimulator/mujocoSimulator.py index 95bcc70..0cf7f81 100644 --- a/src/comodo/mujocoSimulator/mujocoSimulator.py +++ b/src/comodo/mujocoSimulator/mujocoSimulator.py @@ -1,5 +1,6 @@ from comodo.abstractClasses.simulator import Simulator from comodo.robotModel.robotModel import RobotModel +from typing import Sequence import mujoco import math import numpy as np @@ -15,7 +16,7 @@ def __init__(self) -> None: self.compute_misalignment_gravity_fun() super().__init__() - def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=None) -> None: + def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=None, floor_inclination_deg: Sequence[float] = [0, 0, 0]) -> None: """ Loads the robot model into the MuJoCo simulator. @@ -28,10 +29,17 @@ def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=Non Returns: None """ + if len(floor_inclination_deg) != 3: + raise ValueError(f"floor_inclination should be a 3-element sequence, but got a sequence of length {len(floor_inclination)}") + try: + floor_inclination_deg = np.array(floor_inclination_deg, dtype=float) + floor_inclination_deg = np.deg2rad(floor_inclination_deg) + except ValueError: + raise ValueError(f"floor_inclination should be a 3-element sequence of numbers, but got {floor_inclination_deg} of type {type(floor_inclination_deg)}") self.robot_model = robot_model - - mujoco_xml = robot_model.get_mujoco_model() + + mujoco_xml = robot_model.get_mujoco_model(floor_inclination=floor_inclination_deg) self.model = mujoco.MjModel.from_xml_string(mujoco_xml) self.data = mujoco.MjData(self.model) self.create_mapping_vector_from_mujoco() diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index e6ea68d..8ef51c3 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -3,6 +3,7 @@ from urchin import URDF from urchin import Joint from urchin import Link +from typing import Sequence import mujoco import tempfile import xml.etree.ElementTree as ET @@ -329,7 +330,10 @@ def get_mujoco_urdf_string(self) -> str: robot_urdf_string_original = ET.tostring(root, encoding="unicode") return robot_urdf_string_original - def get_mujoco_model(self): + def get_mujoco_model(self, floor_inclination: Sequence[float]) -> mujoco.MjModel: + if len(floor_inclination) != 3: + raise ValueError("Floor inclination must have 3 elements") + urdf_string = self.get_mujoco_urdf_string() with open("temp.urdf", "w+") as f: f.write(urdf_string) @@ -432,6 +436,7 @@ def get_mujoco_model(self): floor.set("type", "plane") floor.set("material", "grid") floor.set("condim", "3") + floor.set("euler", "{} {} {}".format(*floor_inclination)) world_elem.append(floor) new_xml = ET.tostring(tree.getroot(), encoding="unicode") return new_xml From 5ef039bb6fdd2a5f17d44617cbfcd66d003b494e Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Thu, 10 Oct 2024 12:41:23 +0200 Subject: [PATCH 4/6] Changed plane props definition to dict rather than function args --- src/comodo/mujocoSimulator/mujocoSimulator.py | 16 +++------- src/comodo/robotModel/robotModel.py | 31 +++++++++++++++---- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/comodo/mujocoSimulator/mujocoSimulator.py b/src/comodo/mujocoSimulator/mujocoSimulator.py index 0cf7f81..5700b74 100644 --- a/src/comodo/mujocoSimulator/mujocoSimulator.py +++ b/src/comodo/mujocoSimulator/mujocoSimulator.py @@ -1,6 +1,6 @@ from comodo.abstractClasses.simulator import Simulator from comodo.robotModel.robotModel import RobotModel -from typing import Sequence +from typing import Dict import mujoco import math import numpy as np @@ -16,7 +16,7 @@ def __init__(self) -> None: self.compute_misalignment_gravity_fun() super().__init__() - def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=None, floor_inclination_deg: Sequence[float] = [0, 0, 0]) -> None: + def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=None, floor_opts: Dict = {}) -> None: """ Loads the robot model into the MuJoCo simulator. @@ -29,17 +29,9 @@ def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=Non Returns: None """ - if len(floor_inclination_deg) != 3: - raise ValueError(f"floor_inclination should be a 3-element sequence, but got a sequence of length {len(floor_inclination)}") - try: - floor_inclination_deg = np.array(floor_inclination_deg, dtype=float) - floor_inclination_deg = np.deg2rad(floor_inclination_deg) - except ValueError: - raise ValueError(f"floor_inclination should be a 3-element sequence of numbers, but got {floor_inclination_deg} of type {type(floor_inclination_deg)}") - self.robot_model = robot_model - - mujoco_xml = robot_model.get_mujoco_model(floor_inclination=floor_inclination_deg) + self.robot_model = robot_model + mujoco_xml = robot_model.get_mujoco_model(floor_opts=floor_opts) self.model = mujoco.MjModel.from_xml_string(mujoco_xml) self.data = mujoco.MjData(self.model) self.create_mapping_vector_from_mujoco() diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index 8ef51c3..c6a5856 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -3,7 +3,7 @@ from urchin import URDF from urchin import Joint from urchin import Link -from typing import Sequence +from typing import Dict import mujoco import tempfile import xml.etree.ElementTree as ET @@ -330,10 +330,28 @@ def get_mujoco_urdf_string(self) -> str: robot_urdf_string_original = ET.tostring(root, encoding="unicode") return robot_urdf_string_original - def get_mujoco_model(self, floor_inclination: Sequence[float]) -> mujoco.MjModel: - if len(floor_inclination) != 3: - raise ValueError("Floor inclination must have 3 elements") - + def get_mujoco_model(self, floor_opts: Dict) -> mujoco.MjModel: + valid_floor_opts = ["inclination_deg", "friction"] + for key in floor_opts.keys(): + if key not in valid_floor_opts: + raise ValueError(f"Invalid key {key} in floor_opts. Valid keys are {valid_floor_opts}") + + floor_inclination_deg = floor_opts.get("inclination_deg", [0, 0, 0]) + floor_friction = floor_opts.get("friction", 0.6) + + # Type & value checking + try: + floor_inclination = np.array(floor_inclination_deg) + floor_inclination = np.deg2rad(floor_inclination) + except: + raise ValueError(f"floor's inclination_deg must be a sequence of 3 elements, but got {floor_inclination_deg} of type {type(floor_inclination_deg)}") + + if not isinstance(floor_friction, (int, float)): + raise TypeError(f"floor's friction must be a number, but got {floor_friction} of type {type(floor_friction)}") + if not (0 <= floor_friction <= 1): + raise ValueError(f"floor's friction must be a number between 0 and 1, but got {floor_friction}") + + # Get the URDF string urdf_string = self.get_mujoco_urdf_string() with open("temp.urdf", "w+") as f: f.write(urdf_string) @@ -432,11 +450,12 @@ def get_mujoco_model(self, floor_inclination: Sequence[float]) -> mujoco.MjModel break floor = ET.Element("geom") floor.set("name", "floor") - floor.set("size", "0 0 .05") + floor.set("size", "0 0 .04") floor.set("type", "plane") floor.set("material", "grid") floor.set("condim", "3") floor.set("euler", "{} {} {}".format(*floor_inclination)) + floor.set("friction", "{}".format(floor_friction)) world_elem.append(floor) new_xml = ET.tostring(tree.getroot(), encoding="unicode") return new_xml From 11c32de891287c1f6210975c8b6039e0d0b2ffa3 Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Thu, 10 Oct 2024 14:59:15 +0200 Subject: [PATCH 5/6] Minor typing changes - Generalised urdf_path input for robotModel class to pass either string or a pathlib object - [To review] Added save_xml flag on robotModel.get_mujoco_model to save the modified xml file --- src/comodo/mujocoSimulator/mujocoSimulator.py | 2 +- src/comodo/robotModel/robotModel.py | 23 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/comodo/mujocoSimulator/mujocoSimulator.py b/src/comodo/mujocoSimulator/mujocoSimulator.py index 5700b74..bd8d93a 100644 --- a/src/comodo/mujocoSimulator/mujocoSimulator.py +++ b/src/comodo/mujocoSimulator/mujocoSimulator.py @@ -31,7 +31,7 @@ def load_model(self, robot_model: RobotModel, s, xyz_rpy, kv_motors=None, Im=Non """ self.robot_model = robot_model - mujoco_xml = robot_model.get_mujoco_model(floor_opts=floor_opts) + mujoco_xml = robot_model.get_mujoco_model(floor_opts=floor_opts, save_mjc_xml=False) self.model = mujoco.MjModel.from_xml_string(mujoco_xml) self.data = mujoco.MjData(self.model) self.create_mapping_vector_from_mujoco() diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index c6a5856..624faf2 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -1,5 +1,7 @@ from adam.casadi.computations import KinDynComputations import numpy as np +import pathlib +from typing import Union from urchin import URDF from urchin import Joint from urchin import Link @@ -17,7 +19,7 @@ class RobotModel(KinDynComputations): def __init__( self, - urdf_path: str, + urdf_path: Union[str, pathlib.Path, pathlib.PurePath, pathlib.PurePosixPath], robot_name: str, joint_name_list: list, base_link: str = "root", @@ -77,9 +79,13 @@ def __init__( ] ), ) -> None: + valid_path_types = (str, pathlib.Path, pathlib.PurePath, pathlib.PurePosixPath) + if not isinstance(urdf_path, valid_path_types): + raise TypeError(f"urdf_path must be a string or a pathlib object, but got {type(urdf_path)}") + self.collision_keyword = "_collision" self.visual_keyword = "_visual" - self.urdf_path = urdf_path + self.urdf_path = str(urdf_path) self.robot_name = robot_name self.joint_name_list = joint_name_list self.base_link = base_link @@ -260,9 +266,6 @@ def get_mujoco_urdf_string(self) -> str: tempFileOut = tempfile.NamedTemporaryFile(mode="w+") 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() @@ -330,7 +333,7 @@ def get_mujoco_urdf_string(self) -> str: robot_urdf_string_original = ET.tostring(root, encoding="unicode") return robot_urdf_string_original - def get_mujoco_model(self, floor_opts: Dict) -> mujoco.MjModel: + def get_mujoco_model(self, floor_opts: Dict, save_mjc_xml: bool = False) -> mujoco.MjModel: valid_floor_opts = ["inclination_deg", "friction"] for key in floor_opts.keys(): if key not in valid_floor_opts: @@ -353,9 +356,6 @@ def get_mujoco_model(self, floor_opts: Dict) -> mujoco.MjModel: # Get the URDF string 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+") mujoco.mj_saveLastXML(path_temp_xml.name, mujoco_model) @@ -458,6 +458,11 @@ def get_mujoco_model(self, floor_opts: Dict) -> mujoco.MjModel: floor.set("friction", "{}".format(floor_friction)) world_elem.append(floor) new_xml = ET.tostring(tree.getroot(), encoding="unicode") + + if save_mjc_xml: + with open("./mujoco_model.xml", "w") as f: + f.write(new_xml) + return new_xml def get_base_pose_from_contacts(self, s, contact_frames_pose: dict): From 06cd5aca27a089b851a88daa75d8cb5dc3cb285f Mon Sep 17 00:00:00 2001 From: Lorenzo Conti Date: Thu, 10 Oct 2024 15:20:44 +0200 Subject: [PATCH 6/6] Separated floor friction into sliding, torsional and rolling friction with associated default values taken from mujoco --- src/comodo/robotModel/robotModel.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index 624faf2..3194968 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -334,13 +334,15 @@ def get_mujoco_urdf_string(self) -> str: return robot_urdf_string_original def get_mujoco_model(self, floor_opts: Dict, save_mjc_xml: bool = False) -> mujoco.MjModel: - valid_floor_opts = ["inclination_deg", "friction"] + valid_floor_opts = ["inclination_deg", "sliding_friction", "torsional_friction", "rolling_friction"] for key in floor_opts.keys(): if key not in valid_floor_opts: raise ValueError(f"Invalid key {key} in floor_opts. Valid keys are {valid_floor_opts}") floor_inclination_deg = floor_opts.get("inclination_deg", [0, 0, 0]) - floor_friction = floor_opts.get("friction", 0.6) + sliding_friction = floor_opts.get("sliding_friction", 1) + torsional_friction = floor_opts.get("torsional_friction", 0.005) + rolling_friction = floor_opts.get("rolling_friction", 0.0001) # Type & value checking try: @@ -349,10 +351,11 @@ def get_mujoco_model(self, floor_opts: Dict, save_mjc_xml: bool = False) -> mujo except: raise ValueError(f"floor's inclination_deg must be a sequence of 3 elements, but got {floor_inclination_deg} of type {type(floor_inclination_deg)}") - if not isinstance(floor_friction, (int, float)): - raise TypeError(f"floor's friction must be a number, but got {floor_friction} of type {type(floor_friction)}") - if not (0 <= floor_friction <= 1): - raise ValueError(f"floor's friction must be a number between 0 and 1, but got {floor_friction}") + for friction_coeff in ("sliding_friction", "torsional_friction", "rolling_friction"): + if not isinstance(eval(friction_coeff), (int, float)): + raise ValueError(f"{friction_coeff} must be a number (int, float), but got {type(eval(friction_coeff))}") + if not eval(f"0 <= {friction_coeff} <= 1"): + raise ValueError(f"{friction_coeff} must be in the range [0, 1], but got {eval(friction_coeff)}") # Get the URDF string urdf_string = self.get_mujoco_urdf_string() @@ -455,7 +458,7 @@ def get_mujoco_model(self, floor_opts: Dict, save_mjc_xml: bool = False) -> mujo floor.set("material", "grid") floor.set("condim", "3") floor.set("euler", "{} {} {}".format(*floor_inclination)) - floor.set("friction", "{}".format(floor_friction)) + floor.set("friction", "{} {} {}".format(sliding_friction, torsional_friction, rolling_friction)) world_elem.append(floor) new_xml = ET.tostring(tree.getroot(), encoding="unicode")