Mujoco robosuite 機器人模型

import ctypes
import os# 獲取當前腳本所在的目錄
script_dir = os.path.dirname(os.path.abspath(__file__))# 構建庫文件的相對路徑
lib_relative_path = os.path.join('dynamic_models', 'UR5e', 'Jb.so')# 拼接成完整的路徑
lib_path = os.path.join(script_dir, lib_relative_path)try:fun_grav = ctypes.CDLL(lib_path)print("庫文件加載成功！")
except OSError as e:print(f"加載庫文件時出錯: {e}")

<mujoco model="ur5"><compiler angle="radian" meshdir="ur5/collision/" /><size njmax="500" nconmax="100" /><asset><mesh name="base" file="base.stl" /><mesh name="shoulder" file="shoulder.stl" /><mesh name="upperarm" file="upperarm.stl" /><mesh name="forearm" file="forearm.stl" /><mesh name="wrist1" file="wrist1.stl" /><mesh name="wrist2" file="wrist2.stl" /><mesh name="wrist3" file="wrist3.stl" /></asset><worldbody><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="base" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="base" /><body name="shoulder_link" pos="0 0 0.089159"><inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" /><joint name="shoulder_pan_joint" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" /><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="shoulder" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="shoulder" /><body name="upper_arm_link" pos="0 0.13585 0" quat="0.707107 0 0.707107 0"><inertial pos="0 0 0.28" mass="8.393" diaginertia="0.226891 0.226891 0.0151074" /><joint name="shoulder_lift_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="upperarm" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="upperarm" /><body name="forearm_link" pos="0 -0.1197 0.425"><inertial pos="0 0 0.196125" mass="2.275" diaginertia="0.0312168 0.0312168 0.004095" /><joint name="elbow_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="forearm" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="forearm" /><body name="wrist_1_link" pos="0 0 0.39225" quat="0.707107 0 0.707107 0"><inertial pos="0 0.093 0" mass="1.219" diaginertia="0.0025599 0.0025599 0.0021942" /><joint name="wrist_1_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="wrist1" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist1" /><body name="wrist_2_link" pos="0 0.093 0"><inertial pos="0 0 0.09465" mass="1.219" diaginertia="0.0025599 0.0025599 0.0021942" /><joint name="wrist_2_joint" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" /><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="wrist2" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist2" /><body name="wrist_3_link" pos="0 0 0.09465"><inertial pos="0 0.06505 0" quat="1.73123e-12 0.707107 -0.707107 1.73123e-12" mass="0.1879" diaginertia="0.000132117 8.46959e-05 8.46959e-05" /><joint name="wrist_3_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /><geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="wrist3" /><geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist3" /><site name="ee" pos="0 0 0" rgba="1 0 0 1" size="0.01" type="sphere"/></body></body></body></body></body></body></worldbody><actuator><motor name='motor1' ctrllimited="true" ctrlrange="-100 100" joint='shoulder_pan_joint' gear="1"/><motor name='motor2' ctrllimited="true" ctrlrange="-100 100" joint='shoulder_lift_joint' gear="1"/><motor name='motor3' ctrllimited="true" ctrlrange="-100 100" joint='elbow_joint' gear="1"/><motor name='motor4' ctrllimited="true" ctrlrange="-100 100" joint='wrist_1_joint' gear="1"/><motor name='motor5' ctrllimited="true" ctrlrange="-100 100" joint='wrist_2_joint' gear="1"/><motor name='motor6' ctrllimited="true" ctrlrange="-100 100" joint='wrist_3_joint' gear="1"/></actuator>
</mujoco>

雅克比

from dm_control.mujoco.wrapper import mjbindings
import numpy as npmjlib = mjbindings.mjlibdef get_site_jac(model, data, site_id):"""Return the Jacobian' translational component of the end-effector ofthe corresponding site id."""jacp = np.zeros((3, model.nv))jacr = np.zeros((3, model.nv))mjlib.mj_jacSite(model, data, jacp, jacr, site_id)jac = np.vstack([jacp, jacr])return jacdef get_fullM(model, data):M = np.zeros((model.nv, model.nv))mjlib.mj_fullM(model, M, data.qM)return M

任務空間慣量矩陣

def task_space_inertia_matrix(M, J, threshold=1e-3):"""Generate the task-space inertia matrixParameters----------M: np.arraythe generalized coordinates inertia matrixJ: np.arraythe task space Jacobianthreshold: scalar, optional (Default: 1e-3)singular value threshold, if the detminant of Mx_inv is less thanthis value then Mx is calculated using the pseudo-inverse functionand all singular values < threshold * .1 are set = 0"""# calculate the inertia matrix in task spaceM_inv = np.linalg.inv(M)Mx_inv = np.dot(J, np.dot(M_inv, J.T))if abs(np.linalg.det(Mx_inv)) >= threshold:# do the linalg inverse if matrix is non-singular# because it's faster and more accurateMx = np.linalg.inv(Mx_inv)else:# using the rcond to set singular values < thresh to 0# singular values < (rcond * max(singular_values)) set to 0Mx = np.linalg.pinv(Mx_inv, rcond=threshold * 0.1)return Mx, M_inv

def set_state(model, data, qpos, qvel):assert qpos.shape == (model.nq, ) and qvel.shape == (model.nv, )# old_state = data.get_state()# new_state = mujoco.MjSimState(old_state.time, qpos, qvel, old_state.act,#                                  old_state.udd_state)# data.set_state(new_state)# model.forward()data.qpos[:] = qposdata.qvel[:] = qveldef get_contact_force(mj_model, mj_data, body_name):bodyId = mujoco.mj_name2id(mj_model, MJ_BODY_OBJ, body_name)force_com = mj_data.cfrc_ext[bodyId, :]trn_force = force_com.copy()return np.hstack((trn_force[3:], trn_force[:3]))def get_geom_pose(model, geom_name):"""Return the geom pose (relative to parent body).:param mujoco_py.MjModel model::param str geom_name::return: position, quaternion:rtype: tuple(np.array(3), np.array(4))"""geom_id = mujoco.mj_name2id(model, MJ_GEOM_OBJ, geom_name)pos = model.geom_pos[geom_id, :]quat = model.geom_quat[geom_id, :]return pos, quatdef get_geom_size(model, geom_name):"""Return the geom size.:param mujoco_py.MjModel model::param str geom_name::return: (radius, half-length, _) for cylinder geom, and(X half-size; Y half-size; Z half-size) for box geom:rtype: np.array(3)"""geom_id = mujoco.mj_name2id(model, MJ_GEOM_OBJ, geom_name)if model.geom_type[geom_id] == MJ_BOX or model.geom_type[geom_id] == MJ_CYLINDER:return model.geom_size[geom_id, :]else:return Nonedef get_geom_friction(model, geom_name):geom_id = mujoco.mj_name2id(model, MJ_GEOM_OBJ, geom_name)return model.geom_friction[geom_id, :]def get_body_mass(model, body_name):body_id = mujoco.mj_name2id(model, MJ_BODY_OBJ, body_name)return model.body_mass[body_id]def get_body_pose(model, body_name):body_id = mujoco.mj_name2id(model, MJ_BODY_OBJ, body_name)return model.body_pos[body_id], model.body_quat[body_id]def get_mesh_vertex_pos(model, geom_name):geom_id = mujoco.mj_name2id(model, MJ_GEOM_OBJ, geom_name)assert model.geom_type[geom_id] == MJ_MESHmesh_id = model.geom_dataid[geom_id]first_vertex_id = model.mesh_vertadr[mesh_id]no_vertex = model.mesh_vertnum[mesh_id]vertex_pos = model.mesh_vert[first_vertex_id:first_vertex_id + no_vertex]return vertex_posdef set_geom_size(model, geom_name, size):geom_id = mujoco.mj_name2id(model, MJ_GEOM_OBJ, geom_name)model.geom_size[geom_id, :] = sizedef set_body_mass(model, body_name, mass):body_id = mujoco.mj_name2id(model, MJ_BODY_OBJ, body_name)model.body_mass[body_id] = massdef set_geom_friction(model, geom_name, friction):geom_id = mujoco.mj_name2id(model, MJ_GEOM_OBJ, geom_name)model.geom_friction[geom_id, :] = frictiondef set_body_pose(model, body_name, pos, quat):body_id = mujoco.mj_name2id(model, MJ_BODY_OBJ, body_name)model.body_pos[body_id, :] = posmodel.body_quat[body_id, :] = quatdef set_body_pose_rotm(model, body_name, pos, R):quat = mat2quat(R)body_id = mujoco.mj_name2id(model, MJ_BODY_OBJ, body_name)model.body_pos[body_id, :] = posmodel.body_quat[body_id, :] = quat# -------- GEOMETRY TOOLs
def quat_error(q1, q2):"""Compute the rotation vector (expressed in the base frame), that if followin a unit time, will transform a body with orientation `q1` toorientation `q2`:param list/np.ndarray q1: Description of parameter `q1`.:param list/np.ndarray q2: Description of parameter `q2`.:return: a 3D rotation vector:rtype: np.ndarray"""if isinstance(q1, list):q1 = np.array(q1)if isinstance(q2, list):q2 = np.array(q2)dtype = q1.dtypeneg_q1 = np.zeros(4, dtype=dtype)err_rot_quat = np.zeros(4, dtype=dtype)err_rot = np.zeros(3, dtype=dtype)if q1.dot(q2) < 0:q1 = -q1mujoco.mju_negQuat(neg_q1, q1)mujoco.mju_mulQuat(err_rot_quat, q2, neg_q1)mujoco.mju_quat2Vel(err_rot, err_rot_quat, 1)return err_rotdef quat2mat(q):"""Tranform a quaternion to rotation amtrix.:param type q: Description of parameter `q`.:return: 3x3 rotation matrix:rtype: np.array"""mat = np.zeros(9)mujoco.mju_quat2Mat(mat, q)return mat.reshape((3, 3))def pose_transform(p1, q1, p21, q21):"""Coordinate transformation between 2 frames:param np.ndarray p1: position in frame 1:param np.ndarray q1: orientation (quaternion) in frame 1:param np.ndarray p21: relative position between frame 1 and 2:param np.ndarray q21: relative orientation between frame 1 and 2:return: position and orientation in frame 2:rtype: type"""# quat to rotation matrixR21 = quat2mat(q21)p2 = p21 + R21.dot(p1)q2 = np.zeros_like(q1)mujoco.mju_mulQuat(q2, q21, q1)  # q2 = q21*q1return p2, q2def integrate_quat(q, r, dt):"""Integrate quaternion by a fixed angular velocity over the duration dt.:param np.array(4) q: quaternion.:param np.array(3) r: angular velocity.:param float dt: duration.:return: result quaternion.:rtype: np.array(4)"""qres = np.zeros(4)qe = np.zeros(4)r = r * dtangle = np.linalg.norm(r)if angle < 1e-9:# if angle too small then return current qreturn q.copy()axis = r / anglemujoco.mju_axisAngle2Quat(qe, axis, angle)mujoco.mju_mulQuat(qres, qe, q)return qresdef transform_spatial(v1, q21):"""Coordinate transformation of a spatial vector. The spatial vector can be eithertwist (linear + angular velocity) or wrench (force + torque):param type v1: Spatial vector in frame 1:param type q21: transformation matrix (in terms of quaternion):return: Description of returned object.:rtype: type"""R21 = quat2mat(q21)R = np.block([[R21, np.zeros((3, 3))], [np.zeros((3, 3)), R21]])return R.dot(v1)def similarity_transform(A1, q21):"""Similarity transformation of a matrix from frame 1 to frame 2A2 = R21 * A1 * R12:param np.array((3, 3)) A1: 3x3 matrix.:param np.array(4) q21: quaternion representation.:return: 3x3 matrix:rtype: np.array"""R21 = quat2mat(q21)return R21.dot(A1.dot(R21.T))# NOTE: there are infinite rotation vector solutions for a particular
# orientation, the `ref` is to find the closest solution to a reference.
# Is there another minimal representation that could avoid this?
def quat2vec(q, ref=None):"""Transform quaternion representation to rotation vector representation"""r = np.zeros(3)scale = 1mujoco.mju_quat2Vel(r, q, scale)if ref is not None:if r.dot(ref) < 0:angle = np.linalg.norm(r)r = r / angleangle = angle - 2 * np.pir = r * anglereturn rdef inverse_frame(p, q):pi, qi = np.zeros(3), np.zeros(4)mujoco.mju_negPose(pi, qi, p, q)return pi, qidef mat2quat(R):R = R.flatten()q = np.zeros(4)mujoco.mju_mat2Quat(q, R)return qdef mul_quat(q1, q2):q = np.zeros(4)mujoco.mju_mulQuat(q, q1, q2)return q

https://github.com/google-deepmind/mujoco_menageriehttps://github.com/google-deepmind/mujoco_menagerie