├── .gitmodules ├── README.md ├── basic.py ├── basic.xml ├── google_robot.xml ├── google_robot_mocap.py ├── google_robot_scene.xml ├── mocap.py ├── mocap.xml └── mocap_move.py /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "mujoco_menagerie"] 2 | path = mujoco_menagerie 3 | url = https://github.com/google-deepmind/mujoco_menagerie 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mujoco Mocap Tutorial 2 | 3 | Tutorial on how to do mocap with mujoco. 4 | 5 | This covers the following tutorials and articles: 6 | - [Mujoco Mocap Tutorial 1: Mocap object and keyboard movement](https://mochan.org/posts/mujoco_mocap_1/) 7 | - [Mujoco Mocap Tutorial 2: Mujoco Mocap of the Google Robot](https://mochan.org/posts/mujoco-mocap-2/) 8 | -------------------------------------------------------------------------------- /basic.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import mujoco 3 | import mujoco.viewer 4 | 5 | 6 | def main(xml_file, body): 7 | # Load the mujoco model basic.xml 8 | model = mujoco.MjModel.from_xml_path(xml_file) 9 | data = mujoco.MjData(model) 10 | 11 | # Get the ID of the body we want to track 12 | body_id = model.body(body).id 13 | 14 | # Do forward kinematics 15 | mujoco.mj_kinematics(model, data) 16 | 17 | # Get the position of the body from the data 18 | body_pos = data.xpos[body_id] 19 | body_quat = data.xquat[body_id] 20 | print(body_pos, body_quat) 21 | 22 | with mujoco.viewer.launch_passive(model, data) as viewer: 23 | while viewer.is_running(): 24 | mujoco.mj_step(model, data) 25 | viewer.sync() 26 | 27 | 28 | if __name__ == '__main__': 29 | # Parse command line args. xml_file is the path to the xml file to load. 30 | parser = argparse.ArgumentParser() 31 | parser.add_argument('xml_file', help='Path to the xml file to load') 32 | parser.add_argument('body', help='Name of the body to find position of') 33 | args = parser.parse_args() 34 | 35 | main(args.xml_file, body=args.body) 36 | -------------------------------------------------------------------------------- /basic.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /google_robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 218 | -------------------------------------------------------------------------------- /google_robot_mocap.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import mujoco.viewer 3 | 4 | from mocap_move import key_callback_data 5 | 6 | 7 | def main(): 8 | # Load the mujoco model basic.xml 9 | model = mujoco.MjModel.from_xml_path('google_robot_scene.xml') 10 | data = mujoco.MjData(model) 11 | 12 | def key_callback(key): 13 | key_callback_data(key, data) 14 | 15 | with mujoco.viewer.launch_passive(model, data, key_callback=key_callback) as viewer: 16 | while viewer.is_running(): 17 | mujoco.mj_step(model, data) 18 | viewer.sync() 19 | 20 | 21 | if __name__ == '__main__': 22 | main() 23 | -------------------------------------------------------------------------------- /google_robot_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /mocap.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import mujoco.viewer 3 | 4 | 5 | def main(): 6 | # Load the mujoco model basic.xml 7 | model = mujoco.MjModel.from_xml_path('mocap.xml') 8 | data = mujoco.MjData(model) 9 | 10 | with mujoco.viewer.launch_passive(model, data) as viewer: 11 | while viewer.is_running(): 12 | mujoco.mj_step(model, data) 13 | viewer.sync() 14 | 15 | 16 | if __name__ == '__main__': 17 | main() 18 | -------------------------------------------------------------------------------- /mocap.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /mocap_move.py: -------------------------------------------------------------------------------- 1 | import mujoco 2 | import mujoco.viewer 3 | import numpy as np 4 | import pyquaternion as pyq 5 | 6 | 7 | def rotate_quaternion(quat, axis, angle): 8 | """ 9 | Rotate a quaternion by an angle around an axis 10 | """ 11 | angle_rad = np.deg2rad(angle) 12 | axis = axis / np.linalg.norm(axis) 13 | q = pyq.Quaternion(quat) 14 | q = q * pyq.Quaternion(axis=axis, angle=angle_rad) 15 | return q.elements 16 | 17 | 18 | def key_callback_data(key, data): 19 | """ 20 | Callback for key presses but with data passed in 21 | :param key: Key pressed 22 | :param data: MjData object 23 | :return: None 24 | """ 25 | if key == 265: # Up arrow 26 | data.mocap_pos[0, 2] += 0.01 27 | elif key == 264: # Down arrow 28 | data.mocap_pos[0, 2] -= 0.01 29 | elif key == 263: # Left arrow 30 | data.mocap_pos[0, 0] -= 0.01 31 | elif key == 262: # Right arrow 32 | data.mocap_pos[0, 0] += 0.01 33 | elif key == 320: # Numpad 0 34 | data.mocap_pos[0, 1] += 0.01 35 | elif key == 330: # Numpad . 36 | data.mocap_pos[0, 1] -= 0.01 37 | elif key == 260: # Insert 38 | data.mocap_quat[0] = rotate_quaternion(data.mocap_quat[0], [1, 0, 0], 10) 39 | elif key == 261: # Home 40 | data.mocap_quat[0] = rotate_quaternion(data.mocap_quat[0], [1, 0, 0], -10) 41 | elif key == 268: # Home 42 | data.mocap_quat[0] = rotate_quaternion(data.mocap_quat[0], [0, 1, 0], 10) 43 | elif key == 269: # End 44 | data.mocap_quat[0] = rotate_quaternion(data.mocap_quat[0], [0, 1, 0], -10) 45 | elif key == 266: # Page Up 46 | data.mocap_quat[0] = rotate_quaternion(data.mocap_quat[0], [0, 0, 1], 10) 47 | elif key == 267: # Page Down 48 | data.mocap_quat[0] = rotate_quaternion(data.mocap_quat[0], [0, 0, 1], -10) 49 | else: 50 | print(key) 51 | 52 | 53 | def main(): 54 | # Load the mujoco model basic.xml 55 | model = mujoco.MjModel.from_xml_path('mocap.xml') 56 | data = mujoco.MjData(model) 57 | 58 | def key_callback(key): 59 | key_callback_data(key, data) 60 | 61 | with mujoco.viewer.launch_passive(model, data, key_callback=key_callback) as viewer: 62 | while viewer.is_running(): 63 | mujoco.mj_step(model, data) 64 | viewer.sync() 65 | 66 | 67 | if __name__ == '__main__': 68 | main() 69 | --------------------------------------------------------------------------------