UR5e机器人xml文件模型
<mujoco model="ur5e">
<compiler angle="radian" meshdir="assets" autolimits="true"/>
<option integrator="implicitfast"/>
<default>
<default class="ur5e">
<material specular="0.5" shininess="0.25"/>
<joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1"/>
<general gaintype="fixed" biastype="affine" ctrlrange="-6.2831 6.2831" gainprm="2000" biasprm="0 -2000 -400"
forcerange="-150 150"/>
<default class="size3">
<default class="size3_limited">
<joint range="-3.1415 3.1415"/>
<general ctrlrange="-3.1415 3.1415"/>
</default>
</default>
<default class="size1">
<general gainprm="500" biasprm="0 -500 -100" forcerange="-28 28"/>
</default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="collision">
<geom type="capsule" group="3"/>
<default class="eef_collision">
<geom type="cylinder"/>
</default>
</default>
<site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
</default>
</default>
<asset>
<material class="ur5e" name="black" rgba="0.033 0.033 0.033 1"/>
<material class="ur5e" name="jointgray" rgba="0.278 0.278 0.278 1"/>
<material class="ur5e" name="linkgray" rgba="0.82 0.82 0.82 1"/>
<material class="ur5e" name="urblue" rgba="0.49 0.678 0.8 1"/>
<mesh file="base_0.obj"/>
<mesh file="base_1.obj"/>
<mesh file="shoulder_0.obj"/>
<mesh file="shoulder_1.obj"/>
<mesh file="shoulder_2.obj"/>
<mesh file="upperarm_0.obj"/>
<mesh file="upperarm_1.obj"/>
<mesh file="upperarm_2.obj"/>
<mesh file="upperarm_3.obj"/>
<mesh file="forearm_0.obj"/>
<mesh file="forearm_1.obj"/>
<mesh file="forearm_2.obj"/>
<mesh file="forearm_3.obj"/>
<mesh file="wrist1_0.obj"/>
<mesh file="wrist1_1.obj"/>
<mesh file="wrist1_2.obj"/>
<mesh file="wrist2_0.obj"/>
<mesh file="wrist2_1.obj"/>
<mesh file="wrist2_2.obj"/>
<mesh file="wrist3.obj"/>
</asset>
<worldbody>
<light name="spotlight" mode="targetbodycom" target="wrist_2_link" pos="0 -1 2"/>
<body name="base" quat="1 0 0 0" childclass="ur5e">
<inertial mass="4.0" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
<geom mesh="base_0" material="black" class="visual"/>
<geom mesh="base_1" material="jointgray" class="visual"/>
<body name="shoulder_link" pos="0 0 0.163">
<inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666"/>
<joint name="shoulder_pan_joint" class="size3" axis="0 0 1"/>
<geom mesh="shoulder_0" material="urblue" class="visual"/>
<geom mesh="shoulder_1" material="black" class="visual"/>
<geom mesh="shoulder_2" material="jointgray" class="visual"/>
<geom class="collision" size="0.06 0.06" pos="0 0 -0.04"/>
<body name="upper_arm_link" pos="0 0.138 0" quat="1 0 1 0">
<inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074"/>
<joint name="shoulder_lift_joint" class="size3"/>
<geom mesh="upperarm_0" material="linkgray" class="visual"/>
<geom mesh="upperarm_1" material="black" class="visual"/>
<geom mesh="upperarm_2" material="jointgray" class="visual"/>
<geom mesh="upperarm_3" material="urblue" class="visual"/>
<geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06"/>
<geom class="collision" size="0.05 0.2" pos="0 0 0.2"/>
<body name="forearm_link" pos="0 -0.131 0.425">
<inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095"/>
<joint name="elbow_joint" class="size3_limited"/>
<geom mesh="forearm_0" material="urblue" class="visual"/>
<geom mesh="forearm_1" material="linkgray" class="visual"/>
<geom mesh="forearm_2" material="black" class="visual"/>
<geom mesh="forearm_3" material="jointgray" class="visual"/>
<geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06"/>
<geom class="collision" size="0.038 0.19" pos="0 0 0.2"/>
<body name="wrist_1_link" pos="0 0 0.392" quat="1 0 1 0">
<inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942"/>
<joint name="wrist_1_joint" class="size1"/>
<geom mesh="wrist1_0" material="black" class="visual"/>
<geom mesh="wrist1_1" material="urblue" class="visual"/>
<geom mesh="wrist1_2" material="jointgray" class="visual"/>
<geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07"/>
<body name="wrist_2_link" pos="0 0.127 0">
<inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942"/>
<joint name="wrist_2_joint" axis="0 0 1" class="size1"/>
<geom mesh="wrist2_0" material="black" class="visual"/>
<geom mesh="wrist2_1" material="urblue" class="visual"/>
<geom mesh="wrist2_2" material="jointgray" class="visual"/>
<geom class="collision" size="0.04 0.06" pos="0 0 0.04"/>
<geom class="collision" pos="0 0.02 0.1" quat="1 1 0 0" size="0.04 0.04"/>
<body name="wrist_3_link" pos="0 0 0.1">
<inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1"
diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
<joint name="wrist_3_joint" class="size1"/>
<geom material="linkgray" mesh="wrist3" class="visual"/>
<geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
<site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<sensor>
<force site="attachment_site" name="force_sensor"/>
<torque site="attachment_site" name="torque_sensor"/>
</sensor>
<actuator>
<general class="size3" name="shoulder_pan" joint="shoulder_pan_joint"/>
<general class="size3" name="shoulder_lift" joint="shoulder_lift_joint"/>
<general class="size3_limited" name="elbow" joint="elbow_joint"/>
<general class="size1" name="wrist_1" joint="wrist_1_joint"/>
<general class="size1" name="wrist_2" joint="wrist_2_joint"/>
<general class="size1" name="wrist_3" joint="wrist_3_joint"/>
</actuator>
<keyframe>
<key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0" ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0"/>
</keyframe>
</mujoco>
接触力
<sensor>
<force site="attachment_site" name="force_sensor"/>
<torque site="attachment_site" name="torque_sensor"/>
</sensor>
## 获取末端受力和力矩
force = data.sensor('force_sensor').data
torque = data.sensor('torque_sensor').data
## 获取传感器的位置和旋转矩阵
sensor_pos = data.site('attachment_site').xpos
sensor_mat = data.site('attachment_site').xmat.reshape(3, 3)
## 将力转换到世界坐标系
force_world = sensor_mat.dot(force)
## 将力矩转换到世界坐标系
torque_world = sensor_mat.dot(torque)
## 合并力和力矩
end_force = np.concatenate((force_world, torque_world), axis=0)
## 计算feedback_force
feedback_force = np.sqrt(np.sum(np.square(force_world)))
<body name="wrist_3_link" pos="0 0 0.1">
<inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1"
diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
<joint name="wrist_3_joint" class="size1"/>
<geom material="linkgray" mesh="wrist3" class="visual"/>
<geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
<site name="ee_site" pos="0 0.1 0" quat="-1 1 0 0"/>
</body>
<actuator>
<general class="size3" name="shoulder_pan" joint="shoulder_pan_joint"/>
<general class="size3" name="shoulder_lift" joint="shoulder_lift_joint"/>
<general class="size3_limited" name="elbow" joint="elbow_joint"/>
<general class="size1" name="wrist_1" joint="wrist_1_joint"/>
<general class="size1" name="wrist_2" joint="wrist_2_joint"/>
<general class="size1" name="wrist_3" joint="wrist_3_joint"/>
</actuator>
def get_ee_pos(self):
ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
return self.data.site_xpos[ee_site_id].copy()
def get_EE_POS_FROM_QPOS(self, qpos):
self.data.qpos[:self.nq] = qpos[:self.nq]
mujoco.mj_forward(self.model, self.data)
ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
return self.data.site_xpos[ee_site_id].copy()
def get_EE_JACOBIAN(self):
ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
jacp = np.zeros((3, self.nv))
jacr = np.zeros((3, self.nv))
mujoco.mj_jacSite(self.model, self.data, jacp, jacr, ee_site_id)
return jacp
def get_state(self):
return np.concatenate([self.data.qpos[:self.nq], self.data.qvel[:self.nv]])
def set_state(self, x):
self.data.qpos[:self.nq] = x[:self.nq]
self.data.qvel[:self.nv] = x[self.nq:]
mujoco.mj_forward(self.model, self.data)
def step(self, u):
self.data.ctrl[:self.nu] = np.clip(u, -self.model.actuator_ctrlrange[:, 0], self.model.actuator_ctrlrange[:, 1])
mujoco.mj_step(self.model, self.data)
return self.get_state()
def get_ee_position(self):
return self.data.xpos[self.wrist_3_body_id]
def calculate_jacobian(self):
jacp = np.zeros((3, self.model.nv))
jacr = np.zeros((3, self.model.nv))
mujoco.mj_jacBody(self.model, self.data, jacp, jacr, self.wrist_3_body_id)
return jacp
# Get target joint positions using inverse kinematics
J = self.calculate_jacobian()
J_pinv = np.linalg.pinv(J)
def get_pose(self):
p = self.data.site_xpos[self.ee_site_idx].copy() # pos
R = self.data.site_xmat[self.ee_site_idx].copy() # rotation matrix
return p, R.reshape((3,3))
<site name="end_effector" type="sphere" size="0.005" pos="0.0 0 0.205" euler="0 0 0" rgba="1 0 0 1.00"/>
<actuator>
<!-- Physical limits of the actuator. -->
<motor name="indy_joint0_actuator" joint="indy_joint1" gear="1 0 0 0 0 0" ctrllimited="false" ctrlrange="-1e6 1e6" forcerange="-1e6 1e6"/>
<motor name="indy_joint1_actuator" joint="indy_joint2" gear="1 0 0 0 0 0" ctrllimited="false" ctrlrange="-1e6 1e6" forcerange="-1e6 1e6"/>
<motor name="indy_joint2_actuator" joint="indy_joint3" gear="1 0 0 0 0 0" ctrllimited="false" ctrlrange="-1e6 1e6" forcerange="-1e6 1e6"/>
<motor name="indy_joint3_actuator" joint="indy_joint4" gear="1 0 0 0 0 0" ctrllimited="false" ctrlrange="-1e6 1e6" forcerange="-1e6 1e6"/>
<motor name="indy_joint4_actuator" joint="indy_joint5" gear="1 0 0 0 0 0" ctrllimited="false" ctrlrange="-1e6 1e6" forcerange="-1e6 1e6"/>
<motor name="indy_joint5_actuator" joint="indy_joint6" gear="1 0 0 0 0 0" ctrllimited="false" ctrlrange="-1e6 1e6" forcerange="-1e6 1e6"/>
<position ctrllimited="true" ctrlrange="-0.03 0" joint="jointf1" kp="2000" name="actf1"/>
<position ctrllimited="true" ctrlrange="0.0 0.03" joint="jointf2" kp="2000" name="actf2"/>
</actuator>
<sensor>
<framepos objtype="site" objname="end_effector"/>
<force name="force_sensor" site="end_effector"/>
<torque name="torque_sensor" site="end_effector"/>
</sensor>
def 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_ee_force(self,):
sensor_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR, "force_sensor")
# Get address and dimension of the sensor
adr = self.model.sensor_adr[sensor_id]
dim = self.model.sensor_dim[sensor_id]
force = np.copy(self.data.sensordata[adr:adr + dim])
# get torque sensor data
sensor_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR, "torque_sensor")
adr = self.model.sensor_adr[sensor_id]
dim = self.model.sensor_dim[sensor_id]
torque = np.copy(self.data.sensordata[adr:adr + dim])
force_torque = np.concatenate([force, torque])
# update robot state
ft , dft = self.lp_filter_implemented(force_torque)
return ft, dft
def get_ee_force_raw(self,):
sensor_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR, "force_sensor")
# Get address and dimension of the sensor
adr = self.model.sensor_adr[sensor_id]
dim = self.model.sensor_dim[sensor_id]
force = np.copy(self.data.sensordata[adr:adr + dim])
# get torque sensor data
sensor_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR, "torque_sensor")
adr = self.model.sensor_adr[sensor_id]
dim = self.model.sensor_dim[sensor_id]
torque = np.copy(self.data.sensordata[adr:adr + dim])
force_torque = np.concatenate([force, torque])
# force_sensor_data = self.lp_filter_raw(force_torque.reshape((-1, 6)))[0, :]
ft_raw , dft_raw = self.lp_filter_implemented_raw(force_torque)
return ft_raw , dft_raw
def transform_rot(self, fe, desired):
pe, Re = self.get_pose()
ps, Rs = desired
R12 = Rs.T @ Re
Mat = np.block([[R12, np.zeros((3, 3))], [np.zeros((3, 3)), R12]])
return Mat.dot(fe)
状态获取
def get_jacobian(self):
"""Get 6x7 geometric jacobian matrix."""
dtype = self.data.qpos.dtype
N_full = self.model.nv
jac = np.zeros((6, N_full), dtype=dtype)
jac_pos = np.zeros((3 , N_full), dtype=dtype)
jac_rot = np.zeros((3 , N_full), dtype=dtype)
mujoco.mj_jacSite(
self.model, self.data,
jac_pos, jac_rot, self.ee_site_idx)
jac[3:] = jac_rot.reshape((3, N_full))
jac[:3] = jac_pos.reshape((3, N_full))
# only return first 7 dofs
return jac[:, :self.N].copy()
def get_body_jacobian(self):
Js = self.get_jacobian()
p, R = self.get_pose()
transform = np.block([[R.T, np.zeros((3,3))], [np.zeros((3,3)), R.T]])
Jb = transform @ Js
return Jb
def get_body_ee_velocity(self):
Jb = self.get_body_jacobian()
dq = self.get_joint_velocity()[:self.N]
Vb = Jb@dq.reshape((-1,1))
return Vb
def get_spatial_ee_velocity(self):
Js = self.get_jacobian()
dq = self.get_joint_velocity()
Vs = Js@dq.reshape((-1,1))
return Vs
def get_joint_pose(self):
return self.data.qpos.copy()
def get_joint_velocity(self):
return self.data.qvel.copy()
def get_bias_torque(self):
"""Get the gravity and Coriolis, centrifugal torque """
return self.data.qfrc_bias[:self.N].copy()
def get_full_inertia(self):
M = np.zeros((self.model.nv, self.model.nv))
mujoco.mj_fullM(self.model, M, self.data.qM)
return M[:self.N, :self.N]
def get_timestep(self):
"""Timestep of the simulator is timestep of controller."""
return self.model.opt.timestep
def get_sim_time(self):
return self.data.time
def get_FT_value_raw(self):
Fe, dFe = self.robot_state.get_ee_force_raw()
return -Fe
def get_eg(self, g, gd):
p = g[:3,3]
R = g[:3,:3]
pd = gd[:3,3]
Rd = gd[:3,:3]
ep = R.T @ (p - pd)
eR = vee_map(Rd.T @ R - R.T @ Rd).reshape((-1,))
return np.hstack((ep, eR)).reshape((-1,1))
Jb = self.robot_state.get_body_jacobian()
# M,C,G = self.robot_state.get_dynamic_matrices()
qfrc_bias = self.robot_state.get_bias_torque()
M = self.robot_state.get_full_inertia()
def get_bias_torque(self):
"""Get the gravity and Coriolis, centrifugal torque """
return self.data.qfrc_bias[:self.N].copy()
qfrc_bias = self.robot_state.get_bias_torque()
tau_cmd = Jb.T @ tau_tilde + qfrc_bias.reshape((-1,1))
