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').datatorque = data.sensor('torque_sensor').data## 获取传感器的位置和旋转矩阵sensor_pos = data.site('attachment_site').xpossensor_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_forcefeedback_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 jacpdef 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 kinematicsJ = self.calculate_jacobian()J_pinv = np.linalg.pinv(J)
def get_pose(self):p = self.data.site_xpos[self.ee_site_idx].copy() # posR = self.data.site_xmat[self.ee_site_idx].copy() # rotation matrixreturn 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 sensoradr = 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 datasensor_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 stateft , 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 sensoradr = 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 datasensor_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 = desiredR12 = Rs.T @ ReMat = 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.dtypeN_full = self.model.nvjac = 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 dofsreturn 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 @ Jsreturn Jbdef get_body_ee_velocity(self):Jb = self.get_body_jacobian()dq = self.get_joint_velocity()[:self.N]Vb = Jb@dq.reshape((-1,1))return Vbdef get_spatial_ee_velocity(self):Js = self.get_jacobian()dq = self.get_joint_velocity()Vs = Js@dq.reshape((-1,1))return Vsdef 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.timestepdef get_sim_time(self):return self.data.time
def get_FT_value_raw(self):Fe, dFe = self.robot_state.get_ee_force_raw()return -Fedef 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))
