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

Raise exception if root link is missing in URDF #28

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Raise exception if root link is missing in URDF #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9b9490c
[WIP] Removed unused links and modified ET root fetching
lorycontixd Oct 9, 2024
5a8fcad
Raising exception if root link is missing in urdf to avoid misleading…
lorycontixd Dec 11, 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
60 changes: 34 additions & 26 deletions src/comodo/mujocoSimulator/mujocoSimulator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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):
Expand Down Expand Up @@ -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 = []
Expand All @@ -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)
Expand Down Expand Up @@ -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))
Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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)
Expand Down
123 changes: 89 additions & 34 deletions src/comodo/robotModel/robotModel.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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
Expand Down Expand Up @@ -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())
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -210,11 +254,20 @@ 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()
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"):
Expand Down Expand Up @@ -276,11 +329,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+")
Expand Down