├── .gitignore ├── README.md ├── contact_detect.py ├── control_ee_with_pinocchio.py ├── control_joint_pos.py ├── get_body_pos.py ├── get_camera_pic.py ├── get_torque.py ├── ik_path_paln_trajectory_pyroboplan.py ├── ik_pyroboplan_so_arm100.py ├── impedance_control.py ├── install_pykdl.sh ├── joint_impedance_control.py ├── mjcf2usd.py ├── mocap_panda.py ├── model ├── ball.xml ├── franka_emika_panda │ ├── LICENSE │ ├── README.md │ ├── assets │ │ ├── finger_0.obj │ │ ├── finger_1.obj │ │ ├── hand.stl │ │ ├── hand_0.obj │ │ ├── hand_1.obj │ │ ├── hand_2.obj │ │ ├── hand_3.obj │ │ ├── hand_4.obj │ │ ├── link0.stl │ │ ├── link0_0.obj │ │ ├── link0_1.obj │ │ ├── link0_10.obj │ │ ├── link0_11.obj │ │ ├── link0_2.obj │ │ ├── link0_3.obj │ │ ├── link0_4.obj │ │ ├── link0_5.obj │ │ ├── link0_7.obj │ │ ├── link0_8.obj │ │ ├── link0_9.obj │ │ ├── link1.obj │ │ ├── link1.stl │ │ ├── link2.obj │ │ ├── link2.stl │ │ ├── link3.stl │ │ ├── link3_0.obj │ │ ├── link3_1.obj │ │ ├── link3_2.obj │ │ ├── link3_3.obj │ │ ├── link4.stl │ │ ├── link4_0.obj │ │ ├── link4_1.obj │ │ ├── link4_2.obj │ │ ├── link4_3.obj │ │ ├── link5_0.obj │ │ ├── link5_1.obj │ │ ├── link5_2.obj │ │ ├── link5_collision_0.obj │ │ ├── link5_collision_1.obj │ │ ├── link5_collision_2.obj │ │ ├── link6.stl │ │ ├── link6_0.obj │ │ ├── link6_1.obj │ │ ├── link6_10.obj │ │ ├── link6_11.obj │ │ ├── link6_12.obj │ │ ├── link6_13.obj │ │ ├── link6_14.obj │ │ ├── link6_15.obj │ │ ├── link6_16.obj │ │ ├── link6_2.obj │ │ ├── link6_3.obj │ │ ├── link6_4.obj │ │ ├── link6_5.obj │ │ ├── link6_6.obj │ │ ├── link6_7.obj │ │ ├── link6_8.obj │ │ ├── link6_9.obj │ │ ├── link7.stl │ │ ├── link7_0.obj │ │ ├── link7_1.obj │ │ ├── link7_2.obj │ │ ├── link7_3.obj │ │ ├── link7_4.obj │ │ ├── link7_5.obj │ │ ├── link7_6.obj │ │ └── link7_7.obj │ ├── hand.xml │ ├── mjx_panda.xml │ ├── mjx_panda_nohand.xml │ ├── mjx_scene.xml │ ├── mjx_single_cube.xml │ ├── panda.xml │ ├── panda_nohand.xml │ ├── scene.xml │ ├── scene_withcamera.xml │ └── scene_withcube.xml ├── so_arm100_description │ ├── meshes │ │ ├── Base.stl │ │ ├── Base.stl.convex.stl │ │ ├── Base_Motor.stl │ │ ├── Base_Motor.stl.convex.stl │ │ ├── Fixed_Jaw.stl │ │ ├── Fixed_Jaw.stl.convex.stl │ │ ├── Fixed_Jaw_Motor.stl │ │ ├── Fixed_Jaw_Motor.stl.convex.stl │ │ ├── Fixed_Jaw_part1.ply │ │ ├── Fixed_Jaw_part1.ply.convex.stl │ │ ├── Fixed_Jaw_part2.ply │ │ ├── Fixed_Jaw_part2.ply.convex.stl │ │ ├── Lower_Arm.stl │ │ ├── Lower_Arm.stl.convex.stl │ │ ├── Lower_Arm_Motor.stl │ │ ├── Lower_Arm_Motor.stl.convex.stl │ │ ├── Moving_Jaw.stl │ │ ├── Moving_Jaw.stl.convex.stl │ │ ├── Moving_Jaw_part1.ply │ │ ├── Moving_Jaw_part1.ply.convex.stl │ │ ├── Moving_Jaw_part2.ply │ │ ├── Moving_Jaw_part2.ply.convex.stl │ │ ├── Moving_Jaw_part3.ply │ │ ├── Moving_Jaw_part3.ply.convex.stl │ │ ├── Rotation_Pitch.stl │ │ ├── Rotation_Pitch.stl.convex.stl │ │ ├── Rotation_Pitch_Motor.stl │ │ ├── Rotation_Pitch_Motor.stl.convex.stl │ │ ├── Upper_Arm.stl │ │ ├── Upper_Arm.stl.convex.stl │ │ ├── Upper_Arm_Motor.stl │ │ ├── Upper_Arm_Motor.stl.convex.stl │ │ ├── Wrist_Pitch_Roll.stl │ │ ├── Wrist_Pitch_Roll.stl.convex.stl │ │ ├── Wrist_Pitch_Roll_Motor.stl │ │ └── Wrist_Pitch_Roll_Motor.stl.convex.stl │ ├── so100.srdf │ └── so100.urdf ├── so_arm100_with_panda.xml └── trs_so_arm100 │ ├── LICENSE │ ├── README.md │ ├── assets │ ├── Base.stl │ ├── Base_Motor.stl │ ├── Fixed_Jaw.stl │ ├── Fixed_Jaw_Collision_1.stl │ ├── Fixed_Jaw_Collision_2.stl │ ├── Fixed_Jaw_Motor.stl │ ├── Lower_Arm.stl │ ├── Lower_Arm_Motor.stl │ ├── Moving_Jaw.stl │ ├── Moving_Jaw_Collision_1.stl │ ├── Moving_Jaw_Collision_2.stl │ ├── Moving_Jaw_Collision_3.stl │ ├── Rotation_Pitch.stl │ ├── Rotation_Pitch_Motor.stl │ ├── Upper_Arm.stl │ ├── Upper_Arm_Motor.stl │ ├── Wrist_Pitch_Roll.stl │ ├── Wrist_Pitch_Roll_Motor.stl │ ├── angled_extrusion.stl │ ├── corner_bracket.stl │ ├── d405_solid.stl │ ├── extrusion_1000.stl │ ├── extrusion_1220.stl │ ├── extrusion_150.stl │ ├── extrusion_2040_1000.stl │ ├── extrusion_2040_880.stl │ ├── extrusion_600.stl │ ├── overhead_mount.stl │ ├── tablelegs.obj │ ├── tabletop.obj │ ├── vx300s_1_base.stl │ ├── vx300s_2_shoulder.stl │ ├── vx300s_3_upper_arm.stl │ ├── vx300s_4_upper_forearm.stl │ ├── vx300s_5_lower_forearm.stl │ ├── vx300s_6_wrist.stl │ ├── vx300s_7_gripper.stl │ ├── vx300s_7_gripper_bar.stl │ ├── vx300s_7_gripper_camera.stl │ ├── vx300s_7_gripper_prop.stl │ ├── vx300s_7_gripper_prop_bar.stl │ ├── vx300s_7_gripper_wrist_mount.stl │ ├── vx300s_8_custom_finger_left.stl │ ├── vx300s_8_custom_finger_right.stl │ └── wormseye_mount.stl │ ├── scene.xml │ ├── scene_with_cube.xml │ ├── scene_with_table.xml │ ├── scene_without_position.xml │ ├── so_arm100.xml │ └── so_arm100_without_position.xml ├── move_ball.py ├── mujoco_viewer.py ├── path_plan_ompl_rrtconnect.py ├── path_plan_pyroboplan_rrt.py ├── path_plan_pyroboplan_rrt_draw_trajectory.py ├── pid_torque_and_get.py ├── pinocchio-test1.py ├── requirements.txt ├── rl_panda.py ├── set_and_get_qvel.py ├── test_pinocchio.py ├── test_pyroboplan.py ├── test_why_continuous_2q.py ├── trajectory_plan_toppra.py └── uv.toml /.gitignore: -------------------------------------------------------------------------------- 1 | *TXT 2 | *.png 3 | *.pyc 4 | tensorboard 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mujoco-learning 2 | 3 | ## Related package 4 | 5 | ``` 6 | # uv 7 | uv venv 8 | source .venv/bin/activate 9 | uv pip install -r requirements.txt 10 | 11 | # install PyKDL 12 | bash install_pykdl.sh 13 | ``` 14 | 15 | ## Tutorials 16 | |file|url| 17 | |----|-------| 18 | |get_torque.py|[MuJoCo 解析 qfrc 三种力！带你测试鼠标拖拽物理交互效果](https://www.bilibili.com/video/BV1kH79zUEAc/?vd_source=5ba34935b7845cd15c65ef62c64ba82f)| 19 | |joint_impedance_control.py|[MuJoCo 机械臂关节空间阻抗控制Impedance实现（附代码）](https://www.bilibili.com/video/BV1UK5czMEQr/?vd_source=5ba34935b7845cd15c65ef62c64ba82f#reply262516173552)| 20 | |rl_panda.py|[MuJoCo 机械臂 PPO 强化学习逆向运动学（IK）](https://www.bilibili.com/video/BV1mHLVzzEMj?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 21 | |pid_torque_and_get.py|[MuJoCo 机械臂 PID 控制器输出力矩控制到达指定位置（附代码）](https://www.bilibili.com/video/BV1MbL6zSEAY?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 22 | |get_body_pos.py|[MuJoCo 仿真 Panda 机械臂！末端位置实时追踪 + 可视化（含缩放交互）](https://www.bilibili.com/video/BV1gaXxYaEnv?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 23 | |control_joint_pos.py|[MuJoCo 仿真 Panda 机械臂关节空间运动｜含完整代码](https://www.bilibili.com/video/BV1pWoBYcETJ?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 24 | |test_pinocchio.py|[Pinocchio 安装教程｜机器人学的必备库](https://www.bilibili.com/video/BV1UFoRYDEfF?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 25 | |control_ee_with_pinocchio.py|[【逆解机械臂】Pinocchio+MuJuCo 仿真 CLIK 闭环控制！附代码](https://www.bilibili.com/video/BV1aAZYYAE5f?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 26 | |move_ball.py|[MuJoCo 可视化键盘控制球体及位姿实时记录，附代码！](https://www.bilibili.com/video/BV1oTZrYaE2h?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 27 | |trajectory_plan_toppra.py|[MuJoCo 仿真 + TOPPRA 最优时间轨迹规划！机械臂运动效率拉满（附代码）](https://www.bilibili.com/video/BV1fndxYSEui?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 28 | |path_plan_ompl_rrtconnect.py|[MuJoCo + OMPL 进行Panda机械臂关节空间的RRT路径规划](https://www.bilibili.com/video/BV1EJd5YQExw?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 29 | |test_pyroboplan.py|[PyRoboPlan 库，给 panda 机械臂微分 IK 上大分，关节限位、碰撞全不怕](https://www.bilibili.com/video/BV1Rod6YHET2?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 30 | |path_plan_pyroboplan_rrt.py|[MuJoCo 机械臂关节路径规划+轨迹优化+末端轨迹可视化（附代码）](https://www.bilibili.com/video/BV1tZo7YjEgd?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 31 | |path_plan_pyroboplan_rrt_draw_trajectory.py|[MuJoCo 画出机械臂末端轨迹进行可视化（附代码）](https://www.bilibili.com/video/BV1B2ocYSE7r?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 32 | |ik_path_paln_trajectory_pyroboplan.py|[MuJoCo 提高机械臂笛卡尔空间IK+路径规划+轨迹优化的成功率及效率](https://www.bilibili.com/video/BV1qA5EzPEFh?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 33 | |mocap_panda.py|[MuJoCo 动捕接口 Mocap 直接操控机械臂（附代码）](https://www.bilibili.com/video/BV1k651zXEeN?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 34 | |set_and_get_qvel.py|[MuJoCo 关节角速度记录与可视化，监控机械臂运动状态](https://www.bilibili.com/video/BV1kSLdznEMd?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 35 | |get_camera_pic.py|[MuJoCo 相机图片怎么拿？视角调整获取物体图片及实时显示（附代码）](https://www.bilibili.com/video/BV1THGSzvE6t?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 36 | |test_why_continuous_2q.py|[Pinocchio导入URDF关节为continuous的问题及详细解释](https://www.bilibili.com/video/BV1tvVrzmEgx?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 37 | |contact_detect.py|[MuJoCo 机械臂物体碰撞、接触检测方式一](https://www.bilibili.com/video/BV12WfFYYE4T?vd_source=5ba34935b7845cd15c65ef62c64ba82f&spm_id_from=333.788.videopod.sections)| 38 | -------------------------------------------------------------------------------- /contact_detect.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import numpy as np 3 | import glfw 4 | import mujoco.viewer 5 | from scipy.optimize import minimize 6 | 7 | import numpy as np 8 | import pinocchio 9 | from numpy.linalg import norm, solve 10 | 11 | def inverse_kinematics(current_q, target_dir, target_pos): 12 | urdf_filename = './model/franka_panda_description/robots/panda_arm.urdf' 13 | # 从 URDF 文件构建机器人模型 14 | model = pinocchio.buildModelFromUrdf(urdf_filename) 15 | # 为模型创建数据对象，用于存储计算过程中的中间结果 16 | data = model.createData() 17 | 18 | # 指定要控制的关节 ID 19 | JOINT_ID = 7 20 | # 定义期望的位姿，使用目标姿态的旋转矩阵和目标位置创建 SE3 对象 21 | oMdes = pinocchio.SE3(target_dir, np.array(target_pos)) 22 | 23 | # 将当前关节角度赋值给变量 q，作为迭代的初始值 24 | q = current_q 25 | # 定义收敛阈值，当误差小于该值时认为算法收敛 26 | eps = 1e-4 27 | # 定义最大迭代次数，防止算法陷入无限循环 28 | IT_MAX = 1000 29 | # 定义积分步长，用于更新关节角度 30 | DT = 1e-2 31 | # 定义阻尼因子，用于避免矩阵奇异 32 | damp = 1e-12 33 | 34 | # 初始化迭代次数为 0 35 | i = 0 36 | while True: 37 | # 进行正运动学计算，得到当前关节角度下机器人各关节的位置和姿态 38 | pinocchio.forwardKinematics(model, data, q) 39 | # 计算目标位姿到当前位姿之间的变换 40 | iMd = data.oMi[JOINT_ID].actInv(oMdes) 41 | # 通过李群对数映射将变换矩阵转换为 6 维误差向量（包含位置误差和方向误差），用于量化当前位姿与目标位姿的差异 42 | err = pinocchio.log(iMd).vector 43 | 44 | # 判断误差是否小于收敛阈值，如果是则认为算法收敛 45 | if norm(err) < eps: 46 | success = True 47 | break 48 | # 判断迭代次数是否超过最大迭代次数，如果是则认为算法未收敛 49 | if i >= IT_MAX: 50 | success = False 51 | break 52 | 53 | # 计算当前关节角度下的雅可比矩阵，关节速度与末端速度的映射关系 54 | J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) 55 | # 对雅可比矩阵进行变换，转换到李代数空间，以匹配误差向量的坐标系，同时取反以调整误差方向 56 | J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) 57 | # 使用阻尼最小二乘法求解关节速度 58 | v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err)) 59 | # 根据关节速度更新关节角度 60 | q = pinocchio.integrate(model, q, v * DT) 61 | 62 | # 每迭代 10 次打印一次当前的误差信息 63 | if not i % 10: 64 | print(f"{i}: error = {err.T}") 65 | # 迭代次数加 1 66 | i += 1 67 | 68 | # 根据算法是否收敛输出相应的信息 69 | if success: 70 | print("Convergence achieved!") 71 | else: 72 | print( 73 | "\n" 74 | "Warning: the iterative algorithm has not reached convergence " 75 | "to the desired precision" 76 | ) 77 | 78 | # 打印最终的关节角度和误差向量 79 | print(f"\nresult: {q.flatten().tolist()}") 80 | print(f"\nfinal error: {err.T}") 81 | # 返回最终的关节角度向量（以列表形式） 82 | return q.flatten().tolist() 83 | 84 | def limit_angle(angle): 85 | while angle > np.pi: 86 | angle -= 2 * np.pi 87 | while angle < -np.pi: 88 | angle += 2 * np.pi 89 | return angle 90 | 91 | model = mujoco.MjModel.from_xml_path('./model/franka_emika_panda/scene.xml') 92 | data = mujoco.MjData(model) 93 | 94 | class CustomViewer: 95 | def __init__(self, model, data): 96 | self.handle = mujoco.viewer.launch_passive(model, data) 97 | self.pos = 0.0001 98 | 99 | # 找到末端执行器的 body id 100 | self.end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'left_finger') 101 | print(f"End effector ID: {self.end_effector_id}") 102 | if self.end_effector_id == -1: 103 | print("Warning: Could not find the end effector with the given name.") 104 | 105 | # 初始关节角度 106 | self.initial_q = data.qpos[:7].copy() 107 | print(f"Initial joint positions: {self.initial_q}") 108 | theta = np.pi 109 | self.R_x = np.array([ 110 | [1, 0, 0], 111 | [0, np.cos(theta), -np.sin(theta)], 112 | [0, np.sin(theta), np.cos(theta)] 113 | ]) 114 | 115 | self.x = 0.5 116 | self.new_q = self.initial_q 117 | 118 | def is_running(self): 119 | return self.handle.is_running() 120 | 121 | def sync(self): 122 | self.handle.sync() 123 | 124 | @property 125 | def cam(self): 126 | return self.handle.cam 127 | 128 | @property 129 | def viewport(self): 130 | return self.handle.viewport 131 | 132 | def run_loop(self): 133 | status = 0 134 | while self.is_running(): 135 | mujoco.mj_forward(model, data) 136 | if status == 0: 137 | if self.x < 0.6: 138 | self.x += 0.001 139 | new_q = inverse_kinematics(self.initial_q, self.R_x, [self.x, 0.0, 0.38]) 140 | else: 141 | status = 1 142 | elif status == 1: 143 | data.ctrl[7] = 10.0 144 | data.qpos[:7] = new_q 145 | # 遍历所有接触点 146 | for i in range(data.ncon): 147 | contact = data.contact[i] 148 | # 获取几何体对应的body_id 149 | body1_id = model.geom_bodyid[contact.geom1] 150 | body2_id = model.geom_bodyid[contact.geom2] 151 | 152 | # 通过mj_id2name转换body_id为名称 153 | body1_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, body1_id) 154 | body2_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, body2_id) 155 | 156 | print(f"接触点 {i}: 几何体 {contact.geom1} 名字 {body1_name} 和 {contact.geom2} 名字 {body2_name} 在位置 {contact.pos} 发生接触") 157 | mujoco.mj_step(model, data) 158 | self.sync() 159 | 160 | viewer = CustomViewer(model, data) 161 | viewer.cam.distance = 3 162 | viewer.cam.azimuth = 45 163 | viewer.cam.elevation = -30 164 | viewer.run_loop() -------------------------------------------------------------------------------- /control_ee_with_pinocchio.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import numpy as np 3 | import glfw 4 | import mujoco.viewer 5 | from scipy.optimize import minimize 6 | 7 | import numpy as np 8 | import pinocchio 9 | from numpy.linalg import norm, solve 10 | 11 | def inverse_kinematics(current_q, target_dir, target_pos): 12 | urdf_filename = '/home/dar/MuJoCoBin/mujoco-learning/franka_panda_description/robots/panda_arm.urdf' 13 | # 从 URDF 文件构建机器人模型 14 | model = pinocchio.buildModelFromUrdf(urdf_filename) 15 | # 为模型创建数据对象，用于存储计算过程中的中间结果 16 | data = model.createData() 17 | 18 | # 指定要控制的关节 ID 19 | JOINT_ID = 7 20 | # 定义期望的位姿，使用目标姿态的旋转矩阵和目标位置创建 SE3 对象 21 | oMdes = pinocchio.SE3(target_dir, np.array(target_pos)) 22 | 23 | # 将当前关节角度赋值给变量 q，作为迭代的初始值 24 | q = current_q 25 | # 定义收敛阈值，当误差小于该值时认为算法收敛 26 | eps = 1e-4 27 | # 定义最大迭代次数，防止算法陷入无限循环 28 | IT_MAX = 1000 29 | # 定义积分步长，用于更新关节角度 30 | DT = 1e-2 31 | # 定义阻尼因子，用于避免矩阵奇异 32 | damp = 1e-12 33 | 34 | # 初始化迭代次数为 0 35 | i = 0 36 | while True: 37 | # 进行正运动学计算，得到当前关节角度下机器人各关节的位置和姿态 38 | pinocchio.forwardKinematics(model, data, q) 39 | # 计算目标位姿到当前位姿之间的变换 40 | iMd = data.oMi[JOINT_ID].actInv(oMdes) 41 | # 通过李群对数映射将变换矩阵转换为 6 维误差向量（包含位置误差和方向误差），用于量化当前位姿与目标位姿的差异 42 | err = pinocchio.log(iMd).vector 43 | 44 | # 判断误差是否小于收敛阈值，如果是则认为算法收敛 45 | if norm(err) < eps: 46 | success = True 47 | break 48 | # 判断迭代次数是否超过最大迭代次数，如果是则认为算法未收敛 49 | if i >= IT_MAX: 50 | success = False 51 | break 52 | 53 | # 计算当前关节角度下的雅可比矩阵，关节速度与末端速度的映射关系 54 | J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) 55 | # 对雅可比矩阵进行变换，转换到李代数空间，以匹配误差向量的坐标系，同时取反以调整误差方向 56 | J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) 57 | # 使用阻尼最小二乘法求解关节速度 58 | v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err)) 59 | # 根据关节速度更新关节角度 60 | q = pinocchio.integrate(model, q, v * DT) 61 | 62 | # 每迭代 10 次打印一次当前的误差信息 63 | if not i % 10: 64 | print(f"{i}: error = {err.T}") 65 | # 迭代次数加 1 66 | i += 1 67 | 68 | # 根据算法是否收敛输出相应的信息 69 | if success: 70 | print("Convergence achieved!") 71 | else: 72 | print( 73 | "\n" 74 | "Warning: the iterative algorithm has not reached convergence " 75 | "to the desired precision" 76 | ) 77 | 78 | # 打印最终的关节角度和误差向量 79 | print(f"\nresult: {q.flatten().tolist()}") 80 | print(f"\nfinal error: {err.T}") 81 | # 返回最终的关节角度向量（以列表形式） 82 | return q.flatten().tolist() 83 | 84 | def limit_angle(angle): 85 | while angle > np.pi: 86 | angle -= 2 * np.pi 87 | while angle < -np.pi: 88 | angle += 2 * np.pi 89 | return angle 90 | 91 | model = mujoco.MjModel.from_xml_path('/home/dar/MuJoCoBin/mujoco_menagerie/franka_emika_panda/scene.xml') 92 | data = mujoco.MjData(model) 93 | 94 | class CustomViewer: 95 | def __init__(self, model, data): 96 | self.handle = mujoco.viewer.launch_passive(model, data) 97 | self.pos = 0.0001 98 | 99 | # 找到末端执行器的 body id 100 | self.end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'left_finger') 101 | print(f"End effector ID: {self.end_effector_id}") 102 | if self.end_effector_id == -1: 103 | print("Warning: Could not find the end effector with the given name.") 104 | 105 | # 初始关节角度 106 | self.initial_q = data.qpos[:7].copy() 107 | print(f"Initial joint positions: {self.initial_q}") 108 | theta = np.pi 109 | self.R_x = np.array([ 110 | [1, 0, 0], 111 | [0, np.cos(theta), -np.sin(theta)], 112 | [0, np.sin(theta), np.cos(theta)] 113 | ]) 114 | 115 | self.x = 0.3 116 | self.new_q = self.initial_q 117 | 118 | def is_running(self): 119 | return self.handle.is_running() 120 | 121 | def sync(self): 122 | self.handle.sync() 123 | 124 | @property 125 | def cam(self): 126 | return self.handle.cam 127 | 128 | @property 129 | def viewport(self): 130 | return self.handle.viewport 131 | 132 | def run_loop(self): 133 | status = 0 134 | while self.is_running(): 135 | mujoco.mj_forward(model, data) 136 | if self.x < 0.6: 137 | self.x += 0.001 138 | new_q = inverse_kinematics(self.initial_q, self.R_x, [self.x, 0.2, 0.7]) 139 | data.qpos[:7] = new_q 140 | mujoco.mj_step(model, data) 141 | self.sync() 142 | 143 | viewer = CustomViewer(model, data) 144 | viewer.cam.distance = 3 145 | viewer.cam.azimuth = 0 146 | viewer.cam.elevation = -30 147 | viewer.run_loop() -------------------------------------------------------------------------------- /control_joint_pos.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import numpy as np 3 | import glfw 4 | from scipy.optimize import minimize 5 | 6 | def scroll_callback(window, xoffset, yoffset): 7 | global cam 8 | # 调整相机的缩放比例 9 | cam.distance *= 1 - 0.1 * yoffset 10 | 11 | def limit_angle(angle): 12 | while angle > np.pi: 13 | angle -= 2 * np.pi 14 | while angle < -np.pi: 15 | angle += 2 * np.pi 16 | return angle 17 | 18 | def main(): 19 | global cam 20 | # 加载模型 21 | model = mujoco.MjModel.from_xml_path('./model/franka_emika_panda/scene.xml') 22 | data = mujoco.MjData(model) 23 | 24 | # 打印所有 body 的 ID 和名称 25 | print("All bodies in the model:") 26 | for i in range(model.nbody): 27 | body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i) 28 | print(f"ID: {i}, Name: {body_name}") 29 | 30 | # 初始化 GLFW 31 | if not glfw.init(): 32 | return 33 | 34 | window = glfw.create_window(1200, 900, 'Panda Arm Control', None, None) 35 | if not window: 36 | glfw.terminate() 37 | return 38 | 39 | glfw.make_context_current(window) 40 | 41 | # 设置鼠标滚轮回调函数 42 | glfw.set_scroll_callback(window, scroll_callback) 43 | 44 | # 初始化渲染器 45 | cam = mujoco.MjvCamera() 46 | opt = mujoco.MjvOption() 47 | mujoco.mjv_defaultCamera(cam) 48 | mujoco.mjv_defaultOption(opt) 49 | pert = mujoco.MjvPerturb() 50 | con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value) 51 | 52 | scene = mujoco.MjvScene(model, maxgeom=10000) 53 | 54 | # 找到末端执行器的 body id 55 | end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'hand') 56 | print(f"End effector ID: {end_effector_id}") 57 | if end_effector_id == -1: 58 | print("Warning: Could not find the end effector with the given name.") 59 | glfw.terminate() 60 | return 61 | 62 | # 初始关节角度 63 | initial_q = data.qpos[:7].copy() 64 | print(f"Initial joint positions: {initial_q}") 65 | 66 | while not glfw.window_should_close(window): 67 | 68 | # 获取当前末端执行器位置 69 | mujoco.mj_forward(model, data) 70 | end_effector_pos = data.body(end_effector_id).xpos 71 | 72 | initial_q[0] = initial_q[0] + 0.1 73 | initial_q[0] = limit_angle(initial_q[0]) 74 | new_q = initial_q 75 | # 设置关节目标位置 76 | data.qpos[:7] = new_q 77 | 78 | # 模拟一步 79 | mujoco.mj_step(model, data) 80 | 81 | # 更新渲染场景 82 | viewport = mujoco.MjrRect(0, 0, 1200, 900) 83 | mujoco.mjv_updateScene(model, data, opt, pert, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene) 84 | mujoco.mjr_render(viewport, scene, con) 85 | 86 | # 交换前后缓冲区 87 | glfw.swap_buffers(window) 88 | glfw.poll_events() 89 | 90 | # 清理资源 91 | glfw.terminate() 92 | 93 | 94 | if __name__ == "__main__": 95 | main() 96 | -------------------------------------------------------------------------------- /get_body_pos.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import time 3 | import mujoco_viewer 4 | import numpy as np 5 | 6 | class Test(mujoco_viewer.CustomViewer): 7 | def __init__(self, path): 8 | super().__init__(path, 3, azimuth=-45, elevation=-30) 9 | self.path = path 10 | 11 | def runBefore(self): 12 | self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'link7') 13 | print(f"End effector ID: {self.end_effector_id}") 14 | if self.end_effector_id == -1: 15 | # 如果未找到指定名称的末端执行器，打印警告信息并终止 GLFW 16 | print("Warning: Could not find the end effector with the given name.") 17 | 18 | def runFunc(self): 19 | end_effector_pos = self.data.body(self.end_effector_id).xpos, self.data.body(self.end_effector_id).xquat 20 | print(f"End effector position: {end_effector_pos}") 21 | # time.sleep(0.01) 22 | 23 | test = Test("./model/franka_emika_panda/scene.xml") 24 | test.run_loop() -------------------------------------------------------------------------------- /get_camera_pic.py: -------------------------------------------------------------------------------- 1 | 2 | import mujoco 3 | import numpy as np 4 | import glfw 5 | import cv2 6 | 7 | resolution = (640, 480) 8 | # 创建OpenGL上下文（离屏渲染） 9 | glfw.init() 10 | glfw.window_hint(glfw.VISIBLE, glfw.FALSE) 11 | window = glfw.create_window(resolution[0], resolution[1], "Offscreen", None, None) 12 | glfw.make_context_current(window) 13 | 14 | model = mujoco.MjModel.from_xml_path('./model/franka_emika_panda/scene_withcamera.xml') 15 | data = mujoco.MjData(model) 16 | scene = mujoco.MjvScene(model, maxgeom=10000) 17 | context = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value) 18 | 19 | # 设置相机参数 20 | camera_name = "rgb_camera" 21 | camera_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, camera_name) 22 | camera = mujoco.MjvCamera() 23 | # 使用固定相机 24 | # camera.type = mujoco.mjtCamera.mjCAMERA_FIXED 25 | # 设置相机为跟踪模式 26 | camera.type = mujoco.mjtCamera.mjCAMERA_TRACKING 27 | if camera_id != -1: 28 | print("camera_id", camera_id) 29 | camera.fixedcamid = camera_id 30 | 31 | # 创建帧缓冲对象 32 | framebuffer = mujoco.MjrRect(0, 0, resolution[0], resolution[1]) 33 | mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, context) 34 | 35 | while True: 36 | mujoco.mj_step(model, data) 37 | tracking_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cube") 38 | camera.trackbodyid = tracking_body_id 39 | camera.distance = 1.5 # 相机与目标的距离 40 | camera.azimuth = 0 # 水平方位角（度） 41 | camera.elevation = -90 # 俯仰角（度） 42 | viewport = mujoco.MjrRect(0, 0, resolution[0], resolution[1]) 43 | mujoco.mjv_updateScene(model, data, mujoco.MjvOption(), 44 | mujoco.MjvPerturb(), camera, 45 | mujoco.mjtCatBit.mjCAT_ALL, scene) 46 | mujoco.mjr_render(viewport, scene, context) 47 | rgb = np.zeros((resolution[1], resolution[0], 3), dtype=np.uint8) 48 | mujoco.mjr_readPixels(rgb, None, viewport, context) 49 | # 转换颜色空间 (OpenCV使用BGR格式) 50 | bgr = cv2.cvtColor(np.flipud(rgb), cv2.COLOR_RGB2BGR) 51 | cv2.imshow('MuJoCo Camera Output', bgr) 52 | if cv2.waitKey(1) == 27: 53 | break 54 | 55 | cv2.imwrite('debug_output.png', bgr) 56 | cv2.destroyAllWindows() 57 | glfw.terminate() 58 | del context 59 | del scene -------------------------------------------------------------------------------- /get_torque.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import numpy as np 3 | # matplotlib.use('TkAgg') 4 | import matplotlib.pyplot as plt 5 | import time,math 6 | 7 | class Test(mujoco_viewer.CustomViewer): 8 | def __init__(self, path): 9 | super().__init__(path, 3, azimuth=180, elevation=-30) 10 | self.path = path 11 | 12 | def runBefore(self): 13 | # 存储关节力矩的列表 14 | self.torque_history = [] 15 | self.time_history = [] 16 | 17 | def runFunc(self): 18 | if True: 19 | self.time_history.append(self.data.time) 20 | self.torque_history.append(self.data.qfrc_actuator.copy()) # 存储关节力矩 21 | if len(self.torque_history) > 20000: 22 | torque_history = np.array(self.torque_history) 23 | # 绘制关节力矩曲线 24 | plt.figure(figsize=(10, 6)) 25 | for i in range(torque_history.shape[1]): 26 | plt.subplot(torque_history.shape[1], 1, i + 1) 27 | plt.plot(self.time_history, torque_history[:, i], label=f'Joint {i + 1} Torque') 28 | plt.xlabel('Time (s)') 29 | plt.ylabel('Torque (N·m)') 30 | plt.title(f'Joint {i + 1} Torque Over Time') 31 | plt.legend() 32 | plt.grid(True) 33 | # plt.tight_layout() 34 | plt.show() 35 | 36 | if __name__ == "__main__": 37 | test = Test("model/trs_so_arm100/scene.xml") 38 | test.run_loop() 39 | 40 | -------------------------------------------------------------------------------- /ik_path_paln_trajectory_pyroboplan.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import mujoco,time,threading 3 | import numpy as np 4 | import pinocchio 5 | import matplotlib 6 | # matplotlib.use('TkAgg') 7 | import matplotlib.pyplot as plt 8 | from mpl_toolkits.mplot3d import Axes3D 9 | import itertools 10 | 11 | from pyroboplan.core.utils import ( 12 | get_random_collision_free_state, 13 | extract_cartesian_poses, 14 | ) 15 | from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions 16 | from pyroboplan.ik.nullspace_components import ( 17 | joint_limit_nullspace_component, 18 | collision_avoidance_nullspace_component, 19 | ) 20 | 21 | from pyroboplan.models.panda import ( 22 | load_models, 23 | add_self_collisions, 24 | add_object_collisions, 25 | ) 26 | from pyroboplan.planning.rrt import RRTPlanner, RRTPlannerOptions 27 | from pyroboplan.trajectory.trajectory_optimization import ( 28 | CubicTrajectoryOptimization, 29 | CubicTrajectoryOptimizationOptions, 30 | ) 31 | 32 | class Test(mujoco_viewer.CustomViewer): 33 | def __init__(self, path): 34 | super().__init__(path, 3, azimuth=180, elevation=-30) 35 | self.path = path 36 | 37 | def runBefore(self): 38 | # Create models and data 39 | self.model_roboplan, self.collision_model, visual_model = load_models(use_sphere_collisions=True) 40 | add_self_collisions(self.model_roboplan, self.collision_model) 41 | add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1) 42 | 43 | data = self.model_roboplan.createData() 44 | collision_data = self.collision_model.createData() 45 | 46 | self.target_frame = "panda_hand" 47 | ignore_joint_indices = [ 48 | self.model_roboplan.getJointId("panda_finger_joint1") - 1, 49 | self.model_roboplan.getJointId("panda_finger_joint2") - 1, 50 | ] 51 | np.set_printoptions(precision=3) 52 | 53 | # Set up the IK solver 54 | options = DifferentialIkOptions( 55 | max_iters=200, 56 | max_retries=10, 57 | damping=0.0001, 58 | min_step_size=0.05, 59 | max_step_size=0.1, 60 | ignore_joint_indices=ignore_joint_indices, 61 | rng_seed=None, 62 | ) 63 | self.ik = DifferentialIk( 64 | self.model_roboplan, 65 | data=data, 66 | collision_model=self.collision_model, 67 | options=options, 68 | visualizer=None, 69 | ) 70 | self.nullspace_components = [ 71 | lambda model_roboplan, q: collision_avoidance_nullspace_component( 72 | model_roboplan, 73 | data, 74 | self.collision_model, 75 | collision_data, 76 | q, 77 | gain=1.0, 78 | dist_padding=0.05, 79 | ), 80 | lambda model_roboplan, q: joint_limit_nullspace_component( 81 | model_roboplan, q, gain=1.0, padding=0.025 82 | ), 83 | ] 84 | 85 | theta = np.pi 86 | rotation_matrix = np.array([ 87 | [1, 0, 0], 88 | [0, np.cos(theta), -np.sin(theta)], 89 | [0, np.sin(theta), np.cos(theta)] 90 | ]) 91 | 92 | q_start = self.getIk(self.random_valid_state(), rotation_matrix, [0.4, 0.0, 0.4]) 93 | q_goal = self.getIk(self.random_valid_state(), rotation_matrix, [0.3, 0.0, 0.5]) 94 | 95 | while True: 96 | # Search for a path 97 | options = RRTPlannerOptions( 98 | max_step_size=0.05, 99 | max_connection_dist=0.5, 100 | rrt_connect=False, 101 | bidirectional_rrt=False, 102 | rrt_star=True, 103 | max_rewire_dist=0.5, 104 | max_planning_time=5.0, 105 | fast_return=True, 106 | goal_biasing_probability=0.25, 107 | collision_distance_padding=0.0001, 108 | ) 109 | print("") 110 | print(f"Planning a path...") 111 | planner = RRTPlanner(self.model_roboplan, self.collision_model, options=options) 112 | 113 | q_path = planner.plan(q_start, q_goal) 114 | print(f"Path found with {len(q_path)} waypoints") 115 | if q_path is not None and len(q_path) > 0: 116 | print(f"Got a path with {len(q_path)} waypoints") 117 | if len(q_path) > 50: 118 | print("Path is too long, skipping...") 119 | continue 120 | else: 121 | print("Failed to plan.") 122 | 123 | # Perform trajectory optimization. 124 | dt = 0.025 125 | options = CubicTrajectoryOptimizationOptions( 126 | num_waypoints=len(q_path), 127 | samples_per_segment=1, 128 | min_segment_time=0.5, 129 | max_segment_time=10.0, 130 | min_vel=-1.5, 131 | max_vel=1.5, 132 | min_accel=-0.75, 133 | max_accel=0.75, 134 | min_jerk=-1.0, 135 | max_jerk=1.0, 136 | max_planning_time=1.0, 137 | check_collisions=False, 138 | min_collision_dist=0.001, 139 | collision_influence_dist=0.05, 140 | collision_avoidance_cost_weight=0.0, 141 | collision_link_list=[ 142 | "obstacle_box_1", 143 | "obstacle_box_2", 144 | "obstacle_sphere_1", 145 | "obstacle_sphere_2", 146 | "ground_plane", 147 | "panda_hand", 148 | ], 149 | ) 150 | print("Optimizing the path...") 151 | optimizer = CubicTrajectoryOptimization(self.model_roboplan, self.collision_model, options) 152 | traj = optimizer.plan([q_path[0], q_path[-1]], init_path=q_path) 153 | 154 | if traj is None: 155 | print("Retrying with all the RRT waypoints...") 156 | traj = optimizer.plan(q_path, init_path=q_path) 157 | 158 | if traj is not None: 159 | print("Trajectory optimization successful") 160 | traj_gen = traj.generate(dt) 161 | self.q_vec = traj_gen[1] 162 | print(f"path has {self.q_vec.shape[1]} points") 163 | self.tforms = extract_cartesian_poses(self.model_roboplan, "panda_hand", self.q_vec.T) 164 | self.plot(self.tforms) 165 | break 166 | self.index = 0 167 | 168 | def plot(self, tfs): 169 | positions = [] 170 | for tform in tfs: 171 | position = tform.translation 172 | positions.append(position) 173 | positions = np.array(positions) 174 | fig = plt.figure() 175 | ax = fig.add_subplot(111, projection='3d') 176 | ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], marker='o') 177 | 178 | ax.set_xlabel('X') 179 | ax.set_ylabel('Y') 180 | ax.set_zlabel('Z') 181 | 182 | plt.show(block=False) 183 | plt.pause(0.001) 184 | 185 | def getIk(self, init_state, rotation_matrix, pose): 186 | while True: 187 | self.init_state = init_state 188 | target_tform = pinocchio.SE3(rotation_matrix, np.array(pose)) 189 | q_sol = self.ik.solve( 190 | self.target_frame, 191 | target_tform, 192 | init_state=self.init_state, 193 | nullspace_components=self.nullspace_components, 194 | verbose=True, 195 | ) 196 | if q_sol is not None: 197 | print("IK solution found") 198 | return q_sol 199 | 200 | def random_valid_state(self): 201 | return get_random_collision_free_state( 202 | self.model_roboplan, self.collision_model, distance_padding=0.001 203 | ) 204 | 205 | def runFunc(self): 206 | self.data.qpos[:7] = self.q_vec[:7, self.index] 207 | self.index += 1 208 | if self.index >= self.q_vec.shape[1]: 209 | self.index = 0 210 | time.sleep(0.01) 211 | 212 | if __name__ == "__main__": 213 | test = Test("model/franka_emika_panda/scene.xml") 214 | test.run_loop() 215 | 216 | -------------------------------------------------------------------------------- /ik_pyroboplan_so_arm100.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import mujoco,time 3 | import numpy as np 4 | import pinocchio 5 | 6 | from pyroboplan.core.utils import ( 7 | get_random_collision_free_state, 8 | get_random_collision_free_transform, 9 | ) 10 | from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions 11 | from pyroboplan.ik.nullspace_components import ( 12 | joint_limit_nullspace_component, 13 | collision_avoidance_nullspace_component, 14 | ) 15 | from pyroboplan.models.panda import ( 16 | load_models, 17 | add_self_collisions, 18 | add_object_collisions, 19 | ) 20 | 21 | class Test(mujoco_viewer.CustomViewer): 22 | def __init__(self, path, urdf_filepath=None, models_folder=None): 23 | super().__init__(path, 3, azimuth=-45, elevation=-30) 24 | self.path = path 25 | self.urdf_filepath = urdf_filepath 26 | self.models_folder = models_folder 27 | 28 | def runBefore(self): 29 | # Create models and data 30 | self.model_roboplan, self.collision_model, visual_model = pinocchio.buildModelsFromUrdf(self.urdf_filepath, package_dirs=self.models_folder) 31 | 32 | # add_self_collisions(self.model_roboplan, self.collision_model) 33 | # add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1) 34 | 35 | data = self.model_roboplan.createData() 36 | collision_data = self.collision_model.createData() 37 | 38 | self.target_frame = "Fixed_Jaw" 39 | ignore_joint_indices = [ 40 | # self.model_roboplan.getJointId("panda_finger_joint1") - 1, 41 | # self.model_roboplan.getJointId("panda_finger_joint2") - 1, 42 | ] 43 | np.set_printoptions(precision=3) 44 | # Set up the IK solver 45 | options = DifferentialIkOptions( 46 | max_iters=2000, 47 | max_retries=10, 48 | damping=0.0001, 49 | min_step_size=0.01, 50 | max_step_size=0.05, 51 | ignore_joint_indices=ignore_joint_indices, 52 | rng_seed=None, 53 | ) 54 | self.ik = DifferentialIk( 55 | self.model_roboplan, 56 | data=data, 57 | collision_model=self.collision_model, 58 | options=options, 59 | visualizer=None, 60 | ) 61 | self.nullspace_components = [ 62 | lambda model_roboplan, q: collision_avoidance_nullspace_component( 63 | model_roboplan, 64 | data, 65 | self.collision_model, 66 | collision_data, 67 | q, 68 | gain=1.0, 69 | dist_padding=0.05, 70 | ), 71 | lambda model_roboplan, q: joint_limit_nullspace_component( 72 | model_roboplan, q, gain=0.1, padding=0.025 73 | ), 74 | ] 75 | self.x = 0.1 76 | self.init_state = self.data.qpos.copy() 77 | 78 | def runFunc(self): 79 | self.init_state = get_random_collision_free_state(self.model_roboplan, self.collision_model) 80 | # target_tform = get_random_collision_free_transform( 81 | # self.model_roboplan, 82 | # self.collision_model, 83 | # self.target_frame, 84 | # joint_padding=0.05, 85 | # ) 86 | 87 | theta = np.pi 88 | rotation_matrix = np.array([ 89 | [1, 0, 0], 90 | [0, np.cos(theta), -np.sin(theta)], 91 | [0, np.sin(theta), np.cos(theta)] 92 | ]) 93 | # quat = [0.29, 0.79, -0.50, 0.13] 94 | # rotation_matrix = pinocchio.Quaternion(*quat).matrix() 95 | 96 | target_tform = pinocchio.SE3(rotation_matrix, np.array([self.x, -0.0, 0.3])) 97 | 98 | # print(target_tform) 99 | q_sol = self.ik.solve( 100 | self.target_frame, 101 | target_tform, 102 | init_state=self.init_state, 103 | nullspace_components=self.nullspace_components, 104 | verbose=True, 105 | ) 106 | # self.init_state = self.data.qpos.copy() 107 | if q_sol is not None: 108 | self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'Fixed_Jaw') 109 | print(f"End effector position: {self.data.body(self.end_effector_id).xpos}") 110 | print(f"q_sol: {q_sol}") 111 | self.data.qpos[:6] = q_sol[:6] 112 | self.x += 0.001 113 | else: 114 | print("No solution found.") 115 | time.sleep(0.01) 116 | 117 | if __name__ == "__main__": 118 | test = Test("model/trs_so_arm100/scene.xml", urdf_filepath="model/so_arm100_description/so100.urdf", models_folder="model/so_arm100_description") 119 | test.run_loop() 120 | 121 | -------------------------------------------------------------------------------- /impedance_control.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import mujoco 5 | import time 6 | import mujoco_viewer 7 | import numpy as np 8 | 9 | class Test(mujoco_viewer.CustomViewer): 10 | def __init__(self, path): 11 | super().__init__(path, 3, azimuth=-45, elevation=-30) 12 | self.path = path 13 | 14 | def runBefore(self): 15 | # 阻抗控制参数 16 | # 确保 Kp 和 Kd 的维度与实际控制的关节数量匹配 17 | self.Kp = np.diag([100] * self.model.nu) # 刚度矩阵 18 | self.Kd = np.diag([10] * self.model.nu) # 阻尼矩阵 19 | 20 | # 目标关节角度 21 | # self.q_desired = np.zeros(self.model.nu) 22 | self.q_desired = [0.0, -0.991, 0.196, 0.662, -0.88, 0.66] 23 | 24 | # 仿真参数 25 | self.total_time = 30 # 总仿真时间（秒） 26 | self.dt = self.model.opt.timestep # 仿真时间步长 27 | self.num_steps = int(self.total_time / self.dt) 28 | 29 | # 存储数据 30 | self.q_history = np.zeros((self.num_steps, self.model.nu)) 31 | self.qdot_history = np.zeros((self.num_steps, self.model.nu)) 32 | self.torque_history = np.zeros((self.num_steps, self.model.nu)) 33 | self.index = 0 34 | 35 | def runFunc(self): 36 | # 读取当前关节角度和速度 37 | q = self.data.qpos[:self.model.nu] 38 | qdot = self.data.qvel[:self.model.nu] 39 | 40 | # 计算阻抗控制扭矩 41 | error = self.q_desired - q 42 | print(self.index, self.num_steps, self.model.nu, error) 43 | torque = self.Kp @ error - self.Kd @ qdot 44 | 45 | # 设置控制输入 46 | self.data.ctrl[:] = torque 47 | 48 | # 存储数据 49 | self.q_history[self.index] = q 50 | self.qdot_history[self.index] = qdot 51 | self.torque_history[self.index] = torque 52 | self.index += 1 53 | 54 | if self.index >= self.num_steps: 55 | # # 绘制结果 56 | time = np.arange(0, self.total_time, self.dt) 57 | 58 | plt.figure(figsize=(12, 8)) 59 | 60 | # 绘制关节角度 61 | plt.subplot(3, 1, 1) 62 | for j in range(self.model.nu): 63 | plt.plot(time, self.q_history[:, j], label=f'Joint {j+1}') 64 | plt.title('Joint Angles') 65 | plt.xlabel('Time (s)') 66 | plt.ylabel('Angle (rad)') 67 | plt.legend() 68 | 69 | # 绘制关节速度 70 | plt.subplot(3, 1, 2) 71 | for j in range(self.model.nu): 72 | plt.plot(time, self.qdot_history[:, j], label=f'Joint {j+1}') 73 | plt.title('Joint Velocities') 74 | plt.xlabel('Time (s)') 75 | plt.ylabel('Velocity (rad/s)') 76 | plt.legend() 77 | 78 | # 绘制控制扭矩 79 | plt.subplot(3, 1, 3) 80 | for j in range(self.model.nu): 81 | plt.plot(time, self.torque_history[:, j], label=f'Joint {j+1}') 82 | plt.title('Control Torques') 83 | plt.xlabel('Time (s)') 84 | plt.ylabel('Torque (N.m)') 85 | plt.legend() 86 | 87 | plt.tight_layout() 88 | plt.show() 89 | 90 | test = Test("./model/trs_so_arm100/scene_without_position.xml") 91 | test.run_loop() 92 | 93 | -------------------------------------------------------------------------------- /install_pykdl.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ ! -d "orocos_kinematics_dynamics" ]; then 4 | git clone https://github.com/orocos/orocos_kinematics_dynamics.git 5 | cd orocos_kinematics_dynamics 6 | cd orocos_kdl 7 | mkdir build 8 | cd build 9 | cmake .. 10 | make -j$(nproc) 11 | sudo make install 12 | cd ../.. 13 | cd python_orocos_kdl 14 | mkdir build 15 | cd build 16 | cmake .. 17 | make -j$(nproc) 18 | sudo make install 19 | cp PyKDL.so* ../../../.venv/lib/python3.10/site-packages/ 20 | fi 21 | 22 | rm -rf orocos_kinematics_dynamics 23 | -------------------------------------------------------------------------------- /joint_impedance_control.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import time 5 | import mujoco_viewer 6 | 7 | class Test(mujoco_viewer.CustomViewer): 8 | def __init__(self, path): 9 | super().__init__(path, 1, azimuth=45, elevation=-30) 10 | self.path = path 11 | 12 | def runBefore(self): 13 | # 阻抗控制参数 14 | self.Kp = np.diag([10] * self.model.nu) # 刚度矩阵 15 | self.Kd = np.diag([10] * self.model.nu) # 阻尼矩阵 16 | 17 | # 目标关节角度 18 | # self.q_desired = np.zeros(self.model.nu) 19 | self.q_desired = [0.0, -0.991, 0.196, 0.662, -0.88, 0.66] 20 | 21 | # 仿真参数 22 | self.total_time = 30 # 总仿真时间（秒） 23 | self.dt = self.model.opt.timestep # 仿真时间步长 24 | self.num_steps = int(self.total_time / self.dt) 25 | 26 | # 存储数据 27 | self.q_history = np.zeros((self.num_steps, self.model.nu)) 28 | self.qdot_history = np.zeros((self.num_steps, self.model.nu)) 29 | self.torque_history = np.zeros((self.num_steps, self.model.nu)) 30 | self.index = 0 31 | 32 | def runFunc(self): 33 | # 读取当前关节角度和速度 34 | q = self.data.qpos[:self.model.nu] 35 | qdot = self.data.qvel[:self.model.nu] 36 | 37 | # 计算阻抗控制扭矩 38 | error = self.q_desired - q 39 | print(self.index, self.num_steps, self.model.nu, error) 40 | torque = self.Kp @ error - self.Kd @ qdot 41 | 42 | # 设置控制输入 43 | self.data.ctrl[:] = torque 44 | 45 | if False: 46 | self.q_history[self.index] = q 47 | self.qdot_history[self.index] = qdot 48 | self.torque_history[self.index] = torque 49 | self.index += 1 50 | 51 | if self.index >= self.num_steps: 52 | # # 绘制结果 53 | time = np.arange(0, self.total_time, self.dt) 54 | 55 | plt.figure(figsize=(12, 8)) 56 | 57 | # 绘制关节角度 58 | plt.subplot(3, 1, 1) 59 | for j in range(self.model.nu): 60 | plt.plot(time, self.q_history[:, j], label=f'Joint {j+1}') 61 | plt.title('Joint Angles') 62 | plt.xlabel('Time (s)') 63 | plt.ylabel('Angle (rad)') 64 | plt.legend() 65 | 66 | # 绘制关节速度 67 | plt.subplot(3, 1, 2) 68 | for j in range(self.model.nu): 69 | plt.plot(time, self.qdot_history[:, j], label=f'Joint {j+1}') 70 | plt.title('Joint Velocities') 71 | plt.xlabel('Time (s)') 72 | plt.ylabel('Velocity (rad/s)') 73 | plt.legend() 74 | 75 | # 绘制控制扭矩 76 | plt.subplot(3, 1, 3) 77 | for j in range(self.model.nu): 78 | plt.plot(time, self.torque_history[:, j], label=f'Joint {j+1}') 79 | plt.title('Control Torques') 80 | plt.xlabel('Time (s)') 81 | plt.ylabel('Torque (N.m)') 82 | plt.legend() 83 | 84 | plt.tight_layout() 85 | plt.show() 86 | 87 | test = Test("./model/trs_so_arm100/scene.xml") 88 | test.run_loop() 89 | 90 | -------------------------------------------------------------------------------- /mjcf2usd.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | from mujoco.usd import exporter 3 | 4 | m = mujoco.MjModel.from_xml_path('./model/franka_emika_panda/scene.xml') 5 | d = mujoco.MjData(m) 6 | 7 | # Create the USDExporter 8 | exp = exporter.USDExporter(model=m) 9 | 10 | duration = 5 11 | framerate = 60 12 | while d.time < duration: 13 | 14 | # Step the physics 15 | mujoco.mj_step(m, d) 16 | 17 | if exp.frame_count < d.time * framerate: 18 | # Update the USD with a new frame 19 | exp.update_scene(data=d) 20 | 21 | # Export the USD file 22 | exp.save_scene(filetype="usd") -------------------------------------------------------------------------------- /mocap_panda.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | import time 4 | import mujoco_viewer 5 | from pynput import keyboard 6 | 7 | key_states = { 8 | keyboard.Key.up: False, 9 | keyboard.Key.down: False, 10 | keyboard.Key.left: False, 11 | keyboard.Key.right: False, 12 | keyboard.Key.alt_l: False, 13 | keyboard.Key.alt_r: False, 14 | } 15 | 16 | def on_press(key): 17 | if key in key_states: 18 | key_states[key] = True 19 | 20 | def on_release(key): 21 | if key in key_states: 22 | key_states[key] = False 23 | 24 | listener = keyboard.Listener(on_press=on_press, on_release=on_release) 25 | listener.start() 26 | 27 | class Test(mujoco_viewer.CustomViewer): 28 | def __init__(self, path): 29 | super().__init__(path, 3, azimuth=-45, elevation=-30) 30 | self.path = path 31 | 32 | def runBefore(self): 33 | theta = np.pi 34 | rotation_matrix = np.array([ 35 | [1, 0, 0], 36 | [0, np.cos(theta), -np.sin(theta)], 37 | [0, np.sin(theta), np.cos(theta)] 38 | ]) 39 | se3 = pin.SE3(rotation_matrix, np.zeros(3)) 40 | quaternion = pin.Quaternion(se3.rotation) 41 | self.quat = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] 42 | 43 | def runFunc(self): 44 | if key_states[keyboard.Key.up]: 45 | self.data.mocap_pos[0, 2] += 0.001 46 | if key_states[keyboard.Key.down]: 47 | self.data.mocap_pos[0, 2] -= 0.001 48 | if key_states[keyboard.Key.left]: 49 | self.data.mocap_pos[0, 0] -= 0.001 50 | if key_states[keyboard.Key.right]: 51 | self.data.mocap_pos[0, 0] += 0.001 52 | if key_states[keyboard.Key.alt_l]: 53 | self.data.mocap_pos[0, 1] += 0.001 54 | if key_states[keyboard.Key.alt_r]: 55 | self.data.mocap_pos[0, 1] -= 0.001 56 | 57 | self.data.mocap_quat[0] = self.quat 58 | time.sleep(0.01) 59 | 60 | test = Test("./model/franka_emika_panda/scene.xml") 61 | test.run_loop() 62 | -------------------------------------------------------------------------------- /model/ball.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /model/franka_emika_panda/README.md: -------------------------------------------------------------------------------- 1 | # Franka Emika Panda Description (MJCF) 2 | 3 | Requires MuJoCo 2.3.3 or later. 4 | 5 | ## Overview 6 | 7 | This package contains a simplified robot description (MJCF) of the [Franka Emika 8 | Panda](https://www.franka.de/) developed by [Franka 9 | Emika](https://www.franka.de/company). It is derived from the [publicly 10 | available URDF 11 | description](https://github.com/frankaemika/franka_ros/tree/develop/franka_description). 12 | 13 |

14 | 15 |

16 | 17 | ## URDF → MJCF derivation steps 18 | 19 | 1. Converted the DAE [mesh 20 | files](https://github.com/frankaemika/franka_ros/tree/develop/franka_description/meshes/visual) 21 | to OBJ format using [Blender](https://www.blender.org/). 22 | 2. Processed `.obj` files with [`obj2mjcf`](https://github.com/kevinzakka/obj2mjcf). 23 | 3. Eliminated the perfectly flat `link0_6` from the resulting submeshes created for `link0`. 24 | 4. Created a convex decomposition of the STL collision [mesh 25 | file](https://github.com/frankaemika/franka_ros/tree/develop/franka_description/meshes/collision) 26 | for `link5` using [V-HACD](https://github.com/kmammou/v-hacd). 27 | 5. Added ` ` to the 28 | [URDF](https://github.com/frankaemika/franka_ros/tree/develop/franka_description/robots)'s 29 | `` clause in order to preserve visual geometries. 30 | 6. Loaded the URDF into MuJoCo and saved a corresponding MJCF. 31 | 7. Matched inertial parameters with [inertial.yaml]( 32 | https://github.com/frankaemika/franka_ros/blob/develop/franka_description/robots/common/inertial.yaml). 33 | 8. Added a tracking light to the base. 34 | 9. Manually edited the MJCF to extract common properties into the `` section. 35 | 10. Added `` clauses to prevent collisions between `link7` and `link8`. 36 | 11. Manually designed collision geoms for the fingertips. 37 | 12. Added position-controlled actuators for the arm. 38 | 13. Added an equality constraint so that the left finger mimics the position of the right finger. 39 | 14. Added a tendon to split the force equally between both fingers and a 40 | position actuator acting on this tendon. 41 | 15. Added `scene.xml` which includes the robot, with a textured groundplane, skybox, and haze. 42 | 43 | ### MJX 44 | 45 | A version of the Franka Emika Panda environment was created for MJX. Steps: 46 | 47 | 1. Added `mjx_panda.xml`, forked from `panda.xml`. 48 | 2. Added `mjx_scene.xml` and `mjx_single_cube.xml`, forked from `scene.xml`. 49 | 3. Gripper collision geometries were modified to contain less geoms. A capsule collision geom was added to the hand. 50 | 4. Solver parameters were tuned for performance. 51 | 5. Actuator `kp` and `kv` were reduced for more stable simulation. 52 | 6. Added a `site` to the gripper. 53 | 7. Removed tendon and added position actuator for the gripper. Changed gripper `ctrlrange`. 54 | 55 | ## License 56 | 57 | This model is released under an [Apache-2.0 License](LICENSE). 58 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/hand.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/hand_0.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v 0.01525500 -0.06846300 0.02643800 4 | v 0.01525500 -0.06846300 0.02643800 5 | v 0.01525500 -0.06846300 0.02643800 6 | v 0.01525500 -0.06962700 0.02613600 7 | v 0.01525600 0.06845600 0.02644100 8 | v 0.01525600 0.06845600 0.02644100 9 | v 0.01525600 0.06845600 0.02644100 10 | v 0.01525600 0.06845600 0.02644100 11 | v 0.01525400 0.06938500 0.01339000 12 | v 0.01525400 0.06938500 0.01339000 13 | v 0.01525400 0.06938500 0.01339000 14 | v 0.01525400 0.07093400 0.01820800 15 | v 0.01525400 0.07093400 0.01820800 16 | v 0.01525500 0.07196200 0.02213600 17 | v 0.01525500 0.07196200 0.02213600 18 | v 0.01525500 0.07196200 0.02213600 19 | v 0.01525500 0.07189200 0.02323400 20 | v 0.01525500 0.07189200 0.02323400 21 | v 0.01525500 0.07189200 0.02323400 22 | v 0.01525500 0.07155200 0.02429100 23 | v 0.01525500 0.07087700 0.02522400 24 | v 0.01525500 0.07087700 0.02522400 25 | v 0.01525500 0.07016000 0.02582700 26 | v 0.01525500 -0.07078900 0.02532300 27 | v 0.01525500 -0.07078900 0.02532300 28 | v 0.01525500 -0.07078900 0.02532300 29 | v 0.01525500 -0.07078900 0.02532300 30 | v 0.01525500 -0.07159000 0.02423400 31 | v 0.01525500 0.06931900 0.02623500 32 | v 0.01525500 0.06931900 0.02623500 33 | v 0.01525500 0.06931900 0.02623500 34 | v 0.01525500 -0.06638600 0.01107000 35 | v 0.01525500 -0.06638600 0.01107000 36 | v 0.01525500 -0.06638600 0.01107000 37 | v 0.01525500 -0.06638600 0.01107000 38 | v 0.01525500 0.06630700 0.01106100 39 | v 0.01525500 0.06630700 0.01106100 40 | v 0.01525500 0.06630700 0.01106100 41 | v 0.01525500 -0.07197300 0.02268900 42 | v 0.01525500 -0.07197300 0.02268900 43 | v 0.01525500 -0.07185600 0.02158600 44 | v 0.01525500 -0.07185600 0.02158600 45 | v 0.01525500 -0.07185600 0.02158600 46 | v 0.01525400 0.06754800 0.01146300 47 | v 0.01525400 0.06754800 0.01146300 48 | v 0.01525500 0.06862300 0.01224500 49 | v 0.01525500 -0.06930700 0.01320600 50 | v 0.01525500 -0.06930700 0.01320600 51 | v 0.01525500 -0.06930700 0.01320600 52 | v 0.01525500 -0.06847200 0.01210900 53 | v 0.01525500 -0.06758700 0.01148800 54 | v 0.01525500 -0.06758700 0.01148800 55 | v 0.01525500 -0.07075300 0.01761800 56 | v 0.01525500 -0.07075300 0.01761800 57 | v 0.01525500 -0.07075300 0.01761800 58 | v -0.01525500 -0.06846300 0.02643800 59 | v -0.01525500 -0.06846300 0.02643800 60 | v -0.01525500 -0.06846300 0.02643800 61 | v -0.01525500 -0.06962700 0.02613600 62 | v -0.01525600 0.06845600 0.02644100 63 | v -0.01525600 0.06845600 0.02644100 64 | v -0.01525600 0.06845600 0.02644100 65 | v -0.01525600 0.06845600 0.02644100 66 | v -0.01525600 0.06845600 0.02644100 67 | v -0.01525400 0.06938500 0.01339000 68 | v -0.01525400 0.06938500 0.01339000 69 | v -0.01525400 0.06938500 0.01339000 70 | v -0.01525400 0.07093400 0.01820800 71 | v -0.01525400 0.07093400 0.01820800 72 | v -0.01525500 0.07196200 0.02213600 73 | v -0.01525500 0.07196200 0.02213600 74 | v -0.01525500 0.07189200 0.02323400 75 | v -0.01525500 0.07189200 0.02323400 76 | v -0.01525500 0.07189200 0.02323400 77 | v -0.01525500 0.07189200 0.02323400 78 | v -0.01525500 0.07155200 0.02429100 79 | v -0.01525500 0.07087700 0.02522400 80 | v -0.01525500 0.07087700 0.02522400 81 | v -0.01525500 0.07016000 0.02582700 82 | v -0.01525500 -0.07078900 0.02532300 83 | v -0.01525500 -0.07078900 0.02532300 84 | v -0.01525500 -0.07078900 0.02532300 85 | v -0.01525500 -0.07078900 0.02532300 86 | v -0.01525500 -0.07159000 0.02423400 87 | v -0.01525500 0.06931900 0.02623500 88 | v -0.01525500 0.06931900 0.02623500 89 | v -0.01525500 -0.06638600 0.01107000 90 | v -0.01525500 -0.06638600 0.01107000 91 | v -0.01525500 -0.06638600 0.01107000 92 | v -0.01525500 -0.06638600 0.01107000 93 | v -0.01525500 0.06630700 0.01106100 94 | v -0.01525500 0.06630700 0.01106100 95 | v -0.01525500 0.06630700 0.01106100 96 | v -0.01525500 -0.07197300 0.02268900 97 | v -0.01525500 -0.07197300 0.02268900 98 | v -0.01525500 -0.07185600 0.02158600 99 | v -0.01525500 -0.07185600 0.02158600 100 | v -0.01525500 -0.07185600 0.02158600 101 | v -0.01525400 0.06754800 0.01146300 102 | v -0.01525400 0.06754800 0.01146300 103 | v -0.01525500 0.06862300 0.01224500 104 | v -0.01525500 -0.06930700 0.01320600 105 | v -0.01525500 -0.06930700 0.01320600 106 | v -0.01525500 -0.06930700 0.01320600 107 | v -0.01525500 -0.06847200 0.01210900 108 | v -0.01525500 -0.06758700 0.01148800 109 | v -0.01525500 -0.06758700 0.01148800 110 | v -0.01525500 -0.07075300 0.01761800 111 | v -0.01525500 -0.07075300 0.01761800 112 | v -0.01525500 -0.07075300 0.01761800 113 | vn 1.00000000 -0.00000000 -0.00000000 114 | vn 1.00000000 0.00010000 0.00000000 115 | vn 1.00000000 -0.00010000 0.00010000 116 | vn 1.00000000 -0.00000000 -0.00000000 117 | vn 1.00000000 0.00020000 0.00010000 118 | vn 1.00000000 -0.00020000 -0.00020000 119 | vn 1.00000000 0.00030000 -0.00010000 120 | vn 1.00000000 -0.00000000 -0.00000000 121 | vn 1.00000000 0.00070000 -0.00060000 122 | vn 1.00000000 -0.00200000 0.00190000 123 | vn 1.00000000 0.00030000 -0.00010000 124 | vn 1.00000000 -0.00020000 -0.00020000 125 | vn 1.00000000 0.00030000 -0.00010000 126 | vn 1.00000000 0.00020000 0.00010000 127 | vn 1.00000000 -0.00000000 -0.00010000 128 | vn 1.00000000 -0.00020000 -0.00020000 129 | vn 1.00000000 -0.00000000 -0.00000000 130 | vn 1.00000000 0.00020000 0.00010000 131 | vn 1.00000000 -0.00000000 -0.00010000 132 | vn 1.00000000 0.00020000 0.00010000 133 | vn 1.00000000 -0.00000000 -0.00000000 134 | vn 1.00000000 0.00020000 0.00010000 135 | vn 1.00000000 -0.00000000 -0.00000000 136 | vn 1.00000000 -0.00010000 0.00010000 137 | vn 1.00000000 0.00010000 0.00000000 138 | vn 1.00000000 -0.00040000 0.00030000 139 | vn 1.00000000 -0.00000000 -0.00000000 140 | vn 1.00000000 -0.00040000 0.00030000 141 | vn 1.00000000 0.00020000 0.00010000 142 | vn 1.00000000 -0.00000000 -0.00010000 143 | vn 1.00000000 -0.00000000 -0.00000000 144 | vn 1.00000000 0.00070000 0.00060000 145 | vn 1.00000000 -0.00000000 -0.00000000 146 | vn 1.00000000 0.00010000 0.00000000 147 | vn 1.00000000 0.00040000 0.00020000 148 | vn 1.00000000 0.00070000 -0.00060000 149 | vn 1.00000000 -0.00000000 -0.00000000 150 | vn 1.00000000 0.00030000 -0.00010000 151 | vn 1.00000000 -0.00040000 0.00030000 152 | vn 1.00000000 0.00010000 0.00000000 153 | vn 1.00000000 -0.00010000 0.00010000 154 | vn 1.00000000 -0.00000000 -0.00000000 155 | vn 1.00000000 0.00010000 0.00000000 156 | vn 1.00000000 0.00070000 -0.00060000 157 | vn 1.00000000 -0.00200000 0.00190000 158 | vn 1.00000000 -0.00200000 0.00190000 159 | vn 1.00000000 0.00040000 0.00020000 160 | vn 1.00000000 0.00070000 0.00060000 161 | vn 1.00000000 -0.00060000 -0.00070000 162 | vn 1.00000000 -0.00060000 -0.00070000 163 | vn 1.00000000 -0.00060000 -0.00070000 164 | vn 1.00000000 0.00070000 0.00060000 165 | vn 1.00000000 -0.00000000 -0.00000000 166 | vn 1.00000000 0.00010000 0.00000000 167 | vn 1.00000000 0.00040000 0.00020000 168 | vn -1.00000000 0.00010000 0.00000000 169 | vn -1.00000000 -0.00010000 0.00010000 170 | vn -1.00000000 -0.00000000 -0.00000000 171 | vn -1.00000000 -0.00000000 -0.00000000 172 | vn -1.00000000 -0.00000000 -0.00000000 173 | vn -1.00000000 0.00030000 0.00020000 174 | vn -1.00000000 -0.00020000 -0.00020000 175 | vn -1.00000000 0.00000000 -0.00010000 176 | vn -1.00000000 0.00030000 -0.00010000 177 | vn -1.00000000 -0.00200000 0.00190000 178 | vn -1.00000000 0.00070000 -0.00060000 179 | vn -1.00000000 0.00030000 -0.00010000 180 | vn -1.00000000 0.00030000 -0.00010000 181 | vn -1.00000000 -0.00020000 -0.00020000 182 | vn -1.00000000 -0.00020000 -0.00020000 183 | vn -1.00000000 0.00000000 -0.00010000 184 | vn -1.00000000 0.00030000 0.00020000 185 | vn -1.00000000 -0.00000000 -0.00000000 186 | vn -1.00000000 0.00020000 0.00010000 187 | vn -1.00000000 0.00000000 -0.00010000 188 | vn -1.00000000 0.00020000 0.00010000 189 | vn -1.00000000 0.00020000 0.00010000 190 | vn -1.00000000 -0.00000000 -0.00000000 191 | vn -1.00000000 -0.00000000 -0.00000000 192 | vn -1.00000000 -0.00010000 0.00010000 193 | vn -1.00000000 -0.00040000 0.00030000 194 | vn -1.00000000 0.00010000 0.00000000 195 | vn -1.00000000 -0.00000000 -0.00000000 196 | vn -1.00000000 -0.00040000 0.00030000 197 | vn -1.00000000 -0.00000000 -0.00000000 198 | vn -1.00000000 0.00030000 0.00020000 199 | vn -1.00000000 0.00070000 0.00060000 200 | vn -1.00000000 0.00010000 0.00000000 201 | vn -1.00000000 0.00040000 0.00020000 202 | vn -1.00000000 -0.00000000 -0.00000000 203 | vn -1.00000000 0.00030000 -0.00010000 204 | vn -1.00000000 -0.00000000 -0.00000000 205 | vn -1.00000000 0.00070000 -0.00060000 206 | vn -1.00000000 0.00010000 0.00000000 207 | vn -1.00000000 -0.00040000 0.00030000 208 | vn -1.00000000 0.00010000 0.00000000 209 | vn -1.00000000 -0.00000000 -0.00000000 210 | vn -1.00000000 -0.00010000 0.00010000 211 | vn -1.00000000 -0.00200000 0.00190000 212 | vn -1.00000000 0.00070000 -0.00060000 213 | vn -1.00000000 -0.00200000 0.00190000 214 | vn -1.00000000 -0.00060000 -0.00070000 215 | vn -1.00000000 0.00070000 0.00060000 216 | vn -1.00000000 0.00040000 0.00020000 217 | vn -1.00000000 -0.00060000 -0.00070000 218 | vn -1.00000000 -0.00060000 -0.00070000 219 | vn -1.00000000 0.00070000 0.00060000 220 | vn -1.00000000 0.00040000 0.00020000 221 | vn -1.00000000 -0.00000000 -0.00000000 222 | vn -1.00000000 0.00010000 0.00000000 223 | f 37//37 8//8 1//1 224 | f 60//60 92//92 58//58 225 | f 29//29 5//5 14//14 226 | f 1//1 4//4 27//27 227 | f 26//26 28//28 39//39 228 | f 40//40 43//43 25//25 229 | f 55//55 47//47 35//35 230 | f 50//50 51//51 49//49 231 | f 33//33 37//37 1//1 232 | f 45//45 46//46 10//10 233 | f 9//9 36//36 44//44 234 | f 15//15 19//19 30//30 235 | f 20//20 22//22 18//18 236 | f 23//23 31//31 21//21 237 | f 3//3 24//24 41//41 238 | f 42//42 53//53 1//1 239 | f 52//52 32//32 48//48 240 | f 11//11 13//13 38//38 241 | f 17//17 21//21 31//31 242 | f 7//7 38//38 13//13 243 | f 54//54 34//34 2//2 244 | f 12//12 16//16 6//6 245 | f 106//106 105//105 102//102 246 | f 104//104 108//108 89//89 247 | f 96//96 94//94 82//82 248 | f 84//84 81//81 95//95 249 | f 59//59 58//58 83//83 250 | f 61//61 86//86 72//72 251 | f 79//79 78//78 85//85 252 | f 76//76 74//74 77//77 253 | f 71//71 63//63 75//75 254 | f 65//65 101//101 99//99 255 | f 100//100 93//93 66//66 256 | f 87//87 107//107 103//103 257 | f 109//109 97//97 58//58 258 | f 80//80 57//57 98//98 259 | f 85//85 78//78 73//73 260 | f 70//70 69//69 62//62 261 | f 67//67 91//91 68//68 262 | f 88//88 110//110 56//56 263 | f 58//58 92//92 90//90 264 | f 68//68 91//91 64//64 265 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link0.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link1.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link2.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link3.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link4.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link5_collision_0.obj: -------------------------------------------------------------------------------- 1 | v 0.029063642 0.095448048 -0.133538792 2 | v 0.049877137 -0.013343874 -0.215408068 3 | v 0.046604998 0.029219000 -0.259000003 4 | v -0.043494001 0.033698998 -0.259000003 5 | v -0.030187000 -0.020500001 -0.186547995 6 | v -0.017736474 -0.052050648 -0.258999242 7 | v 0.012548444 0.082957777 -0.202027869 8 | v 0.050364564 0.017247290 -0.214926371 9 | v 0.020709741 -0.020753554 -0.182012846 10 | v -0.038214683 0.050463360 -0.167851336 11 | v 0.045944492 -0.030214707 -0.258999161 12 | v -0.051997126 -0.017851673 -0.259000003 13 | v 0.026338819 0.085091480 -0.183646959 14 | v 0.020922000 0.050882000 -0.259000003 15 | v 0.045491802 0.013339541 -0.187636452 16 | v 0.050951000 0.020726001 -0.259000003 17 | v 0.023551804 0.063747043 -0.133252288 18 | v -0.013548823 -0.047308172 -0.207602441 19 | v -0.023108632 0.089348183 -0.175470195 20 | v 0.028642003 -0.042568484 -0.211525585 21 | v -0.051912808 0.016682228 -0.233346078 22 | v 0.021637405 0.093787480 -0.156753967 23 | v 0.028892799 0.093108394 -0.147202451 24 | v -0.021765672 0.050517374 -0.259000003 25 | v 0.029191773 0.046614188 -0.259000003 26 | v 0.020095230 0.083558674 -0.197022477 27 | v 0.050294057 0.012853477 -0.210481151 28 | v 0.041400984 -0.012110881 -0.190499104 29 | v 0.050274495 0.021321905 -0.223857806 30 | v 0.050395896 -0.021963709 -0.259000003 31 | v -0.014053484 -0.016454442 -0.177588961 32 | v -0.023077204 0.053522191 -0.146035077 33 | v 0.028830698 0.067305577 -0.135375573 34 | v -0.038313158 -0.038208997 -0.224413296 35 | v 0.045422879 -0.030121078 -0.229083894 36 | v 0.011903144 -0.042590730 -0.199122301 37 | v 0.012319026 -0.051745998 -0.224350901 38 | v -0.052457958 0.016511494 -0.258999590 39 | v -0.046985660 -0.009035359 -0.196875560 40 | v 0.022625311 0.097598446 -0.133939149 41 | v 0.012620136 0.087869933 -0.188952639 42 | v 0.025225283 0.087563533 -0.179602397 43 | v -0.009634502 0.083175007 -0.202322834 44 | v -0.024273001 0.085358001 -0.188354000 45 | v 0.022706000 0.084515996 -0.192412004 46 | v 0.031834746 0.074158814 -0.139114503 47 | v 0.050391338 -0.004898283 -0.210560332 48 | v 0.036850054 -0.028951537 -0.199617216 49 | v 0.028814057 -0.007357717 -0.177964311 50 | v 0.007988000 -0.025363000 -0.182107002 51 | v -0.017462573 -0.033546585 -0.190912689 52 | v -0.022334082 -0.002848132 -0.172981292 53 | v -0.030079996 0.053249859 -0.151432391 54 | v 0.045789921 -0.008346553 -0.194042762 55 | v 0.037322809 -0.011895567 -0.186316195 56 | v -0.034569787 -0.042770809 -0.259000003 57 | v -0.026268487 -0.046990059 -0.224191914 58 | v 0.033556075 -0.043548766 -0.258998990 59 | v 0.049874237 -0.022123435 -0.233471659 60 | v -0.005066666 -0.052041921 -0.219576018 61 | v 0.016204123 -0.046663484 -0.207808223 62 | v -0.051942493 -0.005029111 -0.219727096 63 | v -0.029441908 0.082404274 -0.175896559 64 | v -0.042174398 -0.025291337 -0.202643498 65 | f 12 4 3 66 | f 14 3 4 67 | f 16 12 3 68 | f 23 3 13 69 | f 23 16 3 70 | f 23 1 16 71 | f 24 14 4 72 | f 25 13 3 73 | f 25 3 14 74 | f 26 14 7 75 | f 26 25 14 76 | f 27 8 1 77 | f 29 16 1 78 | f 29 1 8 79 | f 29 8 16 80 | f 30 12 16 81 | f 32 31 17 82 | f 33 1 17 83 | f 38 4 12 84 | f 40 22 19 85 | f 40 23 22 86 | f 40 1 23 87 | f 40 32 17 88 | f 40 17 1 89 | f 41 19 22 90 | f 41 26 7 91 | f 42 23 13 92 | f 42 22 23 93 | f 42 41 22 94 | f 43 7 14 95 | f 43 14 24 96 | f 43 41 7 97 | f 43 19 41 98 | f 44 24 4 99 | f 44 43 24 100 | f 44 19 43 101 | f 45 13 25 102 | f 45 25 26 103 | f 45 42 13 104 | f 45 26 41 105 | f 45 41 42 106 | f 46 27 1 107 | f 46 15 27 108 | f 46 1 33 109 | f 47 27 15 110 | f 47 30 16 111 | f 47 16 8 112 | f 47 8 27 113 | f 48 20 35 114 | f 48 35 2 115 | f 48 36 20 116 | f 48 9 36 117 | f 49 17 9 118 | f 50 9 17 119 | f 50 17 31 120 | f 50 36 9 121 | f 51 31 5 122 | f 51 50 31 123 | f 51 18 36 124 | f 51 36 50 125 | f 52 32 5 126 | f 52 5 31 127 | f 52 31 32 128 | f 53 10 39 129 | f 53 39 5 130 | f 53 5 32 131 | f 53 32 40 132 | f 54 33 28 133 | f 54 46 33 134 | f 54 15 46 135 | f 54 47 15 136 | f 54 2 47 137 | f 54 48 2 138 | f 54 28 48 139 | f 55 33 17 140 | f 55 28 33 141 | f 55 17 49 142 | f 55 48 28 143 | f 55 49 9 144 | f 55 9 48 145 | f 56 34 12 146 | f 56 30 6 147 | f 56 12 30 148 | f 57 56 6 149 | f 57 34 56 150 | f 58 35 20 151 | f 58 11 35 152 | f 58 20 37 153 | f 58 37 6 154 | f 58 30 11 155 | f 58 6 30 156 | f 59 2 35 157 | f 59 47 2 158 | f 59 30 47 159 | f 59 35 11 160 | f 59 11 30 161 | f 60 6 37 162 | f 60 57 6 163 | f 60 18 57 164 | f 61 37 20 165 | f 61 20 36 166 | f 61 60 37 167 | f 61 36 18 168 | f 61 18 60 169 | f 62 38 12 170 | f 62 21 38 171 | f 62 39 10 172 | f 62 10 21 173 | f 63 38 21 174 | f 63 21 10 175 | f 63 19 44 176 | f 63 44 4 177 | f 63 4 38 178 | f 63 10 53 179 | f 63 53 40 180 | f 63 40 19 181 | f 64 5 39 182 | f 64 51 5 183 | f 64 18 51 184 | f 64 57 18 185 | f 64 34 57 186 | f 64 12 34 187 | f 64 62 12 188 | f 64 39 62 189 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link5_collision_1.obj: -------------------------------------------------------------------------------- 1 | v 0.029605924 0.107695894 -0.057770639 2 | v 0.028114332 0.082823920 -0.058553905 3 | v 0.038816102 0.053402838 -0.175505396 4 | v -0.026856016 0.088084521 -0.175136227 5 | v -0.030626847 0.107119812 -0.057687294 6 | v 0.025670733 0.088513946 -0.175229901 7 | v -0.004989269 0.102164150 -0.123603135 8 | v -0.038214683 0.050463360 -0.167851336 9 | v 0.029153411 0.097073936 -0.123627471 10 | v -0.039514421 0.050433927 -0.176900026 11 | v -0.009555079 0.106683172 -0.095345165 12 | v -0.018650687 0.092581494 -0.165880068 13 | v 0.028410194 0.050382871 -0.151882879 14 | v -0.025116868 0.079917612 -0.059207257 15 | v 0.027728461 0.088850155 -0.170468824 16 | v 0.012301352 0.106583136 -0.090584029 17 | v -0.033031356 0.072019963 -0.176122603 18 | v -0.028518812 0.083365915 -0.058497467 19 | v 0.017236843 0.092763911 -0.165977938 20 | v -0.013955552 0.101873439 -0.118898329 21 | v -0.028958102 0.093907525 -0.142630397 22 | v 0.017254189 0.075705868 -0.064336333 23 | v -0.014409871 0.075430847 -0.064290830 24 | v 0.028214046 0.087293870 -0.171111784 25 | v 0.025659250 0.106544369 -0.076459959 26 | v 0.003562050 0.106723586 -0.095327997 27 | v -0.038642138 0.054804181 -0.176806387 28 | v 0.024373464 0.079432104 -0.059087522 29 | v 0.021627842 0.092908946 -0.161300197 30 | v -0.022714762 0.106727072 -0.081193925 31 | v -0.026651997 0.092292062 -0.156493182 32 | v -0.028411485 0.090537503 -0.161463618 33 | v -0.033238728 0.052765384 -0.155973185 34 | v 0.028928091 0.106502887 -0.067219353 35 | v 0.003488032 0.101747359 -0.123570031 36 | v 0.025518961 0.096898989 -0.133035213 37 | v 0.021620985 0.097180492 -0.137775852 38 | v -0.029182131 0.097825064 -0.119030284 39 | v -0.022783418 0.101743257 -0.109443270 40 | v -0.027976784 0.086163124 -0.175228428 41 | v -0.029871457 0.106777512 -0.062281333 42 | f 1 2 3 43 | f 5 2 1 44 | f 9 1 3 45 | f 10 6 3 46 | f 11 5 1 47 | f 12 6 4 48 | f 13 3 2 49 | f 13 10 3 50 | f 13 8 10 51 | f 17 4 6 52 | f 18 10 8 53 | f 18 5 10 54 | f 18 8 14 55 | f 18 2 5 56 | f 19 12 7 57 | f 19 6 12 58 | f 20 11 7 59 | f 20 7 12 60 | f 23 14 8 61 | f 23 13 22 62 | f 24 15 9 63 | f 24 9 3 64 | f 24 3 6 65 | f 24 6 15 66 | f 25 16 1 67 | f 26 11 1 68 | f 26 1 16 69 | f 26 7 11 70 | f 26 16 19 71 | f 27 17 6 72 | f 27 6 10 73 | f 27 10 5 74 | f 27 5 21 75 | f 28 18 14 76 | f 28 2 18 77 | f 28 23 22 78 | f 28 14 23 79 | f 28 22 13 80 | f 28 13 2 81 | f 29 15 6 82 | f 29 6 19 83 | f 30 5 11 84 | f 30 11 20 85 | f 31 12 4 86 | f 31 21 30 87 | f 32 27 21 88 | f 32 17 27 89 | f 32 31 4 90 | f 32 21 31 91 | f 33 23 8 92 | f 33 8 13 93 | f 33 13 23 94 | f 34 25 1 95 | f 34 1 9 96 | f 34 9 25 97 | f 35 26 19 98 | f 35 19 7 99 | f 35 7 26 100 | f 36 9 15 101 | f 36 15 29 102 | f 36 25 9 103 | f 37 29 19 104 | f 37 19 16 105 | f 37 16 25 106 | f 37 36 29 107 | f 37 25 36 108 | f 38 30 21 109 | f 38 21 5 110 | f 39 30 20 111 | f 39 20 12 112 | f 39 31 30 113 | f 39 12 31 114 | f 40 32 4 115 | f 40 4 17 116 | f 40 17 32 117 | f 41 38 5 118 | f 41 5 30 119 | f 41 30 38 120 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link5_collision_2.obj: -------------------------------------------------------------------------------- 1 | v 0.016219155 0.099363113 0.048750773 2 | v 0.046550363 0.015799999 0.011058674 3 | v 0.046166659 0.105324245 -0.019041548 4 | v -0.005436418 0.111465780 -0.067873175 5 | v -0.043746749 0.015799999 0.019475930 6 | v 0.032618475 0.015799999 -0.035021468 7 | v 0.012971462 0.125447286 0.026076443 8 | v 0.045556378 0.100993129 0.023528159 9 | v 0.016534659 0.015799999 0.044952921 10 | v 0.045622136 0.015799999 -0.014505461 11 | v 0.031590266 0.097675904 -0.057701935 12 | v -0.026144502 0.015799999 -0.040104519 13 | v 0.020916727 0.125582876 -0.010159381 14 | v -0.043170314 0.100482302 0.027956662 15 | v 0.046145375 0.108191241 0.006346428 16 | v 0.028387091 0.112218342 0.031375415 17 | v 0.033002187 0.099171431 0.039965413 18 | v 0.016412747 0.015799999 -0.044989475 19 | v 0.036705779 0.015799999 -0.030707024 20 | v -0.022295018 0.125131249 -0.010109805 21 | v 0.029605924 0.107695894 -0.057770639 22 | v -0.013213095 0.112592014 0.039924672 23 | v -0.017325918 0.015799999 0.044664757 24 | v -0.047688079 0.104963029 -0.014858533 25 | v 0.045808408 0.103510400 0.019105049 26 | v 0.032957420 0.121769349 -0.001719666 27 | v 0.011885780 0.108060682 0.044095325 28 | v 0.016109216 0.089384114 0.048719259 29 | v 0.041214473 0.103236411 0.027403040 30 | v 0.041650999 0.015799999 0.023706000 31 | v 0.028114332 0.082823920 -0.058553905 32 | v -0.017699993 0.015799999 -0.044454039 33 | v 0.030954458 0.092818151 -0.058089685 34 | v -0.013656957 0.125189726 -0.019080479 35 | v 0.020627846 0.125801899 0.015541304 36 | v 0.008510412 0.111150243 -0.067818128 37 | v -0.029781181 0.121768239 0.014799138 38 | v -0.038794729 0.015799999 0.028113908 39 | v -0.017496539 0.098143056 0.049083962 40 | v -0.044002704 0.015799999 -0.018961373 41 | v -0.047333769 0.101445201 0.019313718 42 | v -0.030626847 0.107119812 -0.057687294 43 | v 0.046107670 0.108070694 -0.010345774 44 | v 0.028871230 0.120837663 -0.014381368 45 | v 0.011692637 0.121507793 0.031446733 46 | v 0.020178268 0.103656248 0.044326107 47 | v 0.024954524 0.015799999 0.040826797 48 | v 0.037099553 0.099607308 0.036020114 49 | v 0.028145238 0.121197152 0.018754182 50 | v -0.021927781 0.125344027 0.015252414 51 | v 0.020917077 0.111451583 -0.057406886 52 | v 0.012405067 0.125386135 -0.019235931 53 | v 0.024373464 0.079432104 -0.059087522 54 | v -0.033976639 0.107508587 0.031219781 55 | v -0.032505696 0.120513471 -0.010314825 56 | v -0.026112117 0.015799999 0.040139331 57 | v -0.008981987 0.108489258 0.044495871 58 | v -0.005033433 0.070411983 0.049486264 59 | v -0.047363327 0.103674321 0.015035708 60 | v -0.033832737 0.116474922 0.018745562 61 | v -0.047827588 0.074670758 0.002492660 62 | v -0.028518812 0.083365915 -0.058497467 63 | v -0.022142435 0.111037086 -0.057352830 64 | v 0.041463828 0.111761132 -0.014586542 65 | f 6 2 5 66 | f 9 5 2 67 | f 10 3 2 68 | f 10 2 6 69 | f 11 3 10 70 | f 12 6 5 71 | f 15 2 3 72 | f 18 6 12 73 | f 19 10 6 74 | f 21 3 11 75 | f 23 5 9 76 | f 25 8 2 77 | f 25 2 15 78 | f 28 17 1 79 | f 29 8 25 80 | f 30 9 2 81 | f 30 2 8 82 | f 31 6 18 83 | f 31 19 6 84 | f 32 18 12 85 | f 32 4 18 86 | f 33 11 10 87 | f 33 10 19 88 | f 33 19 31 89 | f 35 13 34 90 | f 35 26 13 91 | f 36 21 11 92 | f 36 11 33 93 | f 36 33 31 94 | f 36 18 4 95 | f 38 14 5 96 | f 38 5 23 97 | f 40 12 5 98 | f 41 5 14 99 | f 42 40 24 100 | f 43 26 15 101 | f 43 15 3 102 | f 44 26 21 103 | f 44 21 13 104 | f 44 13 26 105 | f 45 16 7 106 | f 45 7 22 107 | f 46 17 16 108 | f 46 45 27 109 | f 46 16 45 110 | f 46 27 1 111 | f 46 1 17 112 | f 47 28 9 113 | f 47 17 28 114 | f 47 9 30 115 | f 48 8 29 116 | f 48 30 8 117 | f 48 47 30 118 | f 48 17 47 119 | f 48 29 16 120 | f 48 16 17 121 | f 49 29 25 122 | f 49 16 29 123 | f 49 25 15 124 | f 49 15 26 125 | f 49 26 35 126 | f 49 35 7 127 | f 49 7 16 128 | f 50 7 35 129 | f 50 37 22 130 | f 50 22 7 131 | f 50 35 34 132 | f 50 34 20 133 | f 51 13 21 134 | f 51 21 36 135 | f 52 34 13 136 | f 52 51 36 137 | f 52 13 51 138 | f 52 36 4 139 | f 52 4 34 140 | f 53 36 31 141 | f 53 31 18 142 | f 53 18 36 143 | f 54 22 37 144 | f 54 39 22 145 | f 54 14 39 146 | f 55 42 24 147 | f 55 50 20 148 | f 55 37 50 149 | f 56 38 23 150 | f 56 23 39 151 | f 56 39 14 152 | f 56 14 38 153 | f 57 22 39 154 | f 57 45 22 155 | f 57 27 45 156 | f 57 39 1 157 | f 57 1 27 158 | f 58 28 1 159 | f 58 1 39 160 | f 58 9 28 161 | f 58 39 23 162 | f 58 23 9 163 | f 59 55 24 164 | f 59 37 55 165 | f 60 41 14 166 | f 60 59 41 167 | f 60 37 59 168 | f 60 54 37 169 | f 60 14 54 170 | f 61 40 5 171 | f 61 5 41 172 | f 61 24 40 173 | f 61 59 24 174 | f 61 41 59 175 | f 62 12 40 176 | f 62 40 42 177 | f 62 32 12 178 | f 62 42 4 179 | f 62 4 32 180 | f 63 34 4 181 | f 63 4 42 182 | f 63 20 34 183 | f 63 55 20 184 | f 63 42 55 185 | f 64 43 3 186 | f 64 3 21 187 | f 64 21 26 188 | f 64 26 43 189 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link6.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link6_12.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v 0.08738500 0.07734600 -0.02619700 4 | v 0.08738500 0.07734600 -0.02619700 5 | v 0.08738500 0.07734600 -0.02619700 6 | v 0.08815600 0.07722600 -0.02612400 7 | v 0.08815600 0.07722600 -0.02612400 8 | v 0.08815600 0.07722600 -0.02612400 9 | v 0.08815600 0.07722600 -0.02612400 10 | v 0.08771600 0.07718900 -0.02655000 11 | v 0.08941600 0.07743300 -0.02422300 12 | v 0.08941600 0.07743300 -0.02422300 13 | v 0.08941600 0.07743300 -0.02422300 14 | v 0.08974500 0.07729600 -0.02457100 15 | v 0.08916900 0.07728200 -0.02513600 16 | v 0.08916900 0.07728200 -0.02513600 17 | v 0.08916900 0.07728200 -0.02513600 18 | v 0.08916900 0.07728200 -0.02513600 19 | v 0.08739100 0.07752100 -0.02551400 20 | v 0.08739100 0.07752100 -0.02551400 21 | v 0.08739100 0.07752100 -0.02551400 22 | v 0.08705300 0.07749700 -0.02584300 23 | v 0.08773300 0.07754000 -0.02518000 24 | v 0.08773300 0.07754000 -0.02518000 25 | v 0.08773300 0.07754000 -0.02518000 26 | v 0.08860200 0.07725500 -0.02568900 27 | v 0.08860200 0.07725500 -0.02568900 28 | v 0.08860200 0.07725500 -0.02568900 29 | v 0.08807900 0.07755400 -0.02484200 30 | v 0.08807900 0.07755400 -0.02484200 31 | v 0.08807900 0.07755400 -0.02484200 32 | v 0.08842800 0.07756300 -0.02450000 33 | v 0.08842800 0.07756300 -0.02450000 34 | v 0.08842800 0.07756300 -0.02450000 35 | v 0.08842800 0.07756300 -0.02450000 36 | v 0.08842800 0.07756300 -0.02450000 37 | v 0.08908300 0.07756600 -0.02387100 38 | v 0.08908300 0.07756600 -0.02387100 39 | v 0.08874400 0.07769400 -0.02351600 40 | v 0.08874400 0.07769400 -0.02351600 41 | v 0.08874400 0.07769400 -0.02351600 42 | v 0.08874400 0.07769400 -0.02351600 43 | v 0.08809300 0.07769700 -0.02414700 44 | v 0.08809300 0.07769700 -0.02414700 45 | v 0.08809300 0.07769700 -0.02414700 46 | v 0.08807600 0.07782400 -0.02347800 47 | v 0.08807600 0.07782400 -0.02347800 48 | v 0.08807600 0.07782400 -0.02347800 49 | v 0.08840200 0.07781800 -0.02315600 50 | v 0.08775700 0.07782500 -0.02379300 51 | vn 0.17290000 0.94970000 -0.26090000 52 | vn 0.17250000 0.95380000 -0.24590000 53 | vn 0.17210000 0.95390000 -0.24590000 54 | vn 0.17290000 0.94970000 -0.26090000 55 | vn 0.17910000 0.95470000 -0.23760000 56 | vn 0.17160000 0.95520000 -0.24110000 57 | vn 0.17210000 0.95390000 -0.24590000 58 | vn 0.17290000 0.94970000 -0.26090000 59 | vn 0.18140000 0.96450000 -0.19210000 60 | vn 0.18570000 0.96000000 -0.20940000 61 | vn 0.18050000 0.96130000 -0.20820000 62 | vn 0.18050000 0.96130000 -0.20820000 63 | vn 0.17810000 0.95730000 -0.22760000 64 | vn 0.18050000 0.96130000 -0.20820000 65 | vn 0.18570000 0.96000000 -0.20940000 66 | vn 0.18270000 0.95990000 -0.21270000 67 | vn 0.17250000 0.95380000 -0.24590000 68 | vn 0.17210000 0.95390000 -0.24590000 69 | vn 0.17910000 0.95470000 -0.23760000 70 | vn 0.17250000 0.95380000 -0.24590000 71 | vn 0.17910000 0.95470000 -0.23760000 72 | vn 0.18130000 0.95720000 -0.22550000 73 | vn 0.17160000 0.95520000 -0.24110000 74 | vn 0.17810000 0.95730000 -0.22760000 75 | vn 0.18130000 0.95720000 -0.22550000 76 | vn 0.17160000 0.95520000 -0.24110000 77 | vn 0.18270000 0.95990000 -0.21270000 78 | vn 0.18130000 0.95720000 -0.22550000 79 | vn 0.17810000 0.95730000 -0.22760000 80 | vn 0.18140000 0.96450000 -0.19210000 81 | vn 0.18570000 0.96000000 -0.20940000 82 | vn 0.18270000 0.95990000 -0.21270000 83 | vn 0.17380000 0.96740000 -0.18420000 84 | vn 0.18650000 0.96430000 -0.18780000 85 | vn 0.17380000 0.96740000 -0.18420000 86 | vn 0.18140000 0.96450000 -0.19210000 87 | vn 0.17770000 0.96770000 -0.17880000 88 | vn 0.17900000 0.97000000 -0.16440000 89 | vn 0.18650000 0.96430000 -0.18780000 90 | vn 0.17380000 0.96740000 -0.18420000 91 | vn 0.17770000 0.96770000 -0.17880000 92 | vn 0.18020000 0.96730000 -0.17860000 93 | vn 0.18650000 0.96430000 -0.18780000 94 | vn 0.17900000 0.97000000 -0.16440000 95 | vn 0.18020000 0.96730000 -0.17860000 96 | vn 0.17770000 0.96770000 -0.17880000 97 | vn 0.17900000 0.97000000 -0.16440000 98 | vn 0.18020000 0.96730000 -0.17860000 99 | f 1//1 4//4 8//8 100 | f 11//11 12//12 14//14 101 | f 17//17 2//2 20//20 102 | f 18//18 7//7 3//3 103 | f 23//23 26//26 6//6 104 | f 21//21 5//5 19//19 105 | f 29//29 13//13 24//24 106 | f 28//28 25//25 22//22 107 | f 30//30 36//36 9//9 108 | f 31//31 10//10 15//15 109 | f 32//32 16//16 27//27 110 | f 40//40 35//35 33//33 111 | f 43//43 39//39 34//34 112 | f 44//44 47//47 38//38 113 | f 46//46 37//37 41//41 114 | f 48//48 45//45 42//42 115 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link6_13.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v 0.08937000 0.07712300 0.02554700 4 | v 0.08937000 0.07712300 0.02554700 5 | v 0.08937000 0.07712300 0.02554700 6 | v 0.08885500 0.07735300 0.02500000 7 | v 0.08885500 0.07735300 0.02500000 8 | v 0.08885500 0.07735300 0.02500000 9 | v 0.08722100 0.07724700 0.02659100 10 | v 0.08813200 0.07732100 0.02570800 11 | v 0.08813200 0.07732100 0.02570800 12 | v 0.08813200 0.07732100 0.02570800 13 | v 0.08813200 0.07732100 0.02570800 14 | v 0.08813200 0.07732100 0.02570800 15 | v 0.08813200 0.07732100 0.02570800 16 | v 0.08746500 0.07761000 0.02500000 17 | v 0.08746500 0.07761000 0.02500000 18 | v 0.08746500 0.07761000 0.02500000 19 | v 0.08746500 0.07761000 0.02500000 20 | v 0.08746500 0.07761000 0.02500000 21 | v 0.08696300 0.07758100 0.02548900 22 | v 0.08696300 0.07758100 0.02548900 23 | v 0.08818700 0.07763200 0.02429300 24 | v 0.08818700 0.07763200 0.02429300 25 | v 0.08818700 0.07763200 0.02429300 26 | v 0.08818700 0.07763200 0.02429300 27 | v 0.08818700 0.07763200 0.02429300 28 | v 0.08818700 0.07763200 0.02429300 29 | v 0.08734500 0.07794800 0.02340900 30 | v 0.08968600 0.07697400 0.02588400 31 | v 0.08968600 0.07697400 0.02588400 32 | v 0.08704800 0.07777700 0.02456100 33 | v 0.08704800 0.07777700 0.02456100 34 | v 0.08662600 0.07794100 0.02411600 35 | v 0.08662600 0.07794100 0.02411600 36 | v 0.08857000 0.07711300 0.02617400 37 | v 0.08857000 0.07711300 0.02617400 38 | v 0.08857000 0.07711300 0.02617400 39 | v 0.08895900 0.07692100 0.02659100 40 | v 0.08926000 0.07736400 0.02460300 41 | v 0.08926000 0.07736400 0.02460300 42 | v 0.08926000 0.07736400 0.02460300 43 | v 0.08975600 0.07736900 0.02411600 44 | v 0.08975600 0.07736900 0.02411600 45 | v 0.08975600 0.07736900 0.02411600 46 | v 0.08864800 0.07763300 0.02383900 47 | v 0.08864800 0.07763300 0.02383900 48 | v 0.08908400 0.07763100 0.02340900 49 | v 0.08655700 0.07755200 0.02588400 50 | v 0.08655700 0.07755200 0.02588400 51 | vn 0.18430000 0.95040000 0.25040000 52 | vn 0.18200000 0.95620000 0.22910000 53 | vn 0.18380000 0.95070000 0.24960000 54 | vn 0.17770000 0.96180000 0.20800000 55 | vn 0.18200000 0.95620000 0.22910000 56 | vn 0.17710000 0.95830000 0.22420000 57 | vn 0.16800000 0.95290000 0.25240000 58 | vn 0.17060000 0.95810000 0.23030000 59 | vn 0.16740000 0.95520000 0.24390000 60 | vn 0.18380000 0.95070000 0.24960000 61 | vn 0.17710000 0.95830000 0.22420000 62 | vn 0.18200000 0.95620000 0.22910000 63 | vn 0.16800000 0.95290000 0.25240000 64 | vn 0.17710000 0.95830000 0.22420000 65 | vn 0.17770000 0.96180000 0.20800000 66 | vn 0.17180000 0.96350000 0.20550000 67 | vn 0.17850000 0.96080000 0.21230000 68 | vn 0.17060000 0.95810000 0.23030000 69 | vn 0.17060000 0.95810000 0.23030000 70 | vn 0.16740000 0.95520000 0.24390000 71 | vn 0.17180000 0.96350000 0.20550000 72 | vn 0.18280000 0.96480000 0.18910000 73 | vn 0.17850000 0.96080000 0.21230000 74 | vn 0.18340000 0.96300000 0.19740000 75 | vn 0.17100000 0.96790000 0.18410000 76 | vn 0.16950000 0.96620000 0.19440000 77 | vn 0.17100000 0.96790000 0.18410000 78 | vn 0.18690000 0.94670000 0.26230000 79 | vn 0.18430000 0.95040000 0.25040000 80 | vn 0.17180000 0.96350000 0.20550000 81 | vn 0.16950000 0.96620000 0.19440000 82 | vn 0.17100000 0.96790000 0.18410000 83 | vn 0.16950000 0.96620000 0.19440000 84 | vn 0.18430000 0.95040000 0.25040000 85 | vn 0.18380000 0.95070000 0.24960000 86 | vn 0.18690000 0.94670000 0.26230000 87 | vn 0.18690000 0.94670000 0.26230000 88 | vn 0.17770000 0.96180000 0.20800000 89 | vn 0.18340000 0.96300000 0.19740000 90 | vn 0.17850000 0.96080000 0.21230000 91 | vn 0.18340000 0.96300000 0.19740000 92 | vn 0.18460000 0.96570000 0.18270000 93 | vn 0.18280000 0.96480000 0.18910000 94 | vn 0.18460000 0.96570000 0.18270000 95 | vn 0.18280000 0.96480000 0.18910000 96 | vn 0.18460000 0.96570000 0.18270000 97 | vn 0.16800000 0.95290000 0.25240000 98 | vn 0.16740000 0.95520000 0.24390000 99 | f 8//8 18//18 19//19 100 | f 25//25 27//27 32//32 101 | f 5//5 12//12 2//2 102 | f 6//6 14//14 11//11 103 | f 3//3 10//10 35//35 104 | f 38//38 15//15 4//4 105 | f 40//40 23//23 17//17 106 | f 41//41 24//24 39//39 107 | f 45//45 22//22 43//43 108 | f 21//21 30//30 16//16 109 | f 28//28 36//36 37//37 110 | f 46//46 44//44 42//42 111 | f 13//13 47//47 7//7 112 | f 33//33 31//31 26//26 113 | f 1//1 34//34 29//29 114 | f 20//20 48//48 9//9 115 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link6_7.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v 0.06278800 0.08158900 -0.00050000 4 | v 0.06278800 0.08158900 -0.00050000 5 | v 0.06278800 0.08158900 -0.00050000 6 | v 0.06278800 0.08158900 -0.00050000 7 | v 0.06196700 0.08154300 -0.00050000 8 | v 0.06196800 0.08154400 -0.00000000 9 | v 0.06196800 0.08154400 -0.00000000 10 | v 0.06196800 0.08154400 -0.00000000 11 | v 0.06196800 0.08154400 -0.00000000 12 | v 0.06382300 0.08162700 -0.00073800 13 | v 0.06382300 0.08162700 -0.00073800 14 | v 0.06382300 0.08162700 -0.00073800 15 | v 0.06382300 0.08162700 -0.00073800 16 | v 0.06365600 0.08161700 -0.00135000 17 | v 0.06365600 0.08161700 -0.00135000 18 | v 0.06322500 0.08160600 -0.00092700 19 | v 0.06322500 0.08160600 -0.00092700 20 | v 0.06322500 0.08160600 -0.00092700 21 | v 0.06319700 0.08160800 -0.00012500 22 | v 0.06319700 0.08160800 -0.00012500 23 | v 0.06319700 0.08160800 -0.00012500 24 | v 0.06319700 0.08160800 -0.00000000 25 | v 0.06319700 0.08160800 -0.00000000 26 | v 0.06319700 0.08160800 -0.00000000 27 | v 0.06319700 0.08160800 -0.00000000 28 | v 0.06422200 0.08163600 -0.00079600 29 | v 0.06422200 0.08163600 -0.00079600 30 | v 0.06422200 0.08163600 -0.00079600 31 | v 0.06432000 0.08164000 -0.00025000 32 | v 0.06432000 0.08164000 -0.00025000 33 | v 0.06477800 0.08164500 -0.00025000 34 | v 0.06278800 0.08158900 0.00050000 35 | v 0.06278800 0.08158900 0.00050000 36 | v 0.06278800 0.08158900 0.00050000 37 | v 0.06278800 0.08158900 0.00050000 38 | v 0.06196700 0.08154300 0.00050000 39 | v 0.06319700 0.08160800 0.00012500 40 | v 0.06319700 0.08160800 0.00012500 41 | v 0.06319700 0.08160800 0.00012500 42 | v 0.06322500 0.08160600 0.00092700 43 | v 0.06322500 0.08160600 0.00092700 44 | v 0.06322500 0.08160600 0.00092700 45 | v 0.06382300 0.08162700 0.00073800 46 | v 0.06382300 0.08162700 0.00073800 47 | v 0.06382300 0.08162700 0.00073800 48 | v 0.06382300 0.08162700 0.00073800 49 | v 0.06365600 0.08161700 0.00135000 50 | v 0.06365600 0.08161700 0.00135000 51 | v 0.06422200 0.08163600 0.00079500 52 | v 0.06422200 0.08163600 0.00079500 53 | v 0.06422200 0.08163600 0.00079500 54 | v 0.06432000 0.08164000 0.00025000 55 | v 0.06432000 0.08164000 0.00025000 56 | v 0.06477800 0.08164500 0.00025000 57 | vn -0.05590000 0.99840000 -0.00200000 58 | vn -0.04540000 0.99900000 -0.00040000 59 | vn -0.05180000 0.99860000 0.00480000 60 | vn -0.04210000 0.99910000 -0.00400000 61 | vn -0.05590000 0.99840000 -0.00200000 62 | vn -0.05180000 0.99860000 -0.00480000 63 | vn -0.05180000 0.99860000 0.00480000 64 | vn -0.05590000 0.99840000 0.00200000 65 | vn -0.05590000 0.99840000 -0.00200000 66 | vn -0.02370000 0.99970000 -0.00890000 67 | vn -0.03390000 0.99940000 -0.00370000 68 | vn -0.02280000 0.99970000 -0.00280000 69 | vn -0.03300000 0.99940000 -0.00640000 70 | vn -0.03300000 0.99940000 -0.00640000 71 | vn -0.02370000 0.99970000 -0.00890000 72 | vn -0.03390000 0.99940000 -0.00370000 73 | vn -0.03300000 0.99940000 -0.00640000 74 | vn -0.04210000 0.99910000 -0.00400000 75 | vn -0.04210000 0.99910000 -0.00400000 76 | vn -0.04540000 0.99900000 -0.00040000 77 | vn -0.03390000 0.99940000 -0.00370000 78 | vn -0.05180000 0.99860000 -0.00480000 79 | vn -0.04540000 0.99900000 0.00040000 80 | vn -0.04540000 0.99900000 -0.00040000 81 | vn -0.05180000 0.99860000 0.00480000 82 | vn -0.02370000 0.99970000 -0.00890000 83 | vn -0.02280000 0.99970000 -0.00280000 84 | vn -0.01250000 0.99990000 -0.00460000 85 | vn -0.02280000 0.99970000 -0.00280000 86 | vn -0.01250000 0.99990000 -0.00460000 87 | vn -0.01250000 0.99990000 -0.00460000 88 | vn -0.05590000 0.99840000 0.00200000 89 | vn -0.05180000 0.99860000 -0.00480000 90 | vn -0.04540000 0.99900000 0.00040000 91 | vn -0.04210000 0.99910000 0.00400000 92 | vn -0.05590000 0.99840000 0.00200000 93 | vn -0.04540000 0.99900000 0.00040000 94 | vn -0.04210000 0.99910000 0.00400000 95 | vn -0.03390000 0.99940000 0.00370000 96 | vn -0.03390000 0.99940000 0.00370000 97 | vn -0.03300000 0.99940000 0.00640000 98 | vn -0.04210000 0.99910000 0.00400000 99 | vn -0.03390000 0.99940000 0.00370000 100 | vn -0.03300000 0.99940000 0.00640000 101 | vn -0.02280000 0.99970000 0.00280000 102 | vn -0.02360000 0.99970000 0.00890000 103 | vn -0.03300000 0.99940000 0.00640000 104 | vn -0.02360000 0.99970000 0.00890000 105 | vn -0.02280000 0.99970000 0.00280000 106 | vn -0.02360000 0.99970000 0.00890000 107 | vn -0.01250000 0.99990000 0.00460000 108 | vn -0.01250000 0.99990000 0.00460000 109 | vn -0.02280000 0.99970000 0.00280000 110 | vn -0.01250000 0.99990000 0.00460000 111 | f 1//1 5//5 9//9 112 | f 13//13 14//14 17//17 113 | f 19//19 18//18 4//4 114 | f 21//21 11//11 16//16 115 | f 25//25 3//3 7//7 116 | f 24//24 20//20 2//2 117 | f 26//26 15//15 10//10 118 | f 29//29 27//27 12//12 119 | f 31//31 28//28 30//30 120 | f 33//33 22//22 6//6 121 | f 32//32 8//8 36//36 122 | f 37//37 23//23 34//34 123 | f 42//42 38//38 35//35 124 | f 43//43 39//39 40//40 125 | f 47//47 44//44 41//41 126 | f 49//49 53//53 45//45 127 | f 50//50 46//46 48//48 128 | f 54//54 52//52 51//51 129 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link6_8.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v 0.11308100 0.07180500 -0.00075400 4 | v 0.11308100 0.07180500 -0.00075400 5 | v 0.11373800 0.07140400 -0.00040000 6 | v 0.11373800 0.07140400 -0.00040000 7 | v 0.11373800 0.07140400 -0.00040000 8 | v 0.11373800 0.07140400 -0.00040000 9 | v 0.11331900 0.07165700 -0.00112600 10 | v 0.11331900 0.07165700 -0.00112600 11 | v 0.11331900 0.07165700 -0.00112600 12 | v 0.11388500 0.07131000 -0.00060800 13 | v 0.11388500 0.07131000 -0.00060800 14 | v 0.11388500 0.07131000 -0.00060800 15 | v 0.11519800 0.07044600 0.00008600 16 | v 0.11519800 0.07044600 0.00008600 17 | v 0.11519800 0.07044600 0.00008600 18 | v 0.11519800 0.07044600 0.00008600 19 | v 0.11591500 0.06994200 -0.00024100 20 | v 0.11591500 0.06994200 -0.00024100 21 | v 0.11591500 0.06994200 -0.00024100 22 | v 0.11518000 0.07045900 -0.00020300 23 | v 0.11518000 0.07045900 -0.00020300 24 | v 0.11518000 0.07045900 -0.00020300 25 | v 0.11575300 0.07005400 -0.00077100 26 | v 0.11575300 0.07005400 -0.00077100 27 | v 0.11575300 0.07005400 -0.00077100 28 | v 0.11506800 0.07053400 -0.00048200 29 | v 0.11506800 0.07053400 -0.00048200 30 | v 0.11506800 0.07053400 -0.00048200 31 | v 0.11549700 0.07023200 -0.00115900 32 | v 0.11549700 0.07023200 -0.00115900 33 | v 0.11549700 0.07023200 -0.00115900 34 | v 0.11485400 0.07067800 -0.00071400 35 | v 0.11485400 0.07067800 -0.00071400 36 | v 0.11485400 0.07067800 -0.00071400 37 | v 0.11485400 0.07067800 -0.00071400 38 | v 0.11522800 0.07041600 -0.00139400 39 | v 0.11522800 0.07041600 -0.00139400 40 | v 0.11482900 0.07068500 -0.00160100 41 | v 0.11482900 0.07068500 -0.00160100 42 | v 0.11482900 0.07068500 -0.00160100 43 | v 0.11456100 0.07087200 -0.00084200 44 | v 0.11456100 0.07087200 -0.00084200 45 | v 0.11456100 0.07087200 -0.00084200 46 | v 0.11428000 0.07104700 -0.00164700 47 | v 0.11428000 0.07104700 -0.00164700 48 | v 0.11428000 0.07104700 -0.00164700 49 | v 0.11428000 0.07104700 -0.00164700 50 | v 0.11431700 0.07103200 -0.00084100 51 | v 0.11431700 0.07103200 -0.00084100 52 | v 0.11407900 0.07118600 -0.00076500 53 | v 0.11407900 0.07118600 -0.00076500 54 | v 0.11407900 0.07118600 -0.00076500 55 | v 0.11292200 0.07190200 -0.00022300 56 | v 0.11292200 0.07190200 -0.00022300 57 | v 0.11292200 0.07190200 -0.00022300 58 | v 0.11365200 0.07145800 -0.00008800 59 | v 0.11365200 0.07145800 -0.00008800 60 | v 0.11365200 0.07145800 -0.00008800 61 | v 0.11375000 0.07138600 -0.00148800 62 | v 0.11375000 0.07138600 -0.00148800 63 | v 0.11375000 0.07138600 -0.00148800 64 | v 0.11414600 0.07114300 0.00079700 65 | v 0.11414600 0.07114300 0.00079700 66 | v 0.11414600 0.07114300 0.00079700 67 | v 0.11352000 0.07153200 0.00132400 68 | v 0.11352000 0.07153200 0.00132400 69 | v 0.11352000 0.07153200 0.00132400 70 | v 0.11400800 0.07122200 0.00159700 71 | v 0.11400800 0.07122200 0.00159700 72 | v 0.11400800 0.07122200 0.00159700 73 | v 0.11382600 0.07134700 0.00055400 74 | v 0.11382600 0.07134700 0.00055400 75 | v 0.11382600 0.07134700 0.00055400 76 | v 0.11382600 0.07134700 0.00055400 77 | v 0.11328300 0.07168000 0.00107600 78 | v 0.11328300 0.07168000 0.00107600 79 | v 0.11309400 0.07179600 0.00078300 80 | v 0.11309400 0.07179600 0.00078300 81 | v 0.11309400 0.07179600 0.00078300 82 | v 0.11367100 0.07144600 0.00020300 83 | v 0.11367100 0.07144600 0.00020300 84 | v 0.11367100 0.07144600 0.00020300 85 | v 0.11294600 0.07188700 0.00034000 86 | v 0.11294600 0.07188700 0.00034000 87 | v 0.11294600 0.07188700 0.00034000 88 | v 0.11491300 0.07063900 0.00066700 89 | v 0.11491300 0.07063900 0.00066700 90 | v 0.11491300 0.07063900 0.00066700 91 | v 0.11491300 0.07063900 0.00066700 92 | v 0.11531900 0.07035500 0.00133000 93 | v 0.11531900 0.07035500 0.00133000 94 | v 0.11561600 0.07015000 0.00100700 95 | v 0.11561600 0.07015000 0.00100700 96 | v 0.11561600 0.07015000 0.00100700 97 | v 0.11511500 0.07050300 0.00039600 98 | v 0.11511500 0.07050300 0.00039600 99 | v 0.11511500 0.07050300 0.00039600 100 | v 0.11496000 0.07059700 0.00154500 101 | v 0.11496000 0.07059700 0.00154500 102 | v 0.11496000 0.07059700 0.00154500 103 | v 0.11581700 0.07000900 0.00061700 104 | v 0.11581700 0.07000900 0.00061700 105 | v 0.11581700 0.07000900 0.00061700 106 | v 0.11462500 0.07083000 0.00082600 107 | v 0.11462500 0.07083000 0.00082600 108 | v 0.11462500 0.07083000 0.00082600 109 | v 0.11455800 0.07086500 0.00164800 110 | v 0.11455800 0.07086500 0.00164800 111 | v 0.11455800 0.07086500 0.00164800 112 | v 0.11590900 0.06994600 0.00023300 113 | v 0.11590900 0.06994600 0.00023300 114 | v 0.11438400 0.07098900 0.00084900 115 | v 0.11438400 0.07098900 0.00084900 116 | v 0.11438400 0.07098900 0.00084900 117 | vn 0.52110000 0.85350000 -0.00020000 118 | vn 0.52280000 0.85240000 -0.00470000 119 | vn 0.52020000 0.85400000 -0.00590000 120 | vn 0.52110000 0.85350000 -0.00020000 121 | vn 0.52280000 0.85240000 -0.00470000 122 | vn 0.52720000 0.84970000 -0.00820000 123 | vn 0.52720000 0.84970000 -0.00820000 124 | vn 0.52690000 0.84990000 -0.00770000 125 | vn 0.52280000 0.85240000 -0.00470000 126 | vn 0.52720000 0.84970000 -0.00820000 127 | vn 0.53400000 0.84550000 -0.00920000 128 | vn 0.52690000 0.84990000 -0.00770000 129 | vn 0.57590000 0.81750000 0.00060000 130 | vn 0.57590000 0.81760000 0.00110000 131 | vn 0.57330000 0.81940000 0.00560000 132 | vn 0.57530000 0.81800000 -0.00150000 133 | vn 0.57530000 0.81800000 -0.00190000 134 | vn 0.57530000 0.81800000 -0.00150000 135 | vn 0.57590000 0.81750000 0.00060000 136 | vn 0.57180000 0.82040000 -0.00710000 137 | vn 0.57530000 0.81800000 -0.00190000 138 | vn 0.57530000 0.81800000 -0.00150000 139 | vn 0.57320000 0.81940000 -0.00210000 140 | vn 0.57530000 0.81800000 -0.00190000 141 | vn 0.57180000 0.82040000 -0.00710000 142 | vn 0.57320000 0.81940000 -0.00210000 143 | vn 0.56510000 0.82500000 -0.00980000 144 | vn 0.57180000 0.82040000 -0.00710000 145 | vn 0.56750000 0.82330000 -0.00460000 146 | vn 0.56510000 0.82500000 -0.00980000 147 | vn 0.57320000 0.81940000 -0.00210000 148 | vn 0.56200000 0.82710000 -0.00910000 149 | vn 0.55500000 0.83180000 -0.00880000 150 | vn 0.56750000 0.82330000 -0.00460000 151 | vn 0.56510000 0.82500000 -0.00980000 152 | vn 0.56750000 0.82330000 -0.00460000 153 | vn 0.56200000 0.82710000 -0.00910000 154 | vn 0.56200000 0.82710000 -0.00910000 155 | vn 0.55500000 0.83180000 -0.00880000 156 | vn 0.55070000 0.83460000 -0.01100000 157 | vn 0.54830000 0.83620000 -0.00990000 158 | vn 0.55500000 0.83180000 -0.00880000 159 | vn 0.55070000 0.83460000 -0.01100000 160 | vn 0.53690000 0.84360000 -0.01100000 161 | vn 0.54140000 0.84070000 -0.00950000 162 | vn 0.55070000 0.83460000 -0.01100000 163 | vn 0.54830000 0.83620000 -0.00990000 164 | vn 0.54830000 0.83620000 -0.00990000 165 | vn 0.54140000 0.84070000 -0.00950000 166 | vn 0.54140000 0.84070000 -0.00950000 167 | vn 0.53690000 0.84360000 -0.01100000 168 | vn 0.53400000 0.84550000 -0.00920000 169 | vn 0.52020000 0.85400000 -0.00590000 170 | vn 0.52110000 0.85350000 -0.00020000 171 | vn 0.51930000 0.85460000 0.00050000 172 | vn 0.51930000 0.85460000 0.00050000 173 | vn 0.51970000 0.85440000 0.00120000 174 | vn 0.52020000 0.85400000 -0.00590000 175 | vn 0.53400000 0.84550000 -0.00920000 176 | vn 0.52690000 0.84990000 -0.00770000 177 | vn 0.53690000 0.84360000 -0.01100000 178 | vn 0.53360000 0.84570000 0.00970000 179 | vn 0.54330000 0.83950000 0.01080000 180 | vn 0.53280000 0.84620000 0.00830000 181 | vn 0.53360000 0.84570000 0.00970000 182 | vn 0.52510000 0.85100000 0.00490000 183 | vn 0.53280000 0.84620000 0.00830000 184 | vn 0.54380000 0.83910000 0.01110000 185 | vn 0.53280000 0.84620000 0.00830000 186 | vn 0.54330000 0.83950000 0.01080000 187 | vn 0.52510000 0.85100000 0.00490000 188 | vn 0.52310000 0.85230000 0.00200000 189 | vn 0.53360000 0.84570000 0.00970000 190 | vn 0.52430000 0.85150000 0.00740000 191 | vn 0.52310000 0.85230000 0.00200000 192 | vn 0.52510000 0.85100000 0.00490000 193 | vn 0.51960000 0.85440000 0.00100000 194 | vn 0.52430000 0.85150000 0.00740000 195 | vn 0.52310000 0.85230000 0.00200000 196 | vn 0.51960000 0.85440000 0.00100000 197 | vn 0.51970000 0.85440000 0.00120000 198 | vn 0.52430000 0.85150000 0.00740000 199 | vn 0.51960000 0.85440000 0.00100000 200 | vn 0.51930000 0.85460000 0.00050000 201 | vn 0.51970000 0.85440000 0.00120000 202 | vn 0.56320000 0.82630000 0.00830000 203 | vn 0.56720000 0.82350000 0.00970000 204 | vn 0.55750000 0.83010000 0.00890000 205 | vn 0.56960000 0.82190000 0.00250000 206 | vn 0.56320000 0.82630000 0.00830000 207 | vn 0.56960000 0.82190000 0.00250000 208 | vn 0.56720000 0.82350000 0.00970000 209 | vn 0.57420000 0.81870000 0.00130000 210 | vn 0.56960000 0.82190000 0.00250000 211 | vn 0.57420000 0.81870000 0.00130000 212 | vn 0.57330000 0.81940000 0.00560000 213 | vn 0.56720000 0.82350000 0.00970000 214 | vn 0.55600000 0.83110000 0.00980000 215 | vn 0.55750000 0.83010000 0.00890000 216 | vn 0.56320000 0.82630000 0.00830000 217 | vn 0.57420000 0.81870000 0.00130000 218 | vn 0.57330000 0.81940000 0.00560000 219 | vn 0.57590000 0.81760000 0.00110000 220 | vn 0.54990000 0.83520000 0.00920000 221 | vn 0.55600000 0.83110000 0.00980000 222 | vn 0.55750000 0.83010000 0.00890000 223 | vn 0.54380000 0.83910000 0.01110000 224 | vn 0.54990000 0.83520000 0.00920000 225 | vn 0.55600000 0.83110000 0.00980000 226 | vn 0.57590000 0.81760000 0.00110000 227 | vn 0.57590000 0.81750000 0.00060000 228 | vn 0.54380000 0.83910000 0.01110000 229 | vn 0.54990000 0.83520000 0.00920000 230 | vn 0.54330000 0.83950000 0.01080000 231 | f 8//8 12//12 60//60 232 | f 9//9 2//2 5//5 233 | f 7//7 6//6 10//10 234 | f 22//22 16//16 18//18 235 | f 21//21 17//17 24//24 236 | f 28//28 20//20 25//25 237 | f 31//31 26//26 23//23 238 | f 35//35 27//27 30//30 239 | f 36//36 34//34 29//29 240 | f 38//38 32//32 37//37 241 | f 39//39 42//42 33//33 242 | f 47//47 48//48 41//41 243 | f 46//46 43//43 40//40 244 | f 45//45 50//50 49//49 245 | f 61//61 51//51 44//44 246 | f 53//53 58//58 3//3 247 | f 1//1 54//54 4//4 248 | f 59//59 11//11 52//52 249 | f 64//64 67//67 69//69 250 | f 73//73 65//65 62//62 251 | f 76//76 66//66 71//71 252 | f 79//79 75//75 72//72 253 | f 78//78 74//74 82//82 254 | f 83//83 77//77 80//80 255 | f 85//85 81//81 57//57 256 | f 55//55 84//84 56//56 257 | f 89//89 91//91 94//94 258 | f 97//97 87//87 92//92 259 | f 86//86 100//100 90//90 260 | f 95//95 93//93 101//101 261 | f 105//105 109//109 98//98 262 | f 106//106 99//99 88//88 263 | f 14//14 103//103 110//110 264 | f 15//15 96//96 102//102 265 | f 13//13 111//111 19//19 266 | f 112//112 68//68 107//107 267 | f 113//113 108//108 104//104 268 | f 63//63 70//70 114//114 269 | -------------------------------------------------------------------------------- /model/franka_emika_panda/assets/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/franka_emika_panda/assets/link7.stl -------------------------------------------------------------------------------- /model/franka_emika_panda/hand.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 115 | -------------------------------------------------------------------------------- /model/franka_emika_panda/mjx_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /model/franka_emika_panda/mjx_single_cube.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 22 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /model/franka_emika_panda/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /model/franka_emika_panda/scene_withcamera.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /model/franka_emika_panda/scene_withcube.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Base.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Base.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Base.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Base_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Base_Motor.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Base_Motor.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Base_Motor.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw_Motor.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw_Motor.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw_Motor.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw_part1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw_part1.ply -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw_part1.ply.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw_part1.ply.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw_part2.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw_part2.ply -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Fixed_Jaw_part2.ply.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Fixed_Jaw_part2.ply.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Lower_Arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Lower_Arm.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Lower_Arm.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Lower_Arm.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Lower_Arm_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Lower_Arm_Motor.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Lower_Arm_Motor.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Lower_Arm_Motor.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw_part1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw_part1.ply -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw_part1.ply.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw_part1.ply.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw_part2.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw_part2.ply -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw_part2.ply.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw_part2.ply.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw_part3.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw_part3.ply -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Moving_Jaw_part3.ply.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Moving_Jaw_part3.ply.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Rotation_Pitch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Rotation_Pitch.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Rotation_Pitch.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Rotation_Pitch.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Rotation_Pitch_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Rotation_Pitch_Motor.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Rotation_Pitch_Motor.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Rotation_Pitch_Motor.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Upper_Arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Upper_Arm.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Upper_Arm.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Upper_Arm.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Upper_Arm_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Upper_Arm_Motor.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Upper_Arm_Motor.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Upper_Arm_Motor.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Wrist_Pitch_Roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Wrist_Pitch_Roll.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Wrist_Pitch_Roll.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Wrist_Pitch_Roll.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Wrist_Pitch_Roll_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Wrist_Pitch_Roll_Motor.stl -------------------------------------------------------------------------------- /model/so_arm100_description/meshes/Wrist_Pitch_Roll_Motor.stl.convex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/so_arm100_description/meshes/Wrist_Pitch_Roll_Motor.stl.convex.stl -------------------------------------------------------------------------------- /model/so_arm100_description/so100.srdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /model/trs_so_arm100/README.md: -------------------------------------------------------------------------------- 1 | # Standard Open Arm-100 5DOF - Version 1.3 (MJCF) 2 | 3 | > [!IMPORTANT] 4 | > Requires MuJoCo 3.1.6 or later. 5 | 6 | ## Overview 7 | 8 | This package contains a simplified robot description (MJCF) of the [Standard Open Arm-100](https://github.com/TheRobotStudio/SO-ARM100/tree/main) developed by [The Robot Studio](https://www.therobotstudio.com/). It is derived from the [publicly available URDF description](https://github.com/TheRobotStudio/SO-ARM100/blob/4e9c5588d8a8415b6a6c2142a0ce8c32207cf3e9/URDF/SO_5DOF_ARM100_8j_URDF.SLDASM/urdf/SO_5DOF_ARM100_8j_URDF.SLDASM.urdf). 9 | 10 |

11 | 12 |

13 | 14 | ## URDF → MJCF derivation steps 15 | 16 | 1. Added `` to the URDF’s `` element to preserve visual geometries. 17 | 2. Loaded the URDF into MuJoCo and saved it as a corresponding MJCF. 18 | 3. Manually edited the MJCF to extract shared properties into the `` section. 19 | 4. Edited original meshes: extracted servos into separate files, manually created convex collision meshes for the gripper jaws. 20 | 5. Added extra box-shaped collision geoms to the gripper for additional contact points. 21 | 6. Added an `exclude` clause to prevent collisions between `Base` and `Rotation_Pitch`. 22 | 7. Added approximate joint limits. 23 | 8. Added position-controlled actuators. 24 | 9. Added `impratio="10"` and `cone="elliptic"` attributes for improved no-slip behavior. 25 | 10. Created `scene.xml` to include the robot, along with a textured ground plane, skybox, and haze. 26 | 27 | ## License 28 | 29 | This model is released under an [Apache-2.0 License](LICENSE). 30 | -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Base.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Base_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Base_Motor.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Fixed_Jaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Fixed_Jaw.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Fixed_Jaw_Collision_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Fixed_Jaw_Collision_1.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Fixed_Jaw_Collision_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Fixed_Jaw_Collision_2.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Fixed_Jaw_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Fixed_Jaw_Motor.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Lower_Arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Lower_Arm.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Lower_Arm_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Lower_Arm_Motor.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Moving_Jaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Moving_Jaw.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Moving_Jaw_Collision_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Moving_Jaw_Collision_1.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Moving_Jaw_Collision_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Moving_Jaw_Collision_2.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Moving_Jaw_Collision_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Moving_Jaw_Collision_3.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Rotation_Pitch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Rotation_Pitch.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Rotation_Pitch_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Rotation_Pitch_Motor.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Upper_Arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Upper_Arm.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Upper_Arm_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Upper_Arm_Motor.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Wrist_Pitch_Roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Wrist_Pitch_Roll.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/Wrist_Pitch_Roll_Motor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/Wrist_Pitch_Roll_Motor.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/angled_extrusion.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/angled_extrusion.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/corner_bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/corner_bracket.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/d405_solid.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/d405_solid.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/extrusion_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/extrusion_1000.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/extrusion_1220.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/extrusion_1220.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/extrusion_150.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/extrusion_150.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/extrusion_2040_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/extrusion_2040_1000.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/extrusion_2040_880.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/extrusion_2040_880.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/extrusion_600.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/extrusion_600.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/overhead_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/overhead_mount.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/tabletop.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v -0.37953900 -0.60982800 0.73674500 4 | v -0.37953900 -0.60982800 0.74887200 5 | v -0.38100000 -0.60839100 0.73674500 6 | v -0.38100000 -0.60839100 0.74887200 7 | v -0.18977000 -0.60982800 0.74887200 8 | v -0.18977000 -0.60839100 0.75078500 9 | v -0.37953900 -0.60839100 0.75078700 10 | v -0.38100000 0.60793400 0.74887200 11 | v -0.37953900 0.60793400 0.75078700 12 | v 0.38100000 -0.60839100 0.73674500 13 | v 0.38100000 -0.60839100 0.74887200 14 | v 0.37953900 -0.60982800 0.73674500 15 | v 0.37953900 -0.60982800 0.74887200 16 | v 0.37953900 0.60793400 0.75078700 17 | v 0.37953900 -0.60839100 0.75078700 18 | v 0.38100000 0.60793400 0.74887200 19 | v -0.38100000 0.60793400 0.73674500 20 | v -0.37953900 0.60937200 0.73674500 21 | v -0.37953900 0.60937200 0.74887200 22 | v -0.18977000 0.60793400 0.75078700 23 | v -0.18977000 0.60937200 0.74887200 24 | v 0.37953900 0.60937200 0.73674500 25 | v 0.37953900 0.60937200 0.74887200 26 | v 0.38100000 0.60793400 0.73674500 27 | v -0.37953900 -0.60839100 0.73483100 28 | v -0.37953900 0.60793400 0.73483100 29 | v -0.18977000 0.60937200 0.73674500 30 | v -0.18977000 0.60793400 0.73483100 31 | v 0.37953900 -0.60839100 0.73483100 32 | v 0.37953900 0.60793400 0.73483100 33 | v -0.18977000 -0.60839100 0.73483100 34 | v -0.18977000 -0.60982800 0.73674500 35 | v 0.18977000 0.60793400 0.75078700 36 | v 0.18977000 -0.60839100 0.75078700 37 | v 0.18977000 0.60937200 0.74887200 38 | v 0.18977000 0.60937200 0.73674500 39 | v 0.18977000 0.60793400 0.73483100 40 | v 0.18977000 -0.60839100 0.73483100 41 | v 0.18977000 -0.60982800 0.73674500 42 | v 0.18977000 -0.60982800 0.74887200 43 | v 0.00000000 0.60793400 0.75078700 44 | v 0.00000000 -0.60839100 0.75078500 45 | v 0.00000000 0.60937200 0.74887200 46 | v 0.00000000 0.60937200 0.73674500 47 | v 0.00000000 0.60793400 0.73483100 48 | v 0.00000000 -0.60839100 0.73483100 49 | v 0.00000000 -0.60982800 0.73674500 50 | v 0.00000000 -0.60982800 0.74887200 51 | vt 0.75019800 0.57261000 52 | vt 0.74519900 0.57261000 53 | vt 0.74382100 0.56603500 54 | vt 0.74382100 0.57103100 55 | vt 0.74519900 0.67519800 56 | vt 0.74362100 0.67519800 57 | vt 0.74362100 0.57261000 58 | vt 0.12297800 0.57103200 59 | vt 0.12297900 0.57261000 60 | vt 0.74362100 0.15331600 61 | vt 0.74382100 0.98454000 62 | vt 0.75019800 0.98296000 63 | vt 0.74519900 0.98296000 64 | vt 0.12381100 0.98296000 65 | vt 0.74362100 0.98296000 66 | vt 0.12381700 0.98453800 67 | vt 0.11445600 0.57182100 68 | vt 0.11448400 0.57261000 69 | vt 0.12160500 0.57261000 70 | vt 0.12318700 0.67519800 71 | vt 0.12181300 0.67519800 72 | vt 0.11531600 0.98296000 73 | vt 0.12243700 0.98296000 74 | vt 0.11524200 0.98375000 75 | vt 0.74362100 0.56445500 76 | vt 0.12296000 0.56445500 77 | vt 0.11469200 0.67519800 78 | vt 0.11331800 0.67519800 79 | vt 0.74362100 0.15410500 80 | vt 0.12256600 0.15410500 81 | vt 0.74362100 0.46186700 82 | vt 0.74441000 0.46186700 83 | vt 0.12360300 0.88037300 84 | vt 0.74362100 0.88037300 85 | vt 0.12222900 0.88037300 86 | vt 0.11510800 0.88037300 87 | vt 0.11373500 0.88037300 88 | vt 0.74362100 0.25669200 89 | vt 0.74441000 0.25669200 90 | vt 0.74519900 0.88037300 91 | vt 0.12339500 0.77778500 92 | vt 0.74362100 0.77778500 93 | vt 0.12202100 0.77778500 94 | vt 0.11490000 0.77778500 95 | vt 0.11352700 0.77778500 96 | vt 0.74362100 0.35928000 97 | vt 0.74441000 0.35928000 98 | vt 0.74519900 0.77778500 99 | f 1/1 2/2 3/3 100 | f 3/3 2/2 4/4 101 | f 5/5 6/6 2/2 102 | f 2/2 6/6 7/7 103 | f 8/8 4/4 9/9 104 | f 9/9 4/4 7/7 105 | f 10/10 11/11 12/12 106 | f 12/12 11/11 13/13 107 | f 14/14 15/15 16/16 108 | f 16/16 15/15 11/11 109 | f 17/17 8/8 18/18 110 | f 18/18 8/8 19/19 111 | f 20/20 21/21 9/9 112 | f 9/9 21/21 19/19 113 | f 22/22 23/23 24/24 114 | f 24/24 23/23 16/16 115 | f 3/3 17/17 25/25 116 | f 25/25 17/17 26/26 117 | f 27/27 28/28 18/18 118 | f 18/18 28/28 26/26 119 | f 29/29 30/30 10/10 120 | f 10/10 30/30 24/24 121 | f 31/31 32/32 25/25 122 | f 25/25 32/32 1/1 123 | f 9/9 7/7 20/20 124 | f 20/20 7/7 6/6 125 | f 18/18 19/19 27/27 126 | f 27/27 19/19 21/21 127 | f 25/25 26/26 31/31 128 | f 31/31 26/26 28/28 129 | f 2/2 1/1 5/5 130 | f 5/5 1/1 32/32 131 | f 16/16 11/11 24/24 132 | f 24/24 11/11 10/10 133 | f 17/17 3/3 8/8 134 | f 8/8 3/3 4/4 135 | f 2/2 7/7 4/4 136 | f 11/11 15/15 13/13 137 | f 8/8 9/9 19/19 138 | f 23/23 14/14 16/16 139 | f 17/17 18/18 26/26 140 | f 30/30 22/22 24/24 141 | f 3/3 25/25 1/1 142 | f 12/12 29/29 10/10 143 | f 33/33 34/34 14/14 144 | f 14/14 34/34 15/15 145 | f 23/23 35/35 14/14 146 | f 14/14 35/35 33/33 147 | f 36/36 35/35 22/22 148 | f 22/22 35/35 23/23 149 | f 30/30 37/37 22/22 150 | f 22/22 37/37 36/36 151 | f 38/38 37/37 29/29 152 | f 29/29 37/37 30/30 153 | f 12/12 39/39 29/29 154 | f 29/29 39/39 38/38 155 | f 40/40 39/39 13/13 156 | f 13/13 39/39 12/12 157 | f 15/15 34/34 13/13 158 | f 13/13 34/34 40/40 159 | f 41/41 42/42 33/33 160 | f 33/33 42/42 34/34 161 | f 33/33 35/35 41/41 162 | f 41/41 35/35 43/43 163 | f 44/44 43/43 36/36 164 | f 36/36 43/43 35/35 165 | f 36/36 37/37 44/44 166 | f 44/44 37/37 45/45 167 | f 46/46 45/45 38/38 168 | f 38/38 45/45 37/37 169 | f 38/38 39/39 46/46 170 | f 46/46 39/39 47/47 171 | f 48/48 47/47 40/40 172 | f 40/40 47/47 39/39 173 | f 40/40 34/34 48/48 174 | f 48/48 34/34 42/42 175 | f 20/20 6/6 41/41 176 | f 41/41 6/6 42/42 177 | f 41/41 43/43 20/20 178 | f 20/20 43/43 21/21 179 | f 27/27 21/21 44/44 180 | f 44/44 21/21 43/43 181 | f 44/44 45/45 27/27 182 | f 27/27 45/45 28/28 183 | f 31/31 28/28 46/46 184 | f 46/46 28/28 45/45 185 | f 46/46 47/47 31/31 186 | f 31/31 47/47 32/32 187 | f 5/5 32/32 48/48 188 | f 48/48 32/32 47/47 189 | f 48/48 42/42 5/5 190 | f 5/5 42/42 6/6 191 | -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_1_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_1_base.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_2_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_2_shoulder.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_3_upper_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_3_upper_arm.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_4_upper_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_4_upper_forearm.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_5_lower_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_5_lower_forearm.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_6_wrist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_6_wrist.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_7_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_7_gripper.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_7_gripper_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_7_gripper_bar.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_7_gripper_camera.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_7_gripper_camera.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_7_gripper_prop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_7_gripper_prop.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_7_gripper_prop_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_7_gripper_prop_bar.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_7_gripper_wrist_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_7_gripper_wrist_mount.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_8_custom_finger_left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_8_custom_finger_left.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/vx300s_8_custom_finger_right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/vx300s_8_custom_finger_right.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/assets/wormseye_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LitchiCheng/mujoco-learning/3582c9062710a6714e14e94802c1a9ac2792758c/model/trs_so_arm100/assets/wormseye_mount.stl -------------------------------------------------------------------------------- /model/trs_so_arm100/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /model/trs_so_arm100/scene_with_cube.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /model/trs_so_arm100/scene_with_table.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /model/trs_so_arm100/scene_without_position.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /model/trs_so_arm100/so_arm100.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 150 | -------------------------------------------------------------------------------- /model/trs_so_arm100/so_arm100_without_position.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 150 | -------------------------------------------------------------------------------- /move_ball.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import mujoco.viewer 3 | import numpy as np 4 | from pynput import keyboard 5 | 6 | key_states = { 7 | keyboard.Key.up: False, 8 | keyboard.Key.down: False, 9 | keyboard.Key.left: False, 10 | keyboard.Key.right: False, 11 | keyboard.Key.alt_l: False, 12 | keyboard.Key.alt_r: False, 13 | } 14 | 15 | def on_press(key): 16 | if key in key_states: 17 | key_states[key] = True 18 | 19 | def on_release(key): 20 | if key in key_states: 21 | key_states[key] = False 22 | 23 | listener = keyboard.Listener(on_press=on_press, on_release=on_release) 24 | listener.start() 25 | 26 | XML = """ 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | """ 51 | 52 | model = mujoco.MjModel.from_xml_string(XML) 53 | model.opt.gravity = (0, 0, 0) 54 | data = mujoco.MjData(model) 55 | 56 | class CustomViewer: 57 | def __init__(self, model, data): 58 | self.handle = mujoco.viewer.launch_passive(model, data) 59 | self.pos = 0.0001 60 | 61 | def is_running(self): 62 | return self.handle.is_running() 63 | 64 | def sync(self): 65 | self.handle.sync() 66 | 67 | @property 68 | def cam(self): 69 | return self.handle.cam 70 | 71 | @property 72 | def viewport(self): 73 | return self.handle.viewport 74 | 75 | def run_loop(self): 76 | while self.is_running(): 77 | ball_body_name = "ball" 78 | pos = data.body(ball_body_name).xpos 79 | quat = data.body(ball_body_name).xquat 80 | print(f"Position: {pos}, Orientation: {quat}") 81 | 82 | if key_states[keyboard.Key.up]: 83 | data.qpos[0] += self.pos 84 | if key_states[keyboard.Key.down]: 85 | data.qpos[0] -= self.pos 86 | if key_states[keyboard.Key.left]: 87 | data.qpos[1] += self.pos 88 | if key_states[keyboard.Key.right]: 89 | data.qpos[1] -= self.pos 90 | if key_states[keyboard.Key.alt_l]: 91 | data.qpos[2] += self.pos 92 | if key_states[keyboard.Key.alt_r]: 93 | data.qpos[2] -= self.pos 94 | 95 | mujoco.mj_step(model, data) 96 | self.sync() 97 | 98 | 99 | viewer = CustomViewer(model, data) 100 | viewer.cam.distance = 3 101 | viewer.cam.azimuth = 0 102 | viewer.cam.elevation = -30 103 | viewer.run_loop() -------------------------------------------------------------------------------- /mujoco_viewer.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import mujoco.viewer 3 | 4 | class CustomViewer: 5 | def __init__(self, model_path, distance=3, azimuth=0, elevation=-30): 6 | self.model = mujoco.MjModel.from_xml_path(model_path) 7 | self.data = mujoco.MjData(self.model) 8 | 9 | self.handle = mujoco.viewer.launch_passive(self.model, self.data) 10 | self.handle.cam.distance = distance 11 | self.handle.cam.azimuth = azimuth 12 | self.handle.cam.elevation = elevation 13 | 14 | def is_running(self): 15 | return self.handle.is_running() 16 | 17 | def sync(self): 18 | self.handle.sync() 19 | 20 | @property 21 | def cam(self): 22 | return self.handle.cam 23 | 24 | @property 25 | def viewport(self): 26 | return self.handle.viewport 27 | 28 | def run_loop(self): 29 | self.runBefore() 30 | while self.is_running(): 31 | mujoco.mj_forward(self.model, self.data) 32 | self.runFunc() 33 | mujoco.mj_step(self.model, self.data) 34 | self.sync() 35 | 36 | def runBefore(self): 37 | pass 38 | 39 | def runFunc(self): 40 | pass -------------------------------------------------------------------------------- /path_plan_ompl_rrtconnect.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import ompl.base as ob 3 | import ompl.geometric as og 4 | import time 5 | import mujoco_viewer 6 | 7 | class Test(mujoco_viewer.CustomViewer): 8 | def __init__(self, path): 9 | super().__init__(path, 3, azimuth=-45, elevation=-30) 10 | 11 | def runBefore(self): 12 | state_space = ob.RealVectorStateSpace(self.model.nq) 13 | bounds = ob.RealVectorBounds(self.model.nq) 14 | for i in range(min(self.model.nq, self.model.jnt_range.shape[0])): 15 | bounds.setLow(i, self.model.jnt_range[i, 0]) 16 | bounds.setHigh(i, self.model.jnt_range[i, 1]) 17 | print(self.model.jnt_range[i, 0], self.model.jnt_range[i, 1]) 18 | state_space.setBounds(bounds) 19 | si = ob.SpaceInformation(state_space) 20 | 21 | def is_state_valid(state): 22 | self.data.qpos[:7] = [state[i] for i in range(7)] 23 | mujoco.mj_step(self.model, self.data) 24 | return self.data.ncon == 0 25 | 26 | si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid)) 27 | si.setup() 28 | start = ob.State(state_space) 29 | goal = ob.State(state_space) 30 | start_js = [-2.80558, 0.575492, -0.331959, -0.0709803, 1.21621, 1.03666, 2.21675, 0.0148011, 0.0147989] 31 | goal_js = [-0.40558, 0.375492, -0.231959, -0.0709803, 1.21621, 1.03666, 1.21675, 0.0148011, 0.0247989] 32 | for i in range(min(self.model.nq, self.model.jnt_range.shape[0])): 33 | start[i] = start_js[i] 34 | goal[i] = goal_js[i] 35 | 36 | pdef = ob.ProblemDefinition(si) 37 | pdef.setStartAndGoalStates(start, goal) 38 | planner = og.RRTConnect(si) 39 | planner.setRange(0.01) 40 | # planner.setIntermediateStates(True) 41 | planner.setProblemDefinition(pdef) 42 | planner.setup() 43 | solved = planner.solve(10.0) 44 | self.path_states = [] 45 | if solved: 46 | self.path = pdef.getSolutionPath() 47 | for i in range(self.path.getStateCount()): 48 | state = self.path.getState(i) 49 | state_values = [state[i] for i in range(self.model.nq)] 50 | self.path_states.append(state_values) 51 | print(state_values) 52 | else: 53 | print("No solution found.") 54 | self.index = 0 55 | 56 | def runFunc(self): 57 | if self.index < len(self.path_states): 58 | self.data.qpos[:7] = self.path_states[self.index][:7] 59 | self.index += 1 60 | else: 61 | self.data.qpos[:7] = self.path_states[-1][:7] 62 | time.sleep(0.01) 63 | 64 | test = Test("model/franka_emika_panda/scene.xml") 65 | test.run_loop() -------------------------------------------------------------------------------- /path_plan_pyroboplan_rrt.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import mujoco,time,threading 3 | import numpy as np 4 | import pinocchio 5 | import matplotlib 6 | # matplotlib.use('TkAgg') 7 | import matplotlib.pyplot as plt 8 | from mpl_toolkits.mplot3d import Axes3D 9 | 10 | from pyroboplan.core.utils import ( 11 | get_random_collision_free_state, 12 | extract_cartesian_poses, 13 | ) 14 | from pyroboplan.models.panda import ( 15 | load_models, 16 | add_self_collisions, 17 | add_object_collisions, 18 | ) 19 | from pyroboplan.planning.rrt import RRTPlanner, RRTPlannerOptions 20 | from pyroboplan.trajectory.trajectory_optimization import ( 21 | CubicTrajectoryOptimization, 22 | CubicTrajectoryOptimizationOptions, 23 | ) 24 | 25 | class Test(mujoco_viewer.CustomViewer): 26 | def __init__(self, path): 27 | super().__init__(path, 3, azimuth=180, elevation=-30) 28 | self.path = path 29 | 30 | def runBefore(self): 31 | # Create models and data 32 | self.model_roboplan, self.collision_model, visual_model = load_models(use_sphere_collisions=True) 33 | add_self_collisions(self.model_roboplan, self.collision_model) 34 | add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1) 35 | 36 | data = self.model_roboplan.createData() 37 | collision_data = self.collision_model.createData() 38 | 39 | self.target_frame = "panda_hand" 40 | ignore_joint_indices = [ 41 | self.model_roboplan.getJointId("panda_finger_joint1") - 1, 42 | self.model_roboplan.getJointId("panda_finger_joint2") - 1, 43 | ] 44 | np.set_printoptions(precision=3) 45 | self.distance_padding = 0.001 46 | 47 | self.init_state = self.data.qpos.copy() 48 | 49 | while True: 50 | q_start = self.random_valid_state() 51 | q_goal = self.random_valid_state() 52 | 53 | # Search for a path 54 | options = RRTPlannerOptions( 55 | max_step_size=0.05, 56 | max_connection_dist=5.0, 57 | rrt_connect=False, 58 | bidirectional_rrt=True, 59 | rrt_star=True, 60 | max_rewire_dist=5.0, 61 | max_planning_time=20.0, 62 | fast_return=True, 63 | goal_biasing_probability=0.15, 64 | collision_distance_padding=0.01, 65 | ) 66 | print("") 67 | print(f"Planning a path...") 68 | planner = RRTPlanner(self.model_roboplan, self.collision_model, options=options) 69 | q_path = planner.plan(q_start, q_goal) 70 | if len(q_path) > 0: 71 | print(f"Got a path with {len(q_path)} waypoints") 72 | else: 73 | print("Failed to plan.") 74 | 75 | # Perform trajectory optimization. 76 | dt = 0.025 77 | options = CubicTrajectoryOptimizationOptions( 78 | num_waypoints=len(q_path), 79 | samples_per_segment=7, 80 | min_segment_time=0.5, 81 | max_segment_time=10.0, 82 | min_vel=-1.5, 83 | max_vel=1.5, 84 | min_accel=-0.75, 85 | max_accel=0.75, 86 | min_jerk=-1.0, 87 | max_jerk=1.0, 88 | max_planning_time=30.0, 89 | check_collisions=True, 90 | min_collision_dist=self.distance_padding, 91 | collision_influence_dist=0.05, 92 | collision_avoidance_cost_weight=0.0, 93 | collision_link_list=[ 94 | "obstacle_box_1", 95 | "obstacle_box_2", 96 | "obstacle_sphere_1", 97 | "obstacle_sphere_2", 98 | "ground_plane", 99 | "panda_hand", 100 | ], 101 | ) 102 | print("Optimizing the path...") 103 | optimizer = CubicTrajectoryOptimization(self.model_roboplan, self.collision_model, options) 104 | traj = optimizer.plan([q_path[0], q_path[-1]], init_path=q_path) 105 | 106 | if traj is None: 107 | print("Retrying with all the RRT waypoints...") 108 | traj = optimizer.plan(q_path, init_path=q_path) 109 | 110 | if traj is not None: 111 | print("Trajectory optimization successful") 112 | traj_gen = traj.generate(dt) 113 | self.q_vec = traj_gen[1] 114 | print(f"path has {self.q_vec.shape[1]} points") 115 | tforms = extract_cartesian_poses(self.model_roboplan, "panda_hand", self.q_vec.T) 116 | # 提取位置信息 117 | positions = [] 118 | for tform in tforms: 119 | position = tform.translation 120 | positions.append(position) 121 | 122 | positions = np.array(positions) 123 | 124 | # 创建 3D 图形 125 | fig = plt.figure() 126 | ax = fig.add_subplot(111, projection='3d') 127 | 128 | # 绘制位置轨迹 129 | ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], marker='o') 130 | 131 | # 绘制姿态 132 | for i, tform in enumerate(tforms): 133 | position = tform.translation 134 | rotation_matrix = tform.rotation 135 | # 提取坐标轴方向的向量 136 | x_axis = rotation_matrix[:, 0] 137 | y_axis = rotation_matrix[:, 1] 138 | z_axis = rotation_matrix[:, 2] 139 | # 绘制坐标轴向量 140 | ax.quiver(position[0], position[1], position[2], 141 | x_axis[0], x_axis[1], x_axis[2], color='r', length=0.01) 142 | ax.quiver(position[0], position[1], position[2], 143 | y_axis[0], y_axis[1], y_axis[2], color='g', length=0.01) 144 | ax.quiver(position[0], position[1], position[2], 145 | z_axis[0], z_axis[1], z_axis[2], color='b', length=0.01) 146 | 147 | # 设置坐标轴标签 148 | ax.set_xlabel('X') 149 | ax.set_ylabel('Y') 150 | ax.set_zlabel('Z') 151 | 152 | # 显示图形 153 | plt.show(block=False) 154 | plt.pause(0.001) 155 | break 156 | 157 | self.index = 0 158 | 159 | 160 | def random_valid_state(self): 161 | return get_random_collision_free_state( 162 | self.model_roboplan, self.collision_model, distance_padding=0.01 163 | ) 164 | 165 | def runFunc(self): 166 | self.data.qpos[:7] = self.q_vec[:7, self.index] 167 | self.index += 1 168 | if self.index >= self.q_vec.shape[1]: 169 | self.index = 0 170 | time.sleep(0.01) 171 | 172 | if __name__ == "__main__": 173 | test = Test("model/franka_emika_panda/scene.xml") 174 | test.run_loop() 175 | 176 | -------------------------------------------------------------------------------- /path_plan_pyroboplan_rrt_draw_trajectory.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import mujoco,time,threading 3 | import numpy as np 4 | import pinocchio 5 | import matplotlib 6 | # matplotlib.use('TkAgg') 7 | import matplotlib.pyplot as plt 8 | from mpl_toolkits.mplot3d import Axes3D 9 | import itertools 10 | 11 | from pyroboplan.core.utils import ( 12 | get_random_collision_free_state, 13 | extract_cartesian_poses, 14 | ) 15 | from pyroboplan.models.panda import ( 16 | load_models, 17 | add_self_collisions, 18 | add_object_collisions, 19 | ) 20 | from pyroboplan.planning.rrt import RRTPlanner, RRTPlannerOptions 21 | from pyroboplan.trajectory.trajectory_optimization import ( 22 | CubicTrajectoryOptimization, 23 | CubicTrajectoryOptimizationOptions, 24 | ) 25 | 26 | class Test(mujoco_viewer.CustomViewer): 27 | def __init__(self, path): 28 | super().__init__(path, 3, azimuth=180, elevation=-30) 29 | self.path = path 30 | 31 | def runBefore(self): 32 | # Create models and data 33 | self.model_roboplan, self.collision_model, visual_model = load_models(use_sphere_collisions=True) 34 | add_self_collisions(self.model_roboplan, self.collision_model) 35 | add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1) 36 | 37 | data = self.model_roboplan.createData() 38 | collision_data = self.collision_model.createData() 39 | 40 | self.target_frame = "panda_hand" 41 | ignore_joint_indices = [ 42 | self.model_roboplan.getJointId("panda_finger_joint1") - 1, 43 | self.model_roboplan.getJointId("panda_finger_joint2") - 1, 44 | ] 45 | np.set_printoptions(precision=3) 46 | self.distance_padding = 0.001 47 | 48 | self.init_state = self.data.qpos.copy() 49 | 50 | while True: 51 | q_start = self.random_valid_state() 52 | q_goal = self.random_valid_state() 53 | 54 | # Search for a path 55 | options = RRTPlannerOptions( 56 | max_step_size=0.05, 57 | max_connection_dist=5.0, 58 | rrt_connect=False, 59 | bidirectional_rrt=True, 60 | rrt_star=True, 61 | max_rewire_dist=5.0, 62 | max_planning_time=20.0, 63 | fast_return=True, 64 | goal_biasing_probability=0.15, 65 | collision_distance_padding=0.01, 66 | ) 67 | print("") 68 | print(f"Planning a path...") 69 | planner = RRTPlanner(self.model_roboplan, self.collision_model, options=options) 70 | q_path = planner.plan(q_start, q_goal) 71 | if len(q_path) > 0: 72 | print(f"Got a path with {len(q_path)} waypoints") 73 | else: 74 | print("Failed to plan.") 75 | 76 | # Perform trajectory optimization. 77 | dt = 0.025 78 | options = CubicTrajectoryOptimizationOptions( 79 | num_waypoints=len(q_path), 80 | samples_per_segment=7, 81 | min_segment_time=0.5, 82 | max_segment_time=10.0, 83 | min_vel=-1.5, 84 | max_vel=1.5, 85 | min_accel=-0.75, 86 | max_accel=0.75, 87 | min_jerk=-1.0, 88 | max_jerk=1.0, 89 | max_planning_time=30.0, 90 | check_collisions=True, 91 | min_collision_dist=self.distance_padding, 92 | collision_influence_dist=0.05, 93 | collision_avoidance_cost_weight=0.0, 94 | collision_link_list=[ 95 | "obstacle_box_1", 96 | "obstacle_box_2", 97 | "obstacle_sphere_1", 98 | "obstacle_sphere_2", 99 | "ground_plane", 100 | "panda_hand", 101 | ], 102 | ) 103 | print("Optimizing the path...") 104 | optimizer = CubicTrajectoryOptimization(self.model_roboplan, self.collision_model, options) 105 | traj = optimizer.plan([q_path[0], q_path[-1]], init_path=q_path) 106 | 107 | if traj is None: 108 | print("Retrying with all the RRT waypoints...") 109 | traj = optimizer.plan(q_path, init_path=q_path) 110 | 111 | if traj is not None: 112 | print("Trajectory optimization successful") 113 | traj_gen = traj.generate(dt) 114 | self.q_vec = traj_gen[1] 115 | print(f"path has {self.q_vec.shape[1]} points") 116 | self.tforms = extract_cartesian_poses(self.model_roboplan, "panda_hand", self.q_vec.T) 117 | # 提取位置信息 118 | positions = [] 119 | print(self.tforms[0].translation) 120 | print(self.tforms[0].rotation) 121 | self.handle.user_scn.ngeom = 0 122 | i = 0 123 | for i, tform in enumerate(self.tforms): 124 | if i % 2 == 0: 125 | continue 126 | position = tform.translation 127 | rotation_matrix = tform.rotation 128 | mujoco.mjv_initGeom( 129 | self.handle.user_scn.geoms[i], 130 | type=mujoco.mjtGeom.mjGEOM_SPHERE, 131 | size=[0.005, 0, 0], 132 | pos=np.array([tform.translation[0], tform.translation[1], tform.translation[2]]), 133 | mat=np.eye(3).flatten(), 134 | rgba=np.array([1, 0, 0, 1]) 135 | ) 136 | i += 1 137 | self.handle.user_scn.ngeom = i 138 | print(f"Added {i} spheres to the scene.") 139 | for tform in self.tforms: 140 | position = tform.translation 141 | positions.append(position) 142 | 143 | positions = np.array(positions) 144 | 145 | # 创建 3D 图形 146 | fig = plt.figure() 147 | ax = fig.add_subplot(111, projection='3d') 148 | 149 | # 绘制位置轨迹 150 | ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], marker='o') 151 | 152 | # 绘制姿态 153 | for i, tform in enumerate(self.tforms): 154 | position = tform.translation 155 | rotation_matrix = tform.rotation 156 | # 提取坐标轴方向的向量 157 | x_axis = rotation_matrix[:, 0] 158 | y_axis = rotation_matrix[:, 1] 159 | z_axis = rotation_matrix[:, 2] 160 | # 绘制坐标轴向量 161 | ax.quiver(position[0], position[1], position[2], 162 | x_axis[0], x_axis[1], x_axis[2], color='r', length=0.01) 163 | ax.quiver(position[0], position[1], position[2], 164 | y_axis[0], y_axis[1], y_axis[2], color='g', length=0.01) 165 | ax.quiver(position[0], position[1], position[2], 166 | z_axis[0], z_axis[1], z_axis[2], color='b', length=0.01) 167 | 168 | # 设置坐标轴标签 169 | ax.set_xlabel('X') 170 | ax.set_ylabel('Y') 171 | ax.set_zlabel('Z') 172 | 173 | # 显示图形 174 | plt.show(block=False) 175 | plt.pause(0.001) 176 | break 177 | 178 | self.index = 0 179 | 180 | 181 | def random_valid_state(self): 182 | return get_random_collision_free_state( 183 | self.model_roboplan, self.collision_model, distance_padding=0.01 184 | ) 185 | 186 | def runFunc(self): 187 | self.data.qpos[:7] = self.q_vec[:7, self.index] 188 | self.index += 1 189 | if self.index >= self.q_vec.shape[1]: 190 | self.index = 0 191 | time.sleep(0.01) 192 | 193 | 194 | if __name__ == "__main__": 195 | test = Test("model/franka_emika_panda/scene.xml") 196 | test.run_loop() 197 | 198 | -------------------------------------------------------------------------------- /pid_torque_and_get.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import numpy as np 3 | # matplotlib.use('TkAgg') 4 | import matplotlib.pyplot as plt 5 | import time,math 6 | 7 | class PIDController: 8 | def __init__(self, kp, ki, kd): 9 | self.kp = kp 10 | self.ki = ki 11 | self.kd = kd 12 | self.prev_error = 0 13 | self.integral = 0 14 | 15 | def update(self, error, dt): 16 | self.integral += error * dt 17 | derivative = (error - self.prev_error) / dt 18 | output = self.kp * error + self.ki * self.integral + self.kd * derivative 19 | self.prev_error = error 20 | return output 21 | 22 | class Test(mujoco_viewer.CustomViewer): 23 | def __init__(self, path): 24 | super().__init__(path, 3, azimuth=180, elevation=-30) 25 | self.path = path 26 | 27 | def runBefore(self): 28 | kp = 10.0 29 | ki = 1.0 30 | kd = 5.0 31 | self.pid = PIDController(kp, ki, kd) 32 | self.target_position = 3.14 / 4 33 | self.dt = self.model.opt.timestep 34 | self.positions = [] 35 | self.torques = [] 36 | 37 | def runFunc(self): 38 | wait_time = 0.01 39 | time.sleep(wait_time) 40 | # 获取当前关节位置 41 | current_position = self.data.qpos[0] 42 | # 计算误差 43 | error = self.target_position - current_position 44 | # 使用 PID 控制器计算扭矩 45 | torque = self.pid.update(error, self.dt + wait_time) 46 | print(f"error: {error}, Current Position: {current_position}, Target Position: {self.target_position}, Torque: {torque}") 47 | # 设置关节扭矩 48 | self.data.ctrl[0] = torque 49 | # 记录数据 50 | self.positions.append(current_position) 51 | self.torques.append(torque) 52 | if math.fabs(error) < 0.001: 53 | plt.figure(figsize=(12, 6)) 54 | plt.subplot(2, 1, 1) 55 | plt.plot(self.positions, label='Joint Position') 56 | plt.axhline(y=self.target_position, color='r', linestyle='--', label='Target Position') 57 | plt.xlabel('Time Step') 58 | plt.ylabel('Position') 59 | plt.legend() 60 | 61 | plt.subplot(2, 1, 2) 62 | plt.plot(self.torques, label='Torque') 63 | plt.xlabel('Time Step') 64 | plt.ylabel('Torque') 65 | plt.legend() 66 | 67 | plt.show() 68 | self.target_position = 0.0 69 | 70 | 71 | if __name__ == "__main__": 72 | test = Test("model/franka_emika_panda/scene.xml") 73 | test.run_loop() 74 | 75 | -------------------------------------------------------------------------------- /pinocchio-test1.py: -------------------------------------------------------------------------------- 1 | import pinocchio as pin 2 | import numpy as np 3 | 4 | # 加载 URDF 文件 5 | urdf_path = "/home/dar/MuJoCoBin/mujoco-learning/franka_panda_description/robots/panda_arm.urdf" 6 | model = pin.buildModelFromUrdf(urdf_path) 7 | data = model.createData() 8 | 9 | # 获取关节限位 10 | lower_limits = model.lowerPositionLimit 11 | upper_limits = model.upperPositionLimit 12 | # 打印关节限位 13 | print("Lower limits:", lower_limits) 14 | print("Upper limits:", upper_limits) 15 | 16 | # 打印配置向量和速度向量的维度 17 | print(f"Number of configuration variables (nq): {model.nq}") 18 | print(f"Number of velocity variables (nv): {model.nv}") 19 | 20 | # 计算正运动学 21 | q = np.zeros(model.nq) 22 | data = model.createData() 23 | pin.framesForwardKinematics(model, data, q) 24 | 25 | # 打印 joints 信息 26 | for i, joint in enumerate(model.joints): 27 | print(f"Joint {i}:") 28 | print(f" Name: {model.names[i]}") 29 | 30 | # 打印 jointPlacements 信息 31 | print("\nJoint Placements Information:") 32 | for i, placement in enumerate(model.jointPlacements): 33 | print(f"Joint Placement {i}:") 34 | print(f" Translation: {placement.translation}") 35 | print(f" Rotation Matrix:\n{placement.rotation}") 36 | 37 | # 打印 inertias 信息 38 | print("\nInertias Information:") 39 | for i, inertia in enumerate(model.inertias): 40 | print(f"Inertia {i}:") 41 | print(f" Mass: {inertia.mass}") 42 | print(f" Center of Mass: {inertia.lever}") 43 | print(f" Inertia Matrix:\n{inertia.inertia}") 44 | 45 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | absl-py==2.2.2 2 | asttokens==3.0.0 3 | cloudpickle==3.1.1 4 | cmeel==0.57.3 5 | cmeel-assimp==5.4.3.1 6 | cmeel-boost==1.87.0.1 7 | cmeel-console-bridge==1.0.2.3 8 | cmeel-octomap==1.10.0 9 | cmeel-qhull==8.0.2.1 10 | cmeel-tinyxml2==10.0.0 11 | cmeel-urdfdom==4.0.1 12 | cmeel-zlib==1.3.1 13 | coal-library==3.0.1 14 | contourpy==1.3.2 15 | cycler==0.12.1 16 | decorator==5.2.1 17 | drake==1.39.0 18 | eigenpy==3.10.3 19 | etils==1.12.2 20 | evdev==1.9.2 21 | exceptiongroup==1.3.0 22 | executing==2.2.0 23 | farama-notifications==0.0.4 24 | filelock==3.18.0 25 | fonttools==4.58.0 26 | fsspec==2025.3.2 27 | glfw==2.9.0 28 | grpcio==1.71.0 29 | gym==0.26.2 30 | gym-notices==0.0.8 31 | gymnasium==1.1.1 32 | importlib-resources==6.5.2 33 | ipython==8.36.0 34 | jedi==0.19.2 35 | jinja2==3.1.6 36 | jupyterlab-urdf==0.4.0 37 | kiwisolver==1.4.8 38 | markdown==3.8 39 | markupsafe==3.0.2 40 | matplotlib==3.10.1 41 | matplotlib-inline==0.1.7 42 | meshcat==0.3.2 43 | mpmath==1.3.0 44 | mujoco==3.3.2 45 | networkx==3.4.2 46 | numpy==2.2.6 47 | nvidia-cublas-cu12==12.6.4.1 48 | nvidia-cuda-cupti-cu12==12.6.80 49 | nvidia-cuda-nvrtc-cu12==12.6.77 50 | nvidia-cuda-runtime-cu12==12.6.77 51 | nvidia-cudnn-cu12==9.5.1.17 52 | nvidia-cufft-cu12==11.3.0.4 53 | nvidia-cufile-cu12==1.11.1.6 54 | nvidia-curand-cu12==10.3.7.77 55 | nvidia-cusolver-cu12==11.7.1.2 56 | nvidia-cusparse-cu12==12.5.4.2 57 | nvidia-cusparselt-cu12==0.6.3 58 | nvidia-nccl-cu12==2.26.2 59 | nvidia-nvjitlink-cu12==12.6.85 60 | nvidia-nvtx-cu12==12.6.77 61 | ompl==1.7.0 62 | opencv-python==4.11.0.86 63 | packaging==25.0 64 | pandas==2.2.3 65 | parso==0.8.4 66 | pexpect==4.9.0 67 | pillow==11.2.1 68 | pin==3.4.0 69 | prompt-toolkit==3.0.51 70 | protobuf==6.31.0 71 | psutil==7.0.0 72 | ptyprocess==0.7.0 73 | pure-eval==0.2.3 74 | pydot==4.0.0 75 | pygments==2.19.1 76 | pyngrok==7.2.8 77 | pynput==1.8.1 78 | pyopengl==3.1.9 79 | pyparsing==3.2.3 80 | pyroboplan==1.2.0 81 | python-dateutil==2.9.0.post0 82 | python-xlib==0.33 83 | pytz==2025.2 84 | pyyaml==6.0.2 85 | pyzmq==26.4.0 86 | scipy==1.15.2 87 | setuptools==80.7.1 88 | shimmy==2.0.0 89 | six==1.17.0 90 | stable-baselines3==2.6.0 91 | stack-data==0.6.3 92 | sympy==1.14.0 93 | tensorboard==2.19.0 94 | tensorboard-data-server==0.7.2 95 | tomli==2.2.1 96 | toppra==0.6.3 97 | torch==2.7.0 98 | tornado==6.5 99 | traitlets==5.14.3 100 | triton==3.3.0 101 | typing-extensions==4.13.2 102 | tzdata==2025.2 103 | u-msgpack-python==2.8.0 104 | wcwidth==0.2.13 105 | werkzeug==3.1.3 106 | zipp==3.21.0 107 | -------------------------------------------------------------------------------- /rl_panda.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import mujoco 3 | import gym 4 | from gym import spaces 5 | from stable_baselines3 import PPO 6 | from stable_baselines3.common.env_util import make_vec_env 7 | import torch.nn as nn 8 | import warnings 9 | import torch 10 | import mujoco.viewer 11 | 12 | # 忽略特定警告 13 | warnings.filterwarnings("ignore", category=UserWarning, module="stable_baselines3.common.on_policy_algorithm") 14 | 15 | class PandaEnv(gym.Env): 16 | def __init__(self): 17 | super(PandaEnv, self).__init__() 18 | self.model = mujoco.MjModel.from_xml_path( 19 | './model/franka_emika_panda/scene.xml') 20 | self.data = mujoco.MjData(self.model) 21 | self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'link7') 22 | self.handle = mujoco.viewer.launch_passive(self.model, self.data) 23 | self.handle.cam.distance = 3 24 | self.handle.cam.azimuth = 0 25 | self.handle.cam.elevation = -30 26 | 27 | # 动作空间，7个关节 28 | self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(7,)) 29 | # 观测空间，包含关节位置和目标位置 30 | self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(7 + 3,)) 31 | self.goal = np.random.uniform(-0.3, 0.3, size=3) 32 | print("goal:", self.goal) 33 | self.np_random = None 34 | 35 | def reset(self, seed=None, options=None): 36 | if seed is not None: 37 | self.seed(seed) 38 | mujoco.mj_resetData(self.model, self.data) 39 | self.goal = self.np_random.uniform(-0.3, 0.3, size=3) 40 | print("goal:", self.goal) 41 | obs = np.concatenate([self.data.qpos[:7], self.goal]) 42 | return obs, {} 43 | 44 | def step(self, action): 45 | self.data.qpos[:7] = action 46 | mujoco.mj_step(self.model, self.data) 47 | achieved_goal = self.data.body(self.end_effector_id).xpos 48 | reward = -np.linalg.norm(achieved_goal - self.goal) 49 | reward -= 0.3*self.data.ncon 50 | terminated = np.linalg.norm(achieved_goal - self.goal) < 0.01 51 | truncated = False 52 | info = {'is_success': terminated} 53 | obs = np.concatenate([self.data.qpos[:7], achieved_goal]) 54 | 55 | mujoco.mj_forward(self.model, self.data) 56 | mujoco.mj_step(self.model, self.data) 57 | self.handle.sync() 58 | 59 | return obs, reward, terminated, truncated, info 60 | 61 | def seed(self, seed=None): 62 | self.np_random = np.random.default_rng(seed) 63 | return [seed] 64 | 65 | 66 | if __name__ == "__main__": 67 | env = make_vec_env(lambda: PandaEnv(), n_envs=1) 68 | 69 | policy_kwargs = dict( 70 | activation_fn=nn.ReLU, 71 | net_arch=[dict(pi=[256, 128], vf=[256, 128])] 72 | ) 73 | 74 | model = PPO( 75 | "MlpPolicy", 76 | env, 77 | policy_kwargs=policy_kwargs, 78 | verbose=1, 79 | n_steps=2048, 80 | batch_size=64, 81 | n_epochs=10, 82 | gamma=0.99, 83 | learning_rate=3e-4, 84 | device="cuda" if torch.cuda.is_available() else "cpu", 85 | tensorboard_log="./tensorboard/" 86 | ) 87 | 88 | model.learn(total_timesteps=2048*100) 89 | model.save("panda_ppo_model") 90 | -------------------------------------------------------------------------------- /set_and_get_qvel.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import mujoco,time,threading 3 | import numpy as np 4 | import pinocchio 5 | import matplotlib 6 | # matplotlib.use('TkAgg') 7 | import matplotlib.pyplot as plt 8 | from mpl_toolkits.mplot3d import Axes3D 9 | import itertools 10 | 11 | from pyroboplan.core.utils import ( 12 | get_random_collision_free_state, 13 | extract_cartesian_poses, 14 | ) 15 | from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions 16 | from pyroboplan.ik.nullspace_components import ( 17 | joint_limit_nullspace_component, 18 | collision_avoidance_nullspace_component, 19 | ) 20 | 21 | from pyroboplan.models.panda import ( 22 | load_models, 23 | add_self_collisions, 24 | add_object_collisions, 25 | ) 26 | from pyroboplan.planning.rrt import RRTPlanner, RRTPlannerOptions 27 | from pyroboplan.trajectory.trajectory_optimization import ( 28 | CubicTrajectoryOptimization, 29 | CubicTrajectoryOptimizationOptions, 30 | ) 31 | 32 | class Test(mujoco_viewer.CustomViewer): 33 | def __init__(self, path): 34 | super().__init__(path, 3, azimuth=180, elevation=-30) 35 | self.path = path 36 | 37 | def runBefore(self): 38 | # Create models and data 39 | self.model_roboplan, self.collision_model, visual_model = load_models(use_sphere_collisions=True) 40 | add_self_collisions(self.model_roboplan, self.collision_model) 41 | add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1) 42 | 43 | data = self.model_roboplan.createData() 44 | collision_data = self.collision_model.createData() 45 | 46 | self.target_frame = "panda_hand" 47 | ignore_joint_indices = [ 48 | self.model_roboplan.getJointId("panda_finger_joint1") - 1, 49 | self.model_roboplan.getJointId("panda_finger_joint2") - 1, 50 | ] 51 | np.set_printoptions(precision=3) 52 | 53 | # Set up the IK solver 54 | options = DifferentialIkOptions( 55 | max_iters=200, 56 | max_retries=10, 57 | damping=0.0001, 58 | min_step_size=0.05, 59 | max_step_size=0.1, 60 | ignore_joint_indices=ignore_joint_indices, 61 | rng_seed=None, 62 | ) 63 | self.ik = DifferentialIk( 64 | self.model_roboplan, 65 | data=data, 66 | collision_model=self.collision_model, 67 | options=options, 68 | visualizer=None, 69 | ) 70 | self.nullspace_components = [ 71 | lambda model_roboplan, q: collision_avoidance_nullspace_component( 72 | model_roboplan, 73 | data, 74 | self.collision_model, 75 | collision_data, 76 | q, 77 | gain=1.0, 78 | dist_padding=0.05, 79 | ), 80 | lambda model_roboplan, q: joint_limit_nullspace_component( 81 | model_roboplan, q, gain=1.0, padding=0.025 82 | ), 83 | ] 84 | 85 | theta = np.pi 86 | rotation_matrix = np.array([ 87 | [1, 0, 0], 88 | [0, np.cos(theta), -np.sin(theta)], 89 | [0, np.sin(theta), np.cos(theta)] 90 | ]) 91 | 92 | q_start = self.getIk(self.random_valid_state(), rotation_matrix, [0.4, 0.0, 0.4]) 93 | q_goal = self.getIk(self.random_valid_state(), rotation_matrix, [0.3, 0.0, 0.5]) 94 | 95 | while True: 96 | # Search for a path 97 | options = RRTPlannerOptions( 98 | max_step_size=0.05, 99 | max_connection_dist=0.5, 100 | rrt_connect=False, 101 | bidirectional_rrt=False, 102 | rrt_star=True, 103 | max_rewire_dist=0.5, 104 | max_planning_time=5.0, 105 | fast_return=True, 106 | goal_biasing_probability=0.25, 107 | collision_distance_padding=0.0001, 108 | ) 109 | print("") 110 | print(f"Planning a path...") 111 | planner = RRTPlanner(self.model_roboplan, self.collision_model, options=options) 112 | 113 | q_path = planner.plan(q_start, q_goal) 114 | print(f"Path found with {len(q_path)} waypoints") 115 | if q_path is not None and len(q_path) > 0: 116 | print(f"Got a path with {len(q_path)} waypoints") 117 | if len(q_path) > 50: 118 | print("Path is too long, skipping...") 119 | continue 120 | else: 121 | print("Failed to plan.") 122 | 123 | # Perform trajectory optimization. 124 | # self.model.opt.timestep = 0.1 125 | dt = self.model.opt.timestep 126 | options = CubicTrajectoryOptimizationOptions( 127 | num_waypoints=len(q_path), 128 | samples_per_segment=1, 129 | min_segment_time=0.5, 130 | max_segment_time=10.0, 131 | min_vel=-1.5, 132 | max_vel=1.5, 133 | min_accel=-0.75, 134 | max_accel=0.75, 135 | min_jerk=-1.0, 136 | max_jerk=1.0, 137 | max_planning_time=1.0, 138 | check_collisions=False, 139 | min_collision_dist=0.001, 140 | collision_influence_dist=0.05, 141 | collision_avoidance_cost_weight=0.0, 142 | collision_link_list=[ 143 | "obstacle_box_1", 144 | "obstacle_box_2", 145 | "obstacle_sphere_1", 146 | "obstacle_sphere_2", 147 | "ground_plane", 148 | "panda_hand", 149 | ], 150 | ) 151 | print("Optimizing the path...") 152 | optimizer = CubicTrajectoryOptimization(self.model_roboplan, self.collision_model, options) 153 | traj = optimizer.plan([q_path[0], q_path[-1]], init_path=q_path) 154 | 155 | if traj is None: 156 | print("Retrying with all the RRT waypoints...") 157 | traj = optimizer.plan(q_path, init_path=q_path) 158 | 159 | if traj is not None: 160 | print("Trajectory optimization successful") 161 | traj_gen = traj.generate(dt) 162 | self.q_vec = traj_gen[1] 163 | print(f"path has {self.q_vec.shape[1]} points") 164 | self.tforms = extract_cartesian_poses(self.model_roboplan, "panda_hand", self.q_vec.T) 165 | self.plot(self.tforms) 166 | self.joint_velocities = np.zeros((self.q_vec.shape[1], 7)) 167 | break 168 | self.index = 0 169 | self.last_qpos = self.data.qpos[:7] 170 | 171 | def plot(self, tfs): 172 | positions = [] 173 | for tform in tfs: 174 | position = tform.translation 175 | positions.append(position) 176 | positions = np.array(positions) 177 | fig = plt.figure() 178 | ax = fig.add_subplot(111, projection='3d') 179 | ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], marker='o') 180 | 181 | ax.set_xlabel('X') 182 | ax.set_ylabel('Y') 183 | ax.set_zlabel('Z') 184 | 185 | plt.show(block=False) 186 | plt.pause(0.001) 187 | 188 | def getIk(self, init_state, rotation_matrix, pose): 189 | while True: 190 | self.init_state = init_state 191 | target_tform = pinocchio.SE3(rotation_matrix, np.array(pose)) 192 | q_sol = self.ik.solve( 193 | self.target_frame, 194 | target_tform, 195 | init_state=self.init_state, 196 | nullspace_components=self.nullspace_components, 197 | verbose=True, 198 | ) 199 | if q_sol is not None: 200 | print("IK solution found") 201 | return q_sol 202 | 203 | def random_valid_state(self): 204 | return get_random_collision_free_state( 205 | self.model_roboplan, self.collision_model, distance_padding=0.001 206 | ) 207 | 208 | def runFunc(self): 209 | calc_qvel = (self.last_qpos - self.q_vec[:7, self.index]) / self.model.opt.timestep 210 | self.data.qpos[:7] = self.q_vec[:7, self.index] 211 | self.last_qpos = self.data.qpos[:7] 212 | print(f"qpos:{self.data.qpos[:7]}") 213 | print(f"qvel:{self.data.qvel[:7]}") 214 | print(f"calc_qvel:{calc_qvel}") 215 | print(self.model.nv, self.model.nq) 216 | self.joint_velocities[self.index] = self.data.qvel[:7] 217 | self.index += 1 218 | if self.index >= self.q_vec.shape[1]: 219 | self.index = 0 220 | time_steps = np.arange(self.q_vec.shape[1]) 221 | fig, axes = plt.subplots(7, 1, figsize=(10, 10)) 222 | for j in range(7): 223 | axes[j].plot(time_steps, self.joint_velocities[:, j], label=f'Joint {j + 1}', color=f'C{j}') 224 | axes[j].set_ylabel('Angular Velocity (rad/s)') 225 | axes[j].set_title(f'Joint {j + 1} Angular Velocity') 226 | axes[j].grid(True) 227 | axes[j].legend() 228 | plt.tight_layout() 229 | plt.show() 230 | 231 | if __name__ == "__main__": 232 | test = Test("./model/franka_emika_panda/scene.xml") 233 | test.run_loop() 234 | 235 | -------------------------------------------------------------------------------- /test_pinocchio.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | from sys import argv 3 | 4 | import pinocchio 5 | 6 | # Load the urdf model 7 | model = pinocchio.buildModelFromUrdf("model/franka_panda_description/robots/panda_arm.urdf") 8 | print("model name: " + model.name) 9 | 10 | # Create data required by the algorithms 11 | data = model.createData() 12 | 13 | # Sample a random configuration 14 | q = pinocchio.randomConfiguration(model) 15 | print(f"q: {q.T}") 16 | 17 | # Perform the forward kinematics over the kinematic tree 18 | pinocchio.forwardKinematics(model, data, q) 19 | 20 | # Print out the placement of each joint of the kinematic tree 21 | for name, oMi in zip(model.names, data.oMi): 22 | print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) -------------------------------------------------------------------------------- /test_pyroboplan.py: -------------------------------------------------------------------------------- 1 | import mujoco_viewer 2 | import mujoco,time 3 | import numpy as np 4 | import pinocchio 5 | 6 | from pyroboplan.core.utils import ( 7 | get_random_collision_free_state, 8 | get_random_collision_free_transform, 9 | ) 10 | from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions 11 | from pyroboplan.ik.nullspace_components import ( 12 | joint_limit_nullspace_component, 13 | collision_avoidance_nullspace_component, 14 | ) 15 | from pyroboplan.models.panda import ( 16 | load_models, 17 | add_self_collisions, 18 | add_object_collisions, 19 | ) 20 | 21 | class Test(mujoco_viewer.CustomViewer): 22 | def __init__(self, path): 23 | super().__init__(path, 3, azimuth=-45, elevation=-30) 24 | self.path = path 25 | 26 | def runBefore(self): 27 | # Create models and data 28 | self.model_roboplan, self.collision_model, visual_model = load_models() 29 | add_self_collisions(self.model_roboplan, self.collision_model) 30 | add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1) 31 | 32 | data = self.model_roboplan.createData() 33 | collision_data = self.collision_model.createData() 34 | 35 | self.target_frame = "panda_hand" 36 | ignore_joint_indices = [ 37 | self.model_roboplan.getJointId("panda_finger_joint1") - 1, 38 | self.model_roboplan.getJointId("panda_finger_joint2") - 1, 39 | ] 40 | np.set_printoptions(precision=3) 41 | # Set up the IK solver 42 | options = DifferentialIkOptions( 43 | max_iters=200, 44 | max_retries=10, 45 | damping=0.0001, 46 | min_step_size=0.05, 47 | max_step_size=0.1, 48 | ignore_joint_indices=ignore_joint_indices, 49 | rng_seed=None, 50 | ) 51 | self.ik = DifferentialIk( 52 | self.model_roboplan, 53 | data=data, 54 | collision_model=self.collision_model, 55 | options=options, 56 | visualizer=None, 57 | ) 58 | self.nullspace_components = [ 59 | lambda model_roboplan, q: collision_avoidance_nullspace_component( 60 | model_roboplan, 61 | data, 62 | self.collision_model, 63 | collision_data, 64 | q, 65 | gain=1.0, 66 | dist_padding=0.05, 67 | ), 68 | lambda model_roboplan, q: joint_limit_nullspace_component( 69 | model_roboplan, q, gain=0.1, padding=0.025 70 | ), 71 | ] 72 | self.x = 0.29 73 | self.init_state = self.data.qpos.copy() 74 | 75 | def runFunc(self): 76 | self.init_state = get_random_collision_free_state(self.model_roboplan, self.collision_model) 77 | # target_tform = get_random_collision_free_transform( 78 | # self.model_roboplan, 79 | # self.collision_model, 80 | # self.target_frame, 81 | # joint_padding=0.05, 82 | # ) 83 | 84 | theta = np.pi 85 | rotation_matrix = np.array([ 86 | [1, 0, 0], 87 | [0, np.cos(theta), -np.sin(theta)], 88 | [0, np.sin(theta), np.cos(theta)] 89 | ]) 90 | # quat = [0.29, 0.79, -0.50, 0.13] 91 | # rotation_matrix = pinocchio.Quaternion(*quat).matrix() 92 | 93 | target_tform = pinocchio.SE3(rotation_matrix, np.array([self.x, -0.0, 0.69])) 94 | 95 | # print(target_tform) 96 | q_sol = self.ik.solve( 97 | self.target_frame, 98 | target_tform, 99 | init_state=self.init_state, 100 | nullspace_components=self.nullspace_components, 101 | verbose=True, 102 | ) 103 | # self.init_state = self.data.qpos.copy() 104 | if q_sol is not None: 105 | self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'hand') 106 | print(f"End effector position: {self.data.body(self.end_effector_id).xpos}") 107 | print(f"q_sol: {q_sol}") 108 | self.data.qpos[:7] = q_sol[:7] 109 | self.x += 0.001 110 | else: 111 | print("No solution found.") 112 | time.sleep(0.01) 113 | 114 | if __name__ == "__main__": 115 | test = Test("./model/franka_emika_panda/scene.xml") 116 | test.run_loop() 117 | 118 | -------------------------------------------------------------------------------- /test_why_continuous_2q.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | # 定义角度 4 | angles = [170, -190, 180, -180, 180, 540] 5 | 6 | # 遍历角度列表 7 | for theta in angles: 8 | # 将角度转换为弧度 9 | theta_rad = math.radians(theta) 10 | 11 | # 计算 cos 和 sin 值 12 | cos_theta = math.cos(theta_rad) 13 | sin_theta = math.sin(theta_rad) 14 | 15 | # 输出结果 16 | print(f"当 theta = {theta} 度时:") 17 | print(f"cos(theta) = {cos_theta}") 18 | print(f"sin(theta) = {sin_theta}") 19 | print() 20 | -------------------------------------------------------------------------------- /trajectory_plan_toppra.py: -------------------------------------------------------------------------------- 1 | from scipy.optimize import minimize 2 | import numpy as np 3 | from numpy.linalg import norm, solve 4 | import toppra as ta 5 | import toppra.constraint as constraint 6 | import toppra.algorithm as algo 7 | import pinocchio 8 | import time 9 | import mujoco_viewer 10 | 11 | class Test(mujoco_viewer.CustomViewer): 12 | def __init__(self, path): 13 | super().__init__(path, 3, azimuth=-45, elevation=-30) 14 | 15 | def runBefor(self): 16 | robot = pinocchio.buildModelFromUrdf('model/panda_description/urdf/panda.urdf') 17 | print('robot name: ' + robot.name) 18 | 19 | # 20 | # 21 | # 22 | # 23 | # 24 | # 25 | way_pts = [ 26 | [-1.09146e-23, 0.00126288, -3.32926e-07, -0.0696243, -2.28695e-05, 0.192135, 0.00080101, -5.53841e-09, 2.91266e-07], 27 | [0.00296359, 0.0163993, 0.00368401, -0.0788281, 0.259307, 0.192303, -0.00312336, -4.3278e-08, 1.45579e-07], 28 | [0.00498913, 0.246686, 0.00381545, -0.0800148, 0.415234, 0.193705, -0.00425587, 3.30553e-07, -3.30357e-07], 29 | [0.00602759, 0.424817, 0.00377697, -0.0799391, 0.371875, 0.193076, -0.00368281, 1.0024e-06, -1.11103e-07], 30 | [0.00773196, 0.822049, 0.00373852, -0.0797594, 0.315405, 0.192995, -0.00319569, 1.50177e-06, -9.53418e-07], 31 | [0.00840017, 1.08506, 0.00374512, -0.0796342, 0.304862, 0.193412, -0.00331262, 2.46441e-06, 2.2996e-08] 32 | ] 33 | 34 | path_scalars = np.linspace(0, 1, len(way_pts)) 35 | path = ta.SplineInterpolator(path_scalars, way_pts) 36 | vlim = np.vstack([-robot.velocityLimit, robot.velocityLimit]).T 37 | al = np.array([2,] * robot.nv) 38 | alim = np.vstack([-al, al]).T 39 | pc_vel = constraint.JointVelocityConstraint(vlim) 40 | pc_acc = constraint.JointAccelerationConstraint( 41 | alim, discretization_scheme=constraint.DiscretizationType.Interpolation) 42 | 43 | instance = ta.algorithm.TOPPRA([pc_vel, pc_acc],path,solver_wrapper="seidel") 44 | jnt_traj = instance.compute_trajectory(0, 0) 45 | ts_sample = np.linspace(0, jnt_traj.get_duration(), 1000) 46 | self.qs_sample = jnt_traj.eval(ts_sample) 47 | self.index = 0 48 | 49 | def runFunc(self): 50 | if self.index < len(self.qs_sample): 51 | self.data.qpos[:7] = self.qs_sample[self.index][:7] 52 | self.index += 1 53 | else: 54 | self.data.qpos[:7] = self.qs_sample[-1][:7] 55 | time.sleep(0.01) 56 | 57 | test = Test("model/franka_emika_panda/scene.xml") 58 | test.run_loop() -------------------------------------------------------------------------------- /uv.toml: -------------------------------------------------------------------------------- 1 | [[index]] 2 | url = "https://pypi.tuna.tsinghua.edu.cn/simple" 3 | default = true --------------------------------------------------------------------------------