├── .gitignore ├── README.md ├── examples ├── demo_panda_robot.py ├── demo_task_space_control.py └── demo_test_controller.py ├── mujoco_panda ├── __init__.py ├── controllers │ ├── __init__.py │ ├── controller_base.py │ └── torque_based_controllers │ │ ├── __init__.py │ │ ├── configs.py │ │ └── os_hybrid_force_motion_controller.py ├── gravity_robot.py ├── models │ ├── franka_panda.xml │ ├── franka_panda_no_gripper.xml │ ├── franka_panda_pos.xml │ ├── franka_panda_pos_no_gripper.xml │ ├── meshes │ │ ├── collision │ │ │ ├── finger.stl │ │ │ ├── hand.stl │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ └── visual │ │ │ ├── finger.stl │ │ │ ├── hand.stl │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ ├── other │ │ └── simple_scene.xml │ ├── panda │ │ ├── actuators │ │ │ ├── pos.xml │ │ │ ├── pos_with_gripper.xml │ │ │ ├── torque.xml │ │ │ └── torque_with_gripper.xml │ │ ├── arm.xml │ │ ├── arm_with_block.xml │ │ ├── arm_with_gripper.xml │ │ ├── assets.xml │ │ ├── gripper.xml │ │ └── sensors │ │ │ └── ft_sensor.xml │ └── panda_block_table.xml ├── mujoco_robot.py ├── panda_robot.py └── utils │ ├── __init__.py │ ├── debug_utils.py │ ├── keyboard_poll.py │ ├── tf.py │ └── viewer_utils.py └── set_env.sh /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | __pycache__ 4 | 5 | oldmodels/ 6 | # mujoco_panda/utils 7 | 8 | MUJOCO_LOG.TXT 9 | 10 | tests/ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mujoco Panda 2 | 3 | - Franka Emika Panda Robot model definitions for Mujoco. 4 | - MuJoCo-based robot simulator. 5 | - Python 3 API for controlling and monitoring the simulated Franka Emika Panda Robot. 6 | - Low-level controllers: direct position and torque control. 7 | - Higher-level controller: task-space hybrid force motion controller (torque-based). 8 | 9 | Robot models are in [mujoco_panda/models](mujoco_panda/models). 10 | 11 | ## Requirements 12 | 13 | To use all functionalities of the provided library, the following dependencies have to be met. 14 | 15 | - [mujoco_py](https://github.com/openai/mujoco-py) 16 | - numpy (`pip install numpy`) 17 | - scipy (`pip install scipy`) 18 | - quaternion (`pip install numpy-quaternion`) 19 | - tkinter (`apt-get install python3-tk) (only for visualised debugging`) 20 | 21 | ## Setup Instructions 22 | 23 | Once mujoco_py is correctly installed, this library can be used by sourcing the `set_env.sh` file. 24 | 25 | ```bash 26 | source set_env.sh 27 | ``` 28 | 29 | ## Usage 30 | 31 | See `examples` for controller and API usage. See [MujocoRobot](mujoco_panda/mujoco_robot.py) and [PandaArm](mujoco_panda/panda_robot.py) for full documentation. 32 | -------------------------------------------------------------------------------- /examples/demo_panda_robot.py: -------------------------------------------------------------------------------- 1 | from mujoco_panda import PandaArm 2 | from mujoco_panda.utils.debug_utils import ParallelPythonCmd 3 | 4 | """ 5 | Testing PandaArm instance and parallel command utility 6 | 7 | # in the parallel debug interface run PandaArm arm commands to control and monitor the robot. 8 | # eg: p.set_neutral_pose(), p.joint_angles(), p.ee_pose(), etc. 9 | 10 | """ 11 | 12 | def exec_func(cmd): 13 | if cmd == '': 14 | return None 15 | a = eval(cmd) 16 | print(cmd) 17 | print(a) 18 | if a is not None: 19 | return str(a) 20 | 21 | if __name__ == "__main__": 22 | 23 | p = PandaArm.fullRobotWithTorqueActuators(render=True,compensate_gravity=True) 24 | p.start_asynchronous_run() # run simulation in separate independent thread 25 | 26 | _cth = ParallelPythonCmd(exec_func) 27 | 28 | while True: 29 | p.render() 30 | 31 | # in the parallel debug interface run PandaArm arm commands to control and monitor the robot. 32 | # eg: p.set_neutral_pose(), p.joint_angles(), p.ee_pose(), etc. 33 | -------------------------------------------------------------------------------- /examples/demo_task_space_control.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import threading 4 | import mujoco_py 5 | import quaternion 6 | import numpy as np 7 | from mujoco_panda import PandaArm 8 | from mujoco_panda.utils.tf import quatdiff_in_euler 9 | from mujoco_panda.utils.viewer_utils import render_frame 10 | 11 | """ 12 | Simplified demo of task-space control using joint torque actuation. 13 | 14 | Robot moves its end-effector 10cm upwards (+ve Z axis) from starting position. 15 | """ 16 | 17 | # --------- Modify as required ------------ 18 | # Task-space controller parameters 19 | # stiffness gains 20 | P_pos = 1500. 21 | P_ori = 200. 22 | # damping gains 23 | D_pos = 2.*np.sqrt(P_pos) 24 | D_ori = 1. 25 | # ----------------------------------------- 26 | 27 | 28 | def compute_ts_force(curr_pos, curr_ori, goal_pos, goal_ori, curr_vel, curr_omg): 29 | delta_pos = (goal_pos - curr_pos).reshape([3, 1]) 30 | delta_ori = quatdiff_in_euler(curr_ori, goal_ori).reshape([3, 1]) 31 | # print 32 | F = np.vstack([P_pos*(delta_pos), P_ori*(delta_ori)]) - \ 33 | np.vstack([D_pos*(curr_vel).reshape([3, 1]), 34 | D_ori*(curr_omg).reshape([3, 1])]) 35 | 36 | error = np.linalg.norm(delta_pos) + np.linalg.norm(delta_ori) 37 | 38 | return F, error 39 | 40 | 41 | def controller_thread(ctrl_rate): 42 | global p, target_pos 43 | 44 | threshold = 0.005 45 | 46 | target_pos = curr_ee.copy() 47 | while run_controller: 48 | 49 | error = 100. 50 | while error > threshold: 51 | now_c = time.time() 52 | curr_pos, curr_ori = p.ee_pose() 53 | curr_vel, curr_omg = p.ee_velocity() 54 | 55 | target_pos[2] = z_target 56 | F, error = compute_ts_force( 57 | curr_pos, curr_ori, target_pos, original_ori, curr_vel, curr_omg) 58 | 59 | if error <= threshold: 60 | break 61 | 62 | impedance_acc_des = np.dot(p.jacobian().T, F).flatten().tolist() 63 | 64 | p.set_joint_commands(impedance_acc_des, compensate_dynamics=True) 65 | 66 | p.step(render=False) 67 | 68 | elapsed_c = time.time() - now_c 69 | sleep_time_c = (1./ctrl_rate) - elapsed_c 70 | if sleep_time_c > 0.0: 71 | time.sleep(sleep_time_c) 72 | 73 | 74 | if __name__ == "__main__": 75 | 76 | p = PandaArm.withTorqueActuators(render=True, compensate_gravity=True) 77 | 78 | ctrl_rate = 1/p.model.opt.timestep 79 | 80 | render_rate = 100 81 | 82 | p.set_neutral_pose() 83 | p.step() 84 | time.sleep(0.01) 85 | 86 | new_pose = p._sim.data.qpos.copy()[:7] 87 | 88 | curr_ee, original_ori = p.ee_pose() 89 | 90 | target_z_traj = np.linspace(curr_ee[2], curr_ee[2]+0.1, 25).tolist() 91 | z_target = curr_ee[2] 92 | 93 | target_pos = curr_ee 94 | run_controller = True 95 | ctrl_thread = threading.Thread(target=controller_thread, args=[ctrl_rate]) 96 | ctrl_thread.start() 97 | 98 | now_r = time.time() 99 | i = 0 100 | while i < len(target_z_traj): 101 | z_target = target_z_traj[i] 102 | 103 | robot_pos, robot_ori = p.ee_pose() 104 | elapsed_r = time.time() - now_r 105 | 106 | if elapsed_r >= 0.1: 107 | i += 1 108 | now_r = time.time() 109 | render_frame(p.viewer, robot_pos, robot_ori) 110 | render_frame(p.viewer, target_pos, original_ori, alpha=0.2) 111 | 112 | p.render() 113 | 114 | print("Done controlling. Press Ctrl+C to quit.") 115 | 116 | while True: 117 | robot_pos, robot_ori = p.ee_pose() 118 | render_frame(p.viewer, robot_pos, robot_ori) 119 | render_frame(p.viewer, target_pos, original_ori, alpha=0.2) 120 | p.render() 121 | 122 | run_controller = False 123 | ctrl_thread.join() 124 | -------------------------------------------------------------------------------- /examples/demo_test_controller.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import mujoco_py 4 | import numpy as np 5 | from mujoco_panda import PandaArm 6 | from mujoco_panda.utils.viewer_utils import render_frame 7 | from mujoco_panda.utils.debug_utils import ParallelPythonCmd 8 | from mujoco_panda.controllers.torque_based_controllers import OSHybridForceMotionController 9 | 10 | 11 | def exec_func(cmd): 12 | if cmd == '': 13 | return None 14 | a = eval(cmd) 15 | print(cmd) 16 | print(a) 17 | if a is not None: 18 | return str(a) 19 | 20 | MODEL_PATH = os.environ['MJ_PANDA_PATH'] + \ 21 | '/mujoco_panda/models/' 22 | 23 | 24 | # controller parameters 25 | KP_P = np.array([7000., 7000., 7000.]) 26 | KP_O = np.array([3000., 3000., 3000.]) 27 | ctrl_config = { 28 | 'kp_p': KP_P, 29 | 'kd_p': 2.*np.sqrt(KP_P), 30 | 'kp_o': KP_O, # 10gains for orientation 31 | 'kd_o': [1., 1., 1.], # gains for orientation 32 | 'kp_f': [1., 1., 1.], # gains for force 33 | 'kd_f': [0., 0., 0.], # 25gains for force 34 | 'kp_t': [1., 1., 1.], # gains for torque 35 | 'kd_t': [0., 0., 0.], # gains for torque 36 | 'alpha': 3.14*0, 37 | 'use_null_space_control': True, 38 | 'ft_dir': [0, 0, 0, 0, 0, 0], 39 | # newton meter 40 | 'null_kp': [5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0], 41 | 'null_kd': 0, 42 | 'null_ctrl_wt': 2.5, 43 | 'use_orientation_ctrl': True, 44 | 'linear_error_thr': 0.025, 45 | 'angular_error_thr': 0.01, 46 | } 47 | 48 | if __name__ == "__main__": 49 | p = PandaArm(model_path=MODEL_PATH+'panda_block_table.xml', 50 | render=True, compensate_gravity=False, smooth_ft_sensor=True) 51 | 52 | if mujoco_py.functions.mj_isPyramidal(p.model): 53 | print("Type of friction cone is pyramidal") 54 | else: 55 | print("Type of friction cone is eliptical") 56 | 57 | # cmd = ParallelPythonCmd(exec_func) 58 | p.set_neutral_pose() 59 | p.step() 60 | time.sleep(1.0) 61 | 62 | # create controller instance with default controller gains 63 | ctrl = OSHybridForceMotionController(p, config=ctrl_config) 64 | 65 | # --- define trajectory in position ----- 66 | curr_ee, curr_ori = p.ee_pose() 67 | goal_pos_1 = curr_ee.copy() 68 | goal_pos_1[2] = 0.58 69 | goal_pos_1[0] += 0.15 70 | goal_pos_1[1] -= 0.2 71 | target_traj_1 = np.linspace(curr_ee, goal_pos_1, 100) 72 | z_target = curr_ee[2] 73 | target_ori = np.asarray([0, -0.924, -0.383, 0], dtype=np.float64) 74 | goal_pos_2 = target_traj_1[-1, :].copy() 75 | goal_pos_2[2] = 0.55 76 | wait_traj = np.asarray([target_traj_1[-1, :], ]*30) 77 | target_traj_2 = np.linspace(target_traj_1[-1, :], np.asarray( 78 | [goal_pos_2[0], goal_pos_2[1], goal_pos_2[2]]), 20) 79 | goal_pos_3 = target_traj_2[-1, :].copy() 80 | goal_pos_3[1] += 0.4 81 | target_traj_3 = np.linspace(target_traj_2[-1, :], goal_pos_3, 200) 82 | target_traj = np.vstack( 83 | [target_traj_1, wait_traj, target_traj_2, target_traj_3]) 84 | # -------------------------------------- 85 | 86 | ctrl.set_active(True) # activate controller (simulation step and controller thread now running) 87 | 88 | now_r = time.time() 89 | i = 0 90 | count = 0 91 | while i < target_traj.shape[0]: 92 | # get current robot end-effector pose 93 | robot_pos, robot_ori = p.ee_pose() 94 | 95 | elapsed_r = time.time() - now_r 96 | 97 | # render controller target and current ee pose using frames 98 | render_frame(p.viewer, robot_pos, robot_ori) 99 | render_frame(p.viewer, target_traj[i, :], target_ori, alpha=0.2) 100 | 101 | if i == 130: # activate force control when the robot is near the table 102 | ctrl.change_ft_dir([0,0,1,0,0,0]) # start force control along Z axis 103 | ctrl.set_goal(target_traj[i, :], 104 | target_ori, goal_force=[0, 0, -25]) # target force in cartesian frame 105 | else: 106 | ctrl.set_goal(target_traj[i, :], target_ori) 107 | 108 | if elapsed_r >= 0.1: 109 | i += 1 # change target less frequently compared to render rate 110 | print ("smoothed FT reading: ", p.get_ft_reading(pr=True)) 111 | now_r = time.time() 112 | 113 | p.render() # render the visualisation 114 | 115 | input("Trajectory complete. Hit Enter to deactivate controller") 116 | ctrl.set_active(False) 117 | ctrl.stop_controller_cleanly() 118 | -------------------------------------------------------------------------------- /mujoco_panda/__init__.py: -------------------------------------------------------------------------------- 1 | from .panda_robot import PandaArm 2 | -------------------------------------------------------------------------------- /mujoco_panda/controllers/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/controllers/__init__.py -------------------------------------------------------------------------------- /mujoco_panda/controllers/controller_base.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import threading 3 | import logging 4 | import time 5 | import numpy as np 6 | 7 | LOG_LEVEL = "DEBUG" 8 | 9 | class ControllerBase(object): 10 | """ 11 | Base class for joint controllers 12 | """ 13 | 14 | def __init__(self, robot_object, config={}): 15 | 16 | self._robot = robot_object 17 | 18 | logging.basicConfig(format='\n{}: %(levelname)s: %(message)s\n'.format( 19 | self.__class__.__name__), level=LOG_LEVEL) 20 | self._logger = logging.getLogger(__name__) 21 | 22 | self._config = config 23 | 24 | if 'control_rate' in self._config: 25 | control_rate = self._config['control_rate'] 26 | else: 27 | control_rate = 1./self._robot.model.opt.timestep 28 | 29 | 30 | self._is_active = False 31 | 32 | self._cmd = self._robot.sim.data.ctrl[self._robot.actuated_arm_joints].copy( 33 | ) 34 | 35 | self._mutex = threading.Lock() 36 | 37 | self._ctrl_thread = threading.Thread(target = self._send_cmd, args=[control_rate]) 38 | self._is_running = True 39 | 40 | self._ctrl_thread.start() 41 | 42 | self._error = {'linear': np.zeros(3), 'angular': np.zeros(3)} 43 | 44 | @property 45 | def is_active(self): 46 | """ 47 | Returns True if controller is active 48 | 49 | :return: State of controller 50 | :rtype: bool 51 | """ 52 | return self._is_active 53 | 54 | def set_active(self, status=True): 55 | """ 56 | Activate/deactivate controller 57 | 58 | :param status: To deactivate controller, set False. Defaults to True. 59 | :type status: bool, optional 60 | """ 61 | self._is_active = status 62 | 63 | def toggle_activate(self): 64 | """ 65 | Toggles controller state between active and inactive. 66 | """ 67 | self.set_active(status = not self._is_active) 68 | 69 | @abc.abstractmethod 70 | def _compute_cmd(self): 71 | raise NotImplementedError("Method must be implemented in child class!") 72 | 73 | @abc.abstractmethod 74 | def set_goal(self, *args, **kwargs): 75 | raise NotImplementedError("Method must be implemented in child class!") 76 | 77 | def _send_cmd(self, control_rate): 78 | """ 79 | This method runs automatically in separate thread at the specified controller 80 | rate. If controller is active, the command is computed and robot is commanded 81 | using the api method. Simulation is also stepped forward automatically. 82 | 83 | :param control_rate: rate of control loop, ideally same as simulation step rate. 84 | :type control_rate: float 85 | """ 86 | while self._is_running: 87 | now_c = time.time() 88 | if self._is_active: 89 | self._mutex.acquire() 90 | self._compute_cmd() 91 | self._robot.set_joint_commands( 92 | self._cmd, joints=self._robot.actuated_arm_joints, compensate_dynamics=False) 93 | self._robot.step(render=False) 94 | self._mutex.release() 95 | 96 | elapsed_c = time.time() - now_c 97 | sleep_time_c = (1./control_rate) - elapsed_c 98 | if sleep_time_c > 0.0: 99 | time.sleep(sleep_time_c) 100 | 101 | def stop_controller_cleanly(self): 102 | """ 103 | Method to be called when stopping controller. Stops the controller thread and exits. 104 | """ 105 | self._is_active = False 106 | self._logger.info ("Stopping controller commands; removing ctrl values.") 107 | self._robot.set_joint_commands(np.zeros_like(self._robot.actuated_arm_joints),self._robot.actuated_arm_joints) 108 | self._robot._ignore_grav_comp=False 109 | self._logger.info ("Stopping controller thread. WARNING: PandaArm->step() method has to be called separately to continue simulation.") 110 | self._is_running = False 111 | self._ctrl_thread.join() 112 | -------------------------------------------------------------------------------- /mujoco_panda/controllers/torque_based_controllers/__init__.py: -------------------------------------------------------------------------------- 1 | from .os_hybrid_force_motion_controller import OSHybridForceMotionController 2 | -------------------------------------------------------------------------------- /mujoco_panda/controllers/torque_based_controllers/configs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | KP_P = np.array([15000., 15000., 15000.]) 4 | KP_O = np.array([7000., 7000., 7000.]) 5 | BASIC_HYB_CONFIG = { 6 | 'kp_p': KP_P, 7 | 'kd_p': 2.*np.sqrt(KP_P), 8 | 'kp_o': KP_O, # 10gains for orientation 9 | 'kd_o': [1., 1., 1.], # gains for orientation 10 | 'kp_f': [1., 1., 1.], # gains for force 11 | 'kd_f': [0., 0., 0.], # 25gains for force 12 | 'kp_t': [1., 1., 1.], # gains for torque 13 | 'kd_t': [0., 0., 0.], # gains for torque 14 | 'alpha': 3.14*0, 15 | 'use_null_space_control': True, 16 | 'ft_dir': [0, 0, 0, 0, 0, 0], 17 | # newton meter 18 | 'null_kp': [5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0], 19 | 'null_kd': 0, 20 | 'null_ctrl_wt': 2.5, 21 | 'use_orientation_ctrl': True, 22 | 'linear_error_thr': 0.025, 23 | 'angular_error_thr': 0.01, 24 | } 25 | -------------------------------------------------------------------------------- /mujoco_panda/controllers/torque_based_controllers/os_hybrid_force_motion_controller.py: -------------------------------------------------------------------------------- 1 | from mujoco_panda.controllers.controller_base import ControllerBase 2 | from mujoco_panda.utils.tf import quatdiff_in_euler 3 | from .configs import BASIC_HYB_CONFIG 4 | import numpy as np 5 | 6 | class OSHybridForceMotionController(ControllerBase): 7 | """ 8 | Torque-based task-space hybrid force motion controller. 9 | Computes the joint torques required for achieving a desired 10 | end-effector pose and/or wrench. Goal values and directions 11 | are defined in cartesian coordinates. 12 | 13 | First computes cartesian force for achieving the goal using PD 14 | control law, then computes the corresponding joint torques using 15 | :math:`\tau = J^T F`. 16 | 17 | """ 18 | 19 | def __init__(self, robot_object, config=BASIC_HYB_CONFIG, control_rate=None, *args, **kwargs): 20 | """ 21 | contstructor 22 | 23 | :param robot_object: the :py:class:`PandaArm` object to be controlled 24 | :type robot_object: PandaArm 25 | :param config: dictionary of controller parameters, defaults to 26 | BASIC_HYB_CONFIG (see config for reference) 27 | :type config: dict, optional 28 | """ 29 | if control_rate is not None: 30 | config['control_rate'] = control_rate 31 | 32 | super(OSHybridForceMotionController, 33 | self).__init__(robot_object, config) 34 | 35 | self._goal_pos, self._goal_ori = self._robot.ee_pose() 36 | self._goal_vel, self._goal_omg = np.zeros(3), np.zeros(3) 37 | self._goal_force, self._goal_torque = np.zeros( 38 | 3), np.zeros(3) 39 | 40 | # gains for position 41 | self._kp_p = np.diag(self._config['kp_p']) 42 | self._kd_p = np.diag(self._config['kd_p']) 43 | 44 | # gains for orientation 45 | self._kp_o = np.diag(self._config['kp_o']) 46 | self._kd_o = np.diag(self._config['kd_o']) 47 | 48 | # gains for force 49 | self._kp_f = np.diag(self._config['kp_f']) 50 | self._kd_f = np.diag(self._config['kd_f']) 51 | 52 | # gains for torque 53 | self._kp_t = np.diag(self._config['kp_t']) 54 | self._kd_t = np.diag(self._config['kd_t']) 55 | 56 | self._force_dir = np.diag(self._config['ft_dir'][:3]) 57 | self._torque_dir = np.diag(self._config['ft_dir'][3:]) 58 | 59 | self._pos_p_dir = np.diag([1, 1, 1]) ^ self._force_dir 60 | self._pos_o_dir = np.diag([1, 1, 1]) ^ self._torque_dir 61 | 62 | self._use_null_ctrl = self._config['use_null_space_control'] 63 | 64 | if self._use_null_ctrl: 65 | 66 | self._null_Kp = np.diag(self._config['null_kp']) 67 | 68 | self._null_ctrl_wt = self._config['null_ctrl_wt'] 69 | 70 | self._pos_threshold = self._config['linear_error_thr'] 71 | 72 | self._angular_threshold = self._config['angular_error_thr'] 73 | 74 | def set_active(self, status=True): 75 | """ 76 | Override parent method to reset goal values 77 | 78 | :param status: To deactivate controller, set False. Defaults to True. 79 | :type status: bool, optional 80 | """ 81 | if status: 82 | self._goal_pos, self._goal_ori = self._robot.ee_pose() 83 | self._goal_vel, self._goal_omg = np.zeros(3), np.zeros(3) 84 | self._goal_force, self._goal_torque = np.zeros( 85 | 3), np.zeros(3) 86 | self._is_active = status 87 | 88 | def _compute_cmd(self): 89 | """ 90 | Actual computation of command given the desired goal states 91 | 92 | :return: computed joint torque values 93 | :rtype: np.ndarray (7,) 94 | """ 95 | # calculate the jacobian of the end effector 96 | jac_ee = self._robot.jacobian() 97 | 98 | # get position of the end-effector 99 | curr_pos, curr_ori = self._robot.ee_pose() 100 | curr_vel, curr_omg = self._robot.ee_velocity() 101 | 102 | # current end-effector force and torque measured 103 | curr_force, curr_torque = self._robot.get_ft_reading() 104 | 105 | # error values 106 | delta_pos = self._goal_pos - curr_pos 107 | delta_vel = self._goal_vel - curr_vel 108 | delta_force = self._force_dir.dot(self._goal_force - curr_force) 109 | delta_torque = self._torque_dir.dot(self._goal_torque - curr_torque) 110 | 111 | if self._goal_ori is not None: 112 | delta_ori = quatdiff_in_euler(curr_ori, self._goal_ori) 113 | else: 114 | delta_ori = np.zeros(delta_pos.shape) 115 | 116 | delta_ori = self._pos_o_dir.dot(delta_ori) 117 | 118 | delta_omg = self._pos_o_dir.dot(self._goal_omg - curr_omg) 119 | 120 | # compute force control part along force dimensions # negative sign to convert from experienced to applied 121 | force_control = -self._force_dir.dot(self._kp_f.dot(delta_force) - np.abs(self._kd_f.dot(delta_vel)) + self._goal_force) 122 | # compute position control force along position dimensions (orthogonal to force dims) 123 | position_control = self._pos_p_dir.dot( 124 | self._kp_p.dot(delta_pos) + self._kd_p.dot(delta_vel)) 125 | 126 | # total cartesian force at end-effector 127 | x_des = position_control + force_control 128 | 129 | if self._goal_ori is not None: # orientation conttrol 130 | 131 | if np.linalg.norm(delta_ori) < self._angular_threshold: 132 | delta_ori = np.zeros(delta_ori.shape) 133 | delta_omg = np.zeros(delta_omg.shape) 134 | 135 | # compute orientation control force along orientation dimensions 136 | ori_pos_ctrl = self._pos_o_dir.dot( 137 | self._kp_o.dot(delta_ori) + self._kd_o.dot(delta_omg)) 138 | 139 | # compute torque control force along torque dimensions (orthogonal to orientation dimensions) 140 | # negative sign to convert from experienced to applied 141 | torque_f_ctrl = - self._torque_dir.dot(self._kp_t.dot( 142 | delta_torque) - self._kd_t.dot(delta_omg) + self._goal_torque) 143 | 144 | # total torque in cartesian at end-effector 145 | omg_des = ori_pos_ctrl + torque_f_ctrl 146 | 147 | else: 148 | 149 | omg_des = np.zeros(3) 150 | 151 | f_ee = np.hstack([x_des, omg_des]) # Desired end-effector force 152 | 153 | u = np.dot(jac_ee.T, f_ee) # desired joint torque 154 | 155 | if np.any(np.isnan(u)): 156 | u = self._cmd 157 | else: 158 | self._cmd = u 159 | 160 | if self._use_null_ctrl: # null-space control, if required 161 | 162 | null_space_filter = self._null_Kp.dot( 163 | np.eye(7) - jac_ee.T.dot(np.linalg.pinv(jac_ee.T, rcond=1e-3))) 164 | 165 | # add null-space torque in the null-space projection of primary task 166 | self._cmd = self._cmd + \ 167 | null_space_filter.dot( 168 | self._robot._neutral_pose-self._robot.joint_positions()[:7]) 169 | 170 | # update the error 171 | self._error = {'linear': delta_pos, 'angular': delta_ori} 172 | 173 | return self._cmd 174 | 175 | def set_goal(self, goal_pos, goal_ori=None, goal_vel=np.zeros(3), goal_omg=np.zeros(3), goal_force=None, goal_torque=None): 176 | """ 177 | change the target for the controller 178 | """ 179 | self._mutex.acquire() 180 | self._goal_pos = goal_pos 181 | self._goal_ori = goal_ori 182 | self._goal_vel = goal_vel 183 | self._goal_omg = goal_omg 184 | if goal_force is not None: 185 | self._goal_force = - np.asarray(goal_force) # applied force = - experienced force 186 | if goal_torque is not None: 187 | self._goal_torque = - np.asarray(goal_torque) # applied torque = - experienced torque 188 | self._mutex.release() 189 | 190 | def change_ft_dir(self, directions): 191 | """ 192 | Change directions along which force/torque control is performed. 193 | 194 | :param directions: 6 binary values for [x,y,z,x_rot,y_rot,z_rot], 195 | 1 indicates force direction, 0 position. Eg: [0,0,1,0,0,1] 196 | means force control is along the cartesian Z axis and (torque) 197 | about the Z axis, while other dimensions are position (and 198 | orientation) controlled. 199 | :type directions: [int] * 6 200 | """ 201 | self._mutex.acquire() 202 | self._force_dir = np.diag(directions[:3]) 203 | 204 | self._torque_dir = np.diag(directions[3:]) 205 | 206 | self._pos_p_dir = np.diag([1, 1, 1]) ^ self._force_dir 207 | 208 | self._pos_o_dir = np.diag([1, 1, 1]) ^ self._torque_dir 209 | self._mutex.release() 210 | -------------------------------------------------------------------------------- /mujoco_panda/gravity_robot.py: -------------------------------------------------------------------------------- 1 | import mujoco_py as mjp 2 | 3 | class GravityRobot(object): 4 | """ 5 | Robot instance for gravity compensation only. 6 | """ 7 | 8 | def __init__(self, model_path): 9 | self.model = mjp.load_model_from_path(model_path) 10 | self.sim = mjp.MjSim(self.model) 11 | 12 | self._controllable_joints = self.get_controllable_joints() 13 | 14 | self._all_joints = [self.model.joint_name2id( 15 | j) for j in self.model.joint_names] 16 | 17 | def get_controllable_joints(self): 18 | """ 19 | Return list of movable (actuated) joints in the given model 20 | 21 | :return: list of indices of controllable joints 22 | :rtype: [int] (size: self._nu) 23 | """ 24 | trntype = self.model.actuator_trntype # transmission type (0 == joint) 25 | # transmission id (get joint actuated) 26 | trnid = self.model.actuator_trnid 27 | 28 | mvbl_jnts = [] 29 | for i in range(trnid.shape[0]): 30 | if trntype[i] == 0 and trnid[i, 0] not in mvbl_jnts: 31 | mvbl_jnts.append(trnid[i, 0]) 32 | 33 | return sorted(mvbl_jnts) 34 | -------------------------------------------------------------------------------- /mujoco_panda/models/franka_panda.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mujoco_panda/models/franka_panda_no_gripper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mujoco_panda/models/franka_panda_pos.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mujoco_panda/models/franka_panda_pos_no_gripper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/finger.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/hand.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link0.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link1.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link2.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link3.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link4.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link5.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link6.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/collision/link7.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/finger.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/hand.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link0.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link1.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link2.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link3.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link4.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link5.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link6.stl -------------------------------------------------------------------------------- /mujoco_panda/models/meshes/visual/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/models/meshes/visual/link7.stl -------------------------------------------------------------------------------- /mujoco_panda/models/other/simple_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/actuators/pos.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/actuators/pos_with_gripper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/actuators/torque.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/actuators/torque_with_gripper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/arm.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/arm_with_block.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/arm_with_gripper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/assets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/gripper.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda/sensors/ft_sensor.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /mujoco_panda/models/panda_block_table.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /mujoco_panda/mujoco_robot.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import time 3 | import logging 4 | import threading 5 | import quaternion 6 | import numpy as np 7 | import mujoco_py as mjp 8 | from threading import Lock 9 | 10 | LOG_LEVEL = "DEBUG" 11 | 12 | class ContactInfo(object): 13 | def __init__(self, pos, ori, ft): 14 | self.pos = pos.copy() 15 | self.ori_mat = ori.copy() 16 | self.quat = quaternion.as_float_array(quaternion.from_rotation_matrix(self.ori_mat)) 17 | self.ft = ft.copy() 18 | 19 | def __str__(self): 20 | return "pos: {}, quat: {}, ft: {}".format(self.pos, self.quat, self.ft) 21 | 22 | class MujocoRobot(object): 23 | """ 24 | Constructor 25 | 26 | :param model_path: path to model xml file for robot 27 | :type model_path: str 28 | :param from_model: PyMjModel instance of robot. If provided, will ignore the `model_path` param. 29 | :type from_model: mjp.PyMjModel 30 | :param render: create a visualiser instance in mujoco; defaults to True. 31 | :type render: bool, optional 32 | :param prestep_callables: dictionary of callable iterms to be run before running 33 | sim.run(). Format: {'name_for_callable': [callable_handle, [list_of_arguments_for_callable]]} 34 | :type prestep_callables: {'str': [callable, [*args]]} 35 | :param poststep_callables: dictionary of callable iterms to be run after running 36 | sim.run(). Format: {'name_for_callable': [callable_handle, [list_of_arguments_for_callable]]} 37 | :type poststep_callables: {'str': [callable, [*args]]} 38 | :param config: optional values for setting end-effector and ft sensor locations. 39 | Format: {'ee_name': "name_of_body_or_site_in_model", 40 | 'ft_site': "name_of_site_with_force_torque_sensor"} 41 | :type config: {str: str} 42 | """ 43 | 44 | def __init__(self, model_path=None, render=True, config=None, prestep_callables={}, poststep_callables={}, from_model=False): 45 | 46 | logging.basicConfig(format='\n{}: %(levelname)s: %(message)s\n'.format( 47 | self.__class__.__name__), level=LOG_LEVEL) 48 | self._logger = logging.getLogger(__name__) 49 | 50 | if isinstance(from_model,mjp.cymj.PyMjModel): 51 | self._model = from_model 52 | else: 53 | self._model = mjp.load_model_from_path(model_path) 54 | 55 | self._sim = mjp.MjSim(self._model) 56 | self._viewer = mjp.MjViewer(self._sim) if render else None 57 | 58 | self._has_gripper = False # by default, assume no gripper is attached 59 | 60 | # seprate joints that are controllable, movable, etc. 61 | self._define_joint_ids() 62 | 63 | self._nu = len(self.controllable_joints) # number of actuators 64 | 65 | self._nq = len(self.qpos_joints) # total number of joints 66 | 67 | self._all_joint_names = [ 68 | self.model.joint_id2name(j) for j in self.qpos_joints] 69 | 70 | self._all_joint_dict = dict( 71 | zip(self._all_joint_names, self.qpos_joints)) 72 | 73 | if 'ee_name' in config: 74 | self.set_as_ee(config['ee_name']) 75 | else: 76 | self._ee_idx, self._ee_name = self._use_last_defined_link() 77 | self._ee_is_a_site = False 78 | 79 | if 'ft_site_name' in config: 80 | self._ft_site_name = config['ft_site_name'] 81 | else: 82 | self._ft_site_name = False 83 | 84 | self._mutex = Lock() 85 | self._asynch_thread_active = False 86 | 87 | 88 | self._forwarded = False 89 | 90 | self._pre_step_callables = prestep_callables 91 | self._post_step_callables = poststep_callables 92 | 93 | self._first_step_not_done = True 94 | 95 | def set_as_ee(self, body_name): 96 | """ 97 | Set provided body or site as the end-effector of the robot. 98 | 99 | :param body_name: name of body or site in mujoco model 100 | :type body_name: str 101 | """ 102 | self._ee_name = body_name 103 | 104 | if body_name in self._model.site_names: 105 | self._ee_is_a_site = True 106 | self._ee_idx = self._model.site_name2id(body_name) 107 | self._logger.debug( 108 | "End-effector is a site in model: {}".format(body_name)) 109 | else: 110 | self._ee_is_a_site = False 111 | self._ee_idx = self._model.body_name2id(self._ee_name) 112 | 113 | def _use_last_defined_link(self): 114 | return self._model.nbody-1, self._model.body_id2name(self._model.nbody-1) 115 | 116 | def add_pre_step_callable(self, f_dict): 117 | """ 118 | Add new values to prestep_callable dictionary. See contructor params for details. 119 | 120 | :param f_dict: {"name_of_callable": [callable_handle,[list_of_arguments_for_callable]]} 121 | :type f_dict: {str:[callable,[*args]]} 122 | """ 123 | for key in list(f_dict.keys()): 124 | if key not in self._pre_step_callables: 125 | self._pre_step_callables.update(f_dict) 126 | 127 | def add_post_step_callable(self, f_dict): 128 | """ 129 | Add new values to poststep_callable dictionary. See contructor params for details. 130 | 131 | :param f_dict: {"name_of_callable": [callable_handle,[list_of_arguments_for_callable]]} 132 | :type f_dict: {str:[callable,[*args]]} 133 | """ 134 | for key in list(f_dict.keys()): 135 | if key not in self._post_step_callables: 136 | self._post_step_callables.update(f_dict) 137 | 138 | @property 139 | def sim(self): 140 | """ 141 | Get mujoco_py.sim object associated with this instance 142 | 143 | :return: mujoco_py.sim object associated with this instance 144 | :rtype: mujoco_py.MjSim 145 | """ 146 | return self._sim 147 | 148 | @property 149 | def viewer(self): 150 | """ 151 | :return: mujoco_py.MjViewer object associated with this instance 152 | :rtype: mujoco_py.MjViewer 153 | """ 154 | return self._viewer 155 | 156 | @property 157 | def model(self): 158 | """ 159 | :return: mujoco_py.cymj.PyMjModel object associated with this instance 160 | :rtype: mujoco_py.cymj.PyMjModel 161 | """ 162 | return self._model 163 | 164 | def has_body(self, bodies): 165 | """ 166 | Check if the provided bodies exist in model. 167 | 168 | :param bodies: list of body names 169 | :type bodies: [str] 170 | :return: True if all bodies present in model 171 | :rtype: bool 172 | """ 173 | if isinstance(bodies, str): 174 | bodies = [bodies] 175 | for body in bodies: 176 | if not body in self._model.body_names: 177 | return False 178 | return True 179 | 180 | def get_ft_reading(self, in_global_frame=True): 181 | """ 182 | Return sensordata values. Assumes no other sensor is present. 183 | 184 | :return: Force torque sensor readings from the defined force torque sensor. 185 | :rtype: np.ndarray (3,), np.ndarray (3,) 186 | """ 187 | if self._model.sensor_type[0] == 4 and self._model.sensor_type[1] == 5: 188 | if not self._forwarded: 189 | self.forward_sim() 190 | sensordata = -self._sim.data.sensordata.copy() # change sign to make force relative to parent body 191 | if in_global_frame: 192 | if self._ft_site_name: 193 | new_sensordata = np.zeros(6) 194 | _, site_ori = self.site_pose( 195 | self._model.site_name2id(self._ft_site_name)) 196 | rotation_mat = quaternion.as_rotation_matrix( 197 | np.quaternion(*site_ori)) 198 | new_sensordata[:3] = np.dot( 199 | rotation_mat, np.asarray(sensordata[:3])) 200 | new_sensordata[3:] = np.dot( 201 | rotation_mat, np.asarray(sensordata[3:])) 202 | sensordata = new_sensordata.copy() 203 | else: 204 | self._logger.warning("Could not transform ft sensor values. Please\ 205 | provide ft site name in config file.") 206 | return sensordata[:3], sensordata[3:] 207 | else: 208 | self._logger.debug( 209 | "Could not find FT sensor as the first sensor in model!") 210 | return np.zeros(6) 211 | 212 | def get_contact_info(self): 213 | """ 214 | Get details about physical contacts between bodies. 215 | Includes contact point positions, orientations, contact forces. 216 | 217 | :return: list of ContactInfo objects 218 | :rtype: [ContactInfo] 219 | """ 220 | self._mutex.acquire() 221 | mjp.functions.mj_rnePostConstraint( 222 | self._sim.model, self._sim.data) 223 | 224 | nc = self._sim.data.ncon 225 | 226 | c_array = np.zeros(6, dtype=np.float64) 227 | contact_list = [] 228 | 229 | for i in range(nc): 230 | contact = self._sim.data.contact[i] 231 | c_array.fill(0) 232 | mjp.functions.mj_contactForce( 233 | self._sim.model, self._sim.data, i, c_array) 234 | 235 | ori = np.flip(contact.frame.reshape(3, 3).T, 1) 236 | 237 | # # for mujoco the normal force is along x 238 | # # so for the convention we flip X and Z 239 | # c_array = np.hstack([np.flip(c_array[:3]), 240 | # np.flip(c_array[3:])]) 241 | 242 | contact_list.append(copy.deepcopy(ContactInfo( 243 | contact.pos.copy(), ori.copy(), c_array.copy()))) 244 | 245 | assert nc == len(contact_list) 246 | 247 | self._mutex.release() 248 | 249 | return contact_list 250 | 251 | def body_jacobian(self, body_id=None, joint_indices=None, recompute=True): 252 | """ 253 | return body jacobian at current step 254 | 255 | :param body_id: id of body whose jacobian is to be computed, defaults to end-effector (set in config) 256 | :type body_id: int, optional 257 | :param joint_indices: list of joint indices, defaults to all movable joints. Final jacobian will be of 258 | shape 6 x len(joint_indices) 259 | :type joint_indices: [int], optional 260 | :param recompute: if set to True, will perform forward kinematics computation for the step and provide updated 261 | results; defaults to True 262 | :type recompute: bool 263 | :return: 6xN body jacobian 264 | :rtype: np.ndarray 265 | """ 266 | if body_id is None: 267 | body_id = self._ee_idx 268 | if self._ee_is_a_site: 269 | return self.site_jacobian(body_id, joint_indices) 270 | 271 | if joint_indices is None: 272 | joint_indices = self.controllable_joints 273 | 274 | if recompute and not self._forwarded: 275 | self.forward_sim() 276 | 277 | jacp = self._sim.data.body_jacp[body_id, :].reshape(3, -1) 278 | jacr = self._sim.data.body_jacr[body_id, :].reshape(3, -1) 279 | 280 | return np.vstack([jacp[:, joint_indices], jacr[:, joint_indices]]) 281 | 282 | def site_jacobian(self, site_id, joint_indices=None, recompute=True): 283 | """ 284 | Return jacobian computed for a site defined in model 285 | 286 | :param site_id: index of the site 287 | :type site_id: int 288 | :param joint_indices: list of joint indices, defaults to all movable joints. Final jacobian will be of 289 | shape 6 x len(joint_indices) 290 | :type joint_indices: [int] 291 | :param recompute: if set to True, will perform forward kinematics computation for the step and provide updated 292 | results; defaults to True 293 | :type recompute: bool 294 | :return: 6xN jacobian 295 | :rtype: np.ndarray 296 | """ 297 | if joint_indices is None: 298 | joint_indices = self.controllable_joints 299 | 300 | if recompute and not self._forwarded: 301 | self.forward_sim() 302 | 303 | jacp = self._sim.data.site_jacp[site_id, :].reshape(3, -1) 304 | jacr = self._sim.data.site_jacr[site_id, :].reshape(3, -1) 305 | return np.vstack([jacp[:, joint_indices], jacr[:, joint_indices]]) 306 | 307 | def ee_pose(self): 308 | """ 309 | Return end-effector pose at current sim step 310 | 311 | :return: EE position (x,y,z), EE quaternion (w,x,y,z) 312 | :rtype: np.ndarray, np.ndarray 313 | """ 314 | if self._ee_is_a_site: 315 | return self.site_pose(self._ee_idx) 316 | return self.body_pose(self._ee_idx) 317 | 318 | def body_pose(self, body_id, recompute=True): 319 | """ 320 | Return pose of specified body at current sim step 321 | 322 | :param body_id: id or name of the body whose world pose has to be obtained. 323 | :type body_id: int / str 324 | :param recompute: if set to True, will perform forward kinematics computation for the step and provide updated 325 | results; defaults to True 326 | :type recompute: bool 327 | :return: position (x,y,z), quaternion (w,x,y,z) 328 | :rtype: np.ndarray, np.ndarray 329 | """ 330 | if isinstance(body_id, str): 331 | body_id = self._model.body_name2id(body_id) 332 | if recompute and not self._forwarded: 333 | self.forward_sim() 334 | return self._sim.data.body_xpos[body_id].copy(), self._sim.data.body_xquat[body_id].copy() 335 | 336 | def site_pose(self, site_id, recompute=True): 337 | """ 338 | Return pose of specified site at current sim step 339 | 340 | :param site_id: id or name of the site whose world pose has to be obtained. 341 | :type site_id: int / str 342 | :param recompute: if set to True, will perform forward kinematics computation for the step and provide updated 343 | results; defaults to True 344 | :type recompute: bool 345 | :return: position (x,y,z), quaternion (w,x,y,z) 346 | :rtype: np.ndarray, np.ndarray 347 | """ 348 | if isinstance(site_id, str): 349 | site_id = self._model.site_name2id(site_id) 350 | if recompute and not self._forwarded: 351 | self.forward_sim() 352 | return self._sim.data.site_xpos[site_id].copy(), quaternion.as_float_array(quaternion.from_rotation_matrix(self._sim.data.site_xmat[site_id].copy().reshape(3, 3))) 353 | 354 | def ee_velocity(self): 355 | """ 356 | Return end-effector velocity at current sim step 357 | 358 | :return: EE linear velocity (x,y,z), EE angular velocity (x,y,z) 359 | :rtype: np.ndarray, np.ndarray 360 | """ 361 | if self._ee_is_a_site: 362 | return self.site_velocity(self._ee_idx) 363 | return self.body_velocity(self._ee_idx) 364 | 365 | def body_velocity(self, body_id, recompute=True): 366 | """ 367 | Return velocity of specified body at current sim step 368 | 369 | :param body_id: id or name of the body whose cartesian velocity has to be obtained. 370 | :type body_id: int / str 371 | :param recompute: if set to True, will perform forward kinematics computation for the step and provide updated 372 | results; defaults to True 373 | :type recompute: bool 374 | :return: linear velocity (x,y,z), angular velocity (x,y,z) 375 | :rtype: np.ndarray, np.ndarray 376 | """ 377 | if recompute and not self._forwarded: 378 | self.forward_sim() 379 | return self._sim.data.body_xvelp[body_id].copy(), self._sim.data.body_xvelr[body_id].copy() 380 | 381 | def site_velocity(self, site_id, recompute=True): 382 | """ 383 | Return velocity of specified site at current sim step 384 | 385 | :param site_id: id or name of the site whose cartesian velocity has to be obtained. 386 | :type site_id: int / str 387 | :param recompute: if set to True, will perform forward kinematics computation for the step and provide updated 388 | results; defaults to True 389 | :type recompute: bool 390 | :return: linear velocity (x,y,z), angular velocity (x,y,z) 391 | :rtype: np.ndarray, np.ndarray 392 | """ 393 | if recompute and not self._forwarded: 394 | self.forward_sim() 395 | return self._sim.data.site_xvelp[site_id].copy(), self._sim.data.site_xvelr[site_id].copy() 396 | 397 | def _define_joint_ids(self): 398 | # transmission type (0 == joint) 399 | trntype = self._model.actuator_trntype 400 | # transmission id (get joint actuated) 401 | trnid = self._model.actuator_trnid 402 | 403 | ctrl_joints = [] 404 | for i in range(trnid.shape[0]): 405 | if trntype[i] == 0 and trnid[i, 0] not in ctrl_joints: 406 | ctrl_joints.append(trnid[i, 0]) 407 | 408 | self.controllable_joints = sorted(ctrl_joints) 409 | self.movable_joints = self._model.jnt_dofadr 410 | self.qpos_joints = self._model.jnt_qposadr 411 | 412 | def start_asynchronous_run(self, rate=None): 413 | """ 414 | Start a separate thread running the step simulation 415 | for the robot. Rendering still has to be called in the 416 | main thread. 417 | 418 | :param rate: rate of thread loop, defaults to the simulation timestep defined 419 | in the model 420 | :type rate: float 421 | """ 422 | if rate is None: 423 | rate = 1.0/self._model.opt.timestep 424 | 425 | def continuous_run(): 426 | self._asynch_thread_active = True 427 | while self._asynch_thread_active: 428 | now = time.time() 429 | self.step(render=False) 430 | elapsed = time.time() - now 431 | sleep_time = (1./rate) - elapsed 432 | if sleep_time > 0.0: 433 | time.sleep(sleep_time) 434 | 435 | self._asynch_sim_thread = threading.Thread(target=continuous_run) 436 | self._asynch_sim_thread.start() 437 | 438 | def joint_positions(self, joints=None): 439 | """ 440 | Get positions of robot joints 441 | 442 | :param joints: list of joints whose positions are to be obtained, defaults to all present joints 443 | :type joints: [int], optional 444 | :return: joint positions 445 | :rtype: np.ndarray 446 | """ 447 | if joints is None: 448 | joints = self.qpos_joints 449 | return self._sim.data.qpos[joints] 450 | 451 | def joint_velocities(self, joints=None): 452 | """ 453 | Get velocities of robot joints 454 | 455 | :param joints: list of joints whose velocities are to be obtained, defaults to all present joints 456 | :type joints: [int], optional 457 | :return: joint velocities 458 | :rtype: np.ndarray 459 | """ 460 | if joints is None: 461 | joints = self.movable_joints 462 | return self._sim.data.qvel[joints] 463 | 464 | def joint_accelerations(self, joints=None): 465 | """ 466 | Get accelerations of robot joints 467 | 468 | :param joints: list of joints whose accelerations are to be obtained, defaults to all present joints 469 | :type joints: [int], optional 470 | :return: joint accelerations 471 | :rtype: np.ndarray 472 | """ 473 | if joints is None: 474 | joints = self.movable_joints 475 | return self._sim.data.qacc[joints] 476 | 477 | def stop_asynchronous_run(self): 478 | """ 479 | Stop asynchronous run thread. See :func:`start_asynchronous_run`. 480 | """ 481 | self._asynch_thread_active = False 482 | self._asynch_sim_thread.join() 483 | 484 | def set_actuator_ctrl(self, cmd, actuator_ids=None): 485 | """ 486 | Set controller values. Each cmd should be an appropriate 487 | value for the controller type of actuator specified. 488 | 489 | @param cmd : actuator ctrl values 490 | @type cmd : [float] * self._nu 491 | """ 492 | cmd = np.asarray(cmd).flatten() 493 | actuator_ids = np.r_[:cmd.shape[0] 494 | ] if actuator_ids is None else np.r_[actuator_ids] 495 | 496 | assert cmd.shape[0] == actuator_ids.shape[0] 497 | 498 | self._sim.data.ctrl[actuator_ids] = cmd 499 | 500 | def hard_set_joint_positions(self, values, indices=None): 501 | """ 502 | Hard set robot joints to the specified values. Used mainly when 503 | using torque actuators in model. 504 | 505 | :param values: joint positions 506 | :type values: np.ndarray / [float] 507 | :param indices: indices of joints to use, defaults to the first :math:`n` joints in model, 508 | where :math:`n` is the number of values in :param:`joints`. 509 | :type indices: [int], optional 510 | """ 511 | if indices is None: 512 | indices = range(np.asarray(values).shape[0]) 513 | 514 | self._sim.data.qpos[indices] = values 515 | 516 | def __del__(self): 517 | if hasattr(self, '_asynch_sim_thread') and self._asynch_sim_thread.isAlive(): 518 | self._asynch_thread_active = False 519 | self._asynch_sim_thread.join() 520 | 521 | def step(self, render=True): 522 | """ 523 | The actual step function to forward the simulation. Not required or recommended if 524 | using asynchronous run. 525 | 526 | :param render: flag to forward the renderer as well, defaults to True 527 | :type render: bool, optional 528 | """ 529 | 530 | for f_id in self._pre_step_callables: 531 | self._pre_step_callables[f_id][0]( 532 | *self._pre_step_callables[f_id][1]) 533 | 534 | 535 | self._sim.step() 536 | 537 | # if self._first_step_not_done: 538 | # self._first_step_not_done = False 539 | # self._sim.data.sensordata[:] = np.zeros_like(self._sim.data.sensordata) 540 | 541 | self._forwarded = False 542 | 543 | for f_id in self._post_step_callables: 544 | self._post_step_callables[f_id][0]( 545 | *self._post_step_callables[f_id][1]) 546 | 547 | if render: 548 | self.render() 549 | 550 | def forward_sim(self): 551 | """ 552 | Perform no-motion forward simulation without integrating over time, i.e. sim step is 553 | not increased. 554 | """ 555 | self._sim.forward() 556 | self._forwarded = True 557 | 558 | def render(self): 559 | """ 560 | Separate function to render the visualiser. Required for visualising in main thread if using 561 | asynchronous simulation. 562 | """ 563 | if self._viewer is not None: 564 | self._viewer.render() 565 | -------------------------------------------------------------------------------- /mujoco_panda/panda_robot.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import mujoco_py as mjp 4 | from collections import deque 5 | from .mujoco_robot import MujocoRobot 6 | from .gravity_robot import GravityRobot 7 | 8 | MODEL_PATH = os.environ['MJ_PANDA_PATH'] + \ 9 | '/mujoco_panda/models/franka_panda.xml' 10 | 11 | DEFAULT_CONFIG = { 12 | 'ft_site_name': 'ee_site', 13 | 'ee_name': 'ee_site' 14 | } 15 | 16 | 17 | class PandaArm(MujocoRobot): 18 | """ 19 | Contructor 20 | 21 | :param model_path: path to model xml file for Panda, defaults to MODEL_PATH 22 | :type model_path: str, optional 23 | :param render: if set to True, will create a visualiser instance, defaults to True 24 | :type render: bool, optional 25 | :param config: see :py:class:`MujocoRobot`, defaults to DEFAULT_CONFIG 26 | :type config: dict, optional 27 | :param compensate_gravity: if set to True, will perform gravity compensation. Only required when using torque actuators 28 | and gravity is enabled. Defaults to True. 29 | :type compensate_gravity: bool, optional 30 | :param grav_comp_model_path: path to xml file for gravity compensation robot, defaults to :param:`model_path`. 31 | :type grav_comp_model_path: str, optional 32 | :param smooth_ft_sensor: (experimental) if set to True, :py:meth:`MujocoRobot.get_ft_reading` returns 33 | smoothed (low-pass-filter) force torque value **in world frame**. Defaults to False 34 | :type smooth_ft_sensor: bool, optional 35 | """ 36 | 37 | def __init__(self, model_path=MODEL_PATH, render=True, config=DEFAULT_CONFIG, compensate_gravity=True, grav_comp_model_path=None, smooth_ft_sensor=False, **kwargs): 38 | 39 | super(PandaArm, self).__init__(model_path, 40 | render=render, config=config, **kwargs) 41 | 42 | self._compensate_gravity = compensate_gravity 43 | 44 | self._grav_comp_robot = None 45 | if self._compensate_gravity: 46 | grav_comp_model_path = grav_comp_model_path if grav_comp_model_path is not None else model_path 47 | self._grav_comp_robot = GravityRobot(grav_comp_model_path) 48 | 49 | assert self._grav_comp_robot.model.nv == self._model.nv 50 | 51 | self.add_pre_step_callable( 52 | {'grav_comp': [self._grav_compensator_handle, {}]}) 53 | 54 | def _resetter(): 55 | self._ignore_grav_comp = False 56 | self.add_post_step_callable({'grav_resetter': [_resetter, {}]}) 57 | 58 | self._has_gripper = self.has_body( 59 | ['panda_hand', 'panda_leftfinger', 'panda_rightfinger']) 60 | 61 | self._logger.info("PandaArm: Robot instance initialised with{} gripper".format( 62 | "out" if not self._has_gripper else "")) 63 | 64 | if 'ee_name' not in config: 65 | if self._has_gripper: 66 | self.set_as_ee("panda_hand") 67 | else: 68 | self.set_as_ee("ee_site") 69 | 70 | self._group_actuator_joints() # identifies position and torque actuators 71 | 72 | self._neutral_pose = [0., -0.785, 0, -2.356, 0, 1.571, 0.785] 73 | self._ignore_grav_comp = False 74 | 75 | self._smooth_ft = smooth_ft_sensor 76 | 77 | if self._smooth_ft: 78 | 79 | # change length of smoothing buffer if required 80 | self._smooth_ft_buffer = deque(maxlen=10) 81 | self._smooth_ft_buffer.append(np.asarray([0,0,0,0,0,0])) 82 | self.add_pre_step_callable( 83 | {'ft_smoother': [self._smoother_handle, [True]]}) 84 | 85 | @property 86 | def has_gripper(self): 87 | """ 88 | :return: True if gripper is present in model 89 | :rtype: bool 90 | """ 91 | return self._has_gripper 92 | 93 | def _group_actuator_joints(self): 94 | arm_joint_names = ["panda_joint{}".format(i) for i in range(1, 8)] 95 | 96 | self.actuated_arm_joints = [] 97 | self.actuated_arm_joint_names = [] 98 | 99 | self.unactuated_arm_joints = [] 100 | self.unactuated_arm_joint_names = [] 101 | 102 | for jnt in arm_joint_names: 103 | jnt_id = self._model.joint_name2id(jnt) 104 | if jnt_id in self.controllable_joints: 105 | self.actuated_arm_joints.append(jnt_id) 106 | self.actuated_arm_joint_names.append(jnt) 107 | else: 108 | self.unactuated_arm_joints.append(jnt_id) 109 | self.unactuated_arm_joint_names.append(jnt) 110 | 111 | self._logger.debug("Movable arm joints: {}".format( 112 | str(self.actuated_arm_joint_names))) 113 | 114 | if len(self.unactuated_arm_joint_names) > 0: 115 | self._logger.info("These arm joints are not actuated: {}".format( 116 | str(self.unactuated_arm_joint_names))) 117 | 118 | self.actuated_gripper_joints = [] 119 | self.actuated_gripper_joint_names = [] 120 | 121 | self.unactuated_gripper_joints = [] 122 | self.unactuated_gripper_joint_names = [] 123 | 124 | if self._has_gripper: 125 | gripper_joint_names = [ 126 | "panda_finger_joint1", "panda_finger_joint2"] 127 | 128 | for jnt in gripper_joint_names: 129 | jnt_id = self._model.joint_name2id(jnt) 130 | if jnt_id in self.controllable_joints: 131 | self.actuated_gripper_joints.append(jnt_id) 132 | self.actuated_gripper_joint_names.append(jnt) 133 | else: 134 | self.unactuated_gripper_joints.append(jnt_id) 135 | self.unactuated_gripper_joint_names.append(jnt) 136 | 137 | self._logger.debug("Movable gripper joints: {}".format( 138 | str(self.actuated_gripper_joint_names))) 139 | if len(self.unactuated_gripper_joint_names) > 0: 140 | self._logger.info("These gripper joints are not actuated: {}".format( 141 | str(self.unactuated_gripper_joint_names))) 142 | 143 | self._pos_actuators, self._torque_actuators = self._identify_actuators( 144 | self.actuated_arm_joints+self.actuated_gripper_joints) 145 | 146 | self._logger.debug("Position actuators: {}\nTorque actuators: {}".format( 147 | str(self._pos_actuators), str(self._torque_actuators))) 148 | 149 | def _identify_actuators(self, joint_ids): 150 | 151 | pos_actuator_ids = [] 152 | torque_actuator_ids = [] 153 | 154 | self._joint_to_actuator_map = {} 155 | 156 | for idx, jnt in enumerate(self._model.actuator_trnid[:, 0].tolist()): 157 | assert jnt == joint_ids[idx] 158 | actuator_name = self._model.actuator_id2name(idx) 159 | controller_type = actuator_name.split('_')[1] 160 | if controller_type == 'torque' or controller_type == 'direct': 161 | torque_actuator_ids.append(idx) 162 | assert jnt not in self._joint_to_actuator_map, "Joint {} already has an actuator assigned!".format( 163 | self._model.joint_id2name(jnt)) 164 | self._joint_to_actuator_map[jnt] = idx 165 | elif controller_type == 'position' or controller_type == 'pos': 166 | pos_actuator_ids.append(idx) 167 | assert jnt not in self._joint_to_actuator_map, "Joint {} already has an actuator assigned!".format( 168 | self._model.joint_id2name(jnt)) 169 | self._joint_to_actuator_map[jnt] = idx 170 | else: 171 | self._logger.warn( 172 | "Unknown actuator type for '{}'. Ignoring. This actuator will not be controllable via PandaArm api.".format(actuator_name)) 173 | 174 | return pos_actuator_ids, torque_actuator_ids 175 | 176 | def get_ft_reading(self, *args, **kwargs): 177 | """ 178 | Overriding the parent class method for FT smoothing. FT smoothing has to be 179 | enabled while initialising PandaArm instance. 180 | 181 | :return: force, torque 182 | :rtype: np.ndarray, np.ndarray 183 | """ 184 | if not self._smooth_ft: 185 | return super(PandaArm, self).get_ft_reading(*args, **kwargs) 186 | else: 187 | vals = np.mean(np.asarray(self._smooth_ft_buffer).copy(), 0) 188 | return vals[:3], vals[3:] 189 | 190 | def jacobian(self, body_id=None): 191 | """ 192 | Return body jacobian of end-effector based on the arm joints. 193 | 194 | :param body_id: index of end-effector to be used, defaults to "panda_hand" or "ee_site" 195 | :type body_id: int, optional 196 | :return: 6x7 body jacobian 197 | :rtype: np.ndarray 198 | """ 199 | return self.body_jacobian(body_id=body_id, joint_indices=self.actuated_arm_joints) 200 | 201 | def set_neutral_pose(self, hard=True): 202 | """ 203 | Set robot to neutral pose (tuck pose) 204 | 205 | :param hard: if False, uses position control (requires position actuators in joints). Defaults to True, 206 | hard set joint positions in simulator. 207 | :type hard: bool, optional 208 | """ 209 | if hard: 210 | self.hard_set_joint_positions(self._neutral_pose) 211 | else: 212 | # position control, requires position actuators. 213 | self.set_joint_positions(self._neutral_pose) 214 | 215 | def get_actuator_ids(self, joint_list): 216 | """ 217 | Get actuator ids for the provided list of joints 218 | 219 | :param joint_list: list of joint indices 220 | :type joint_list: [int] 221 | :raises ValueError: if joint does not have a valid actuator attached. 222 | :return: list of actuator ids 223 | :rtype: np.ndarray 224 | """ 225 | try: 226 | return np.asarray([self._joint_to_actuator_map[i] for i in joint_list]) 227 | except KeyError as e: 228 | raise ValueError( 229 | "Joint {} does not have a valid actuator".format(e)) 230 | 231 | def set_joint_commands(self, cmd=None, joints=None, *args, **kwargs): 232 | """ 233 | Uses available actuator to control the specified joints. Automatically selects 234 | and controls the actuator for joints of the provided ids. 235 | 236 | :param cmd: joint commands 237 | :type cmd: np.ndarray or [float] 238 | :param joints: ids of joints to command, defaults to all controllable joints 239 | :type joints: [int], optional 240 | """ 241 | if cmd is None: 242 | self._ignore_grav_comp = False 243 | return 244 | 245 | if joints is None: 246 | joints = self.actuated_arm_joints[:len(cmd)] 247 | 248 | act_ids = self.get_actuator_ids(joints) 249 | cmd = np.asarray(cmd) 250 | 251 | torque_ids = np.intersect1d(act_ids, self._torque_actuators) 252 | pos_ids = np.intersect1d(act_ids, self._pos_actuators) 253 | 254 | if len(torque_ids) > 0: 255 | self.set_joint_torques( 256 | cmd[torque_ids], torque_ids, *args, **kwargs) 257 | 258 | if len(pos_ids) > 0: 259 | self.set_joint_positions(cmd[pos_ids], pos_ids, *args, **kwargs) 260 | 261 | def set_joint_torques(self, cmd, joint_ids=None, compensate_dynamics=False): 262 | """ 263 | Set joint torques for direct torque control. Use for torque controlling. 264 | Works for joints whose actuators are of type 'motor'. 265 | 266 | :param cmd: torque values 267 | :type cmd: [float] (size: self._nu) 268 | :param joint_ids: ids of joints to control 269 | :type joint_ids: [int] 270 | :param compensate_dynamics: if set to True, compensates for external dynamics using inverse 271 | dynamics computation. Not recommended when performing contact tasks. 272 | :type compensate_dynamics: bool 273 | """ 274 | 275 | cmd = np.asarray(cmd).flatten() 276 | joint_ids = np.r_[:cmd.shape[0] 277 | ] if joint_ids is None else np.r_[joint_ids] 278 | 279 | act_ids = self.get_actuator_ids(joint_ids) 280 | 281 | torque_ids = np.intersect1d(act_ids, self._torque_actuators) 282 | 283 | assert cmd[torque_ids].shape[0] == torque_ids.shape[0] 284 | 285 | # Cancel other dynamics 286 | if compensate_dynamics: 287 | self._ignore_grav_comp = True 288 | acc_des = np.zeros(self._sim.model.nv) 289 | acc_des[torque_ids] = cmd[torque_ids] 290 | self._sim.data.qacc[:] = acc_des 291 | mjp.functions.mj_inverse(self._model, self._sim.data) 292 | cmd = self._sim.data.qfrc_inverse[torque_ids].copy() 293 | elif not compensate_dynamics and self._compensate_gravity: 294 | self._ignore_grav_comp = True 295 | cmd[torque_ids] += self.gravity_compensation_torques()[torque_ids] 296 | 297 | if len(torque_ids) > 0: 298 | self.set_actuator_ctrl(cmd[torque_ids], torque_ids) 299 | 300 | def set_joint_positions(self, cmd=None, joint_ids=None, *args, **kwargs): 301 | """ 302 | Set joint positions for position control. 303 | Works for joints whose actuators are of type 'motor'. 304 | 305 | :param cmd: torque values 306 | :type cmd: [float] (size: self._nu) 307 | :param joint_ids: ids of joints to control 308 | :type joint_ids: [int] 309 | """ 310 | self._ignore_grav_comp = False 311 | if cmd is None: 312 | return 313 | 314 | cmd = np.asarray(cmd).flatten() 315 | joint_ids = np.r_[:cmd.shape[0] 316 | ] if joint_ids is None else np.r_[joint_ids] 317 | 318 | act_ids = self.get_actuator_ids(joint_ids) 319 | 320 | pos_ids = np.intersect1d(act_ids, self._pos_actuators) 321 | 322 | assert cmd[pos_ids].shape[0] == pos_ids.shape[0] 323 | 324 | if len(pos_ids) > 0: 325 | self.set_actuator_ctrl(cmd[pos_ids], pos_ids) 326 | 327 | def gravity_compensation_torques(self): 328 | """ 329 | Get the gravity compensation torque at current sim timestep 330 | 331 | :return: gravity compensation torques in all movable joints 332 | :rtype: np.ndarray 333 | """ 334 | self._grav_comp_robot.sim.data.qpos[self._grav_comp_robot._all_joints] = self._sim.data.qpos[self.qpos_joints].copy( 335 | ) 336 | self._grav_comp_robot.sim.data.qvel[self._grav_comp_robot._controllable_joints] = 0. 337 | self._grav_comp_robot.sim.data.qacc[self._grav_comp_robot._controllable_joints] = 0. 338 | mjp.functions.mj_inverse( 339 | self._grav_comp_robot.model, self._grav_comp_robot.sim.data) 340 | 341 | return self._grav_comp_robot.sim.data.qfrc_inverse.copy() 342 | 343 | def _grav_compensator_handle(self): 344 | if self._compensate_gravity and not self._ignore_grav_comp: 345 | self._sim.data.ctrl[self._torque_actuators] = self.gravity_compensation_torques()[ 346 | self._torque_actuators] 347 | 348 | def _smoother_handle(self, *args, **kwargs): 349 | self._smooth_ft_buffer.append( 350 | np.append(*(super(PandaArm, self).get_ft_reading(*args, **kwargs)))) 351 | 352 | @classmethod 353 | def fullRobotWithTorqueActuators(cls, **kwargs): 354 | """ 355 | Create an instance of this class using the model of the full 356 | robot (arm + gripper) and torque actuators at arm joints. 357 | """ 358 | model_path = os.environ['MJ_PANDA_PATH'] + \ 359 | '/mujoco_panda/models/franka_panda.xml' 360 | return cls(model_path=model_path, **kwargs) 361 | 362 | @classmethod 363 | def fullRobotWithPositionActuators(cls, **kwargs): 364 | """ 365 | Create an instance of this class using the model of the full 366 | robot (arm + gripper) and position actuators at arm joints. 367 | """ 368 | model_path = os.environ['MJ_PANDA_PATH'] + \ 369 | '/mujoco_panda/models/franka_panda_pos.xml' 370 | return cls(model_path=model_path, **kwargs) 371 | 372 | @classmethod 373 | def withTorqueActuators(cls, **kwargs): 374 | """ 375 | Create an instance of this class using the model of the 376 | robot arm without gripper, and torque actuators at arm joints. 377 | """ 378 | model_path = os.environ['MJ_PANDA_PATH'] + \ 379 | '/mujoco_panda/models/franka_panda_no_gripper.xml' 380 | return cls(model_path=model_path, **kwargs) 381 | 382 | @classmethod 383 | def withPositionActuators(cls, **kwargs): 384 | """ 385 | Create an instance of this class using the model of the 386 | robot arm without gripper, and position actuators at arm joints. 387 | """ 388 | model_path = os.environ['MJ_PANDA_PATH'] + \ 389 | '/mujoco_panda/models/franka_panda_pos_no_gripper.xml' 390 | return cls(model_path=model_path, **kwargs) 391 | -------------------------------------------------------------------------------- /mujoco_panda/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/justagist/mujoco_panda/4f853a6c7f7e911ac39160ddb80c1fc5618fe80e/mujoco_panda/utils/__init__.py -------------------------------------------------------------------------------- /mujoco_panda/utils/debug_utils.py: -------------------------------------------------------------------------------- 1 | try: 2 | from tkinter import * 3 | _NO_TK = False 4 | except ImportError: 5 | print("tkinter not found. Cannot run with ParallelPythonCmd with tkinter.") 6 | _NO_TK = True 7 | 8 | import threading 9 | from .keyboard_poll import KBHit 10 | 11 | 12 | class ParallelPythonCmd(object): 13 | 14 | def __init__(self, callable_func, kbhit=False): 15 | """ 16 | Provides a debugger thread when using mujoco visualiser rendering. 17 | Debug commands can be sent in this separate thread 18 | to control or retrieve info from the simulated robot. 19 | 20 | :param callable_func: a handle to a function (should be defined somewhere with 21 | access to the mujoco robot) which executes the provided command. 22 | See `exec_func` below. 23 | :type callable_func: callable 24 | :param kbhit: a handle to a function, defaults to False 25 | :type kbhit: bool, optional 26 | """ 27 | 28 | if kbhit or _NO_TK: 29 | target = self._run 30 | else: 31 | target = self._run_tk 32 | self._th = threading.Thread(target=target) 33 | self._callable = callable_func 34 | self.start_thread() 35 | 36 | def start_thread(self): 37 | self._th.start() 38 | 39 | def _run(self): 40 | kb = KBHit() 41 | print("Running KBHit debugger...") 42 | string = '' 43 | while True: 44 | c = kb.getch() 45 | if ord(c) == 27: 46 | string = '' 47 | elif ord(c) == 10: 48 | ret = self._callable(string) 49 | if ret is not None: 50 | print("Cmd: {}\nOutput: {}".format(string, ret)) 51 | string = '' 52 | elif ord(c) == 127: 53 | if string != '': 54 | string = string[:-1] 55 | else: 56 | string += c 57 | 58 | def _run_tk(self): 59 | print("Running tk debugger...") 60 | self._root = Tk() 61 | self._root.title("Python Cmd") 62 | self._e = Entry(self._root) 63 | self._e.pack() 64 | self._text = Text(self._root) 65 | self._text.insert(INSERT, "Hello.....") 66 | self._text.pack() 67 | self._e.focus_set() 68 | self._root.bind('', self._printtext) 69 | self._root.mainloop() 70 | 71 | def _printtext(self, event=None): 72 | string = self._e.get() 73 | a = self._callable(string) 74 | if a is not None: 75 | self._text.delete("%d.%d" % (0, 0), END) 76 | self._text.insert(INSERT, str(a)) 77 | self._e.delete(0, 'end') 78 | 79 | # demo exec function handle 80 | 81 | 82 | def exec_func(cmd): 83 | """ 84 | Sample handle that can be used with :py:class`ParallelPythonCmd`. 85 | Copy this to somewhere which has access to all required objects 86 | (eg: PandaArm object), and pass the handle to this function as 87 | argument to :py:class`ParallelPythonCmd` instance. 88 | 89 | Requirement for function handle: 90 | - take an argument `cmd` 91 | - perform eval or exec on `cmd` 92 | - optionally return a string 93 | """ 94 | 95 | if cmd == '': 96 | return None 97 | print(cmd) 98 | try: 99 | if "=" in cmd: 100 | exec(cmd) 101 | a = cmd 102 | else: 103 | a = eval(cmd) 104 | print(a) 105 | except Exception as e: 106 | a = "Exception: {}: {}".format(e.what(),e) 107 | if a is not None: 108 | return str(a) 109 | -------------------------------------------------------------------------------- /mujoco_panda/utils/keyboard_poll.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import atexit 3 | import termios 4 | from select import select 5 | 6 | 7 | class KBHit: 8 | 9 | def __init__(self): 10 | '''Creates a KBHit object that you can call to do various keyboard things. 11 | ''' 12 | # Save the terminal settings 13 | self.fd = sys.stdin.fileno() 14 | self.new_term = termios.tcgetattr(self.fd) 15 | self.old_term = termios.tcgetattr(self.fd) 16 | 17 | # New terminal setting unbuffered 18 | self.new_term[3] = (self.new_term[3] & ~termios.ICANON & ~termios.ECHO) 19 | termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.new_term) 20 | 21 | # Support normal-terminal reset at exit 22 | atexit.register(self.set_normal_term) 23 | 24 | def set_normal_term(self): 25 | ''' Resets to normal terminal. On Windows this is a no-op. 26 | ''' 27 | termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old_term) 28 | 29 | def getch(self): 30 | ''' Returns a keyboard character after kbhit() has been called. 31 | Should not be called in the same program as getarrow(). 32 | ''' 33 | s = '' 34 | return sys.stdin.read(1) 35 | 36 | def getarrow(self): 37 | ''' Returns an arrow-key code after kbhit() has been called. Codes are 38 | 0 : up 39 | 1 : right 40 | 2 : down 41 | 3 : left 42 | Should not be called in the same program as getch(). 43 | ''' 44 | c = sys.stdin.read(3)[2] 45 | vals = [65, 67, 66, 68] 46 | return vals.index(ord(c.decode('utf-8'))) 47 | 48 | 49 | def kbhit(self): 50 | ''' Returns True if keyboard character was hit, False otherwise. 51 | ''' 52 | dr,dw,de = select([sys.stdin], [], [], 0) 53 | return dr != [] 54 | 55 | def __del__(self): 56 | self.set_normal_term() 57 | 58 | 59 | if __name__ == "__main__": 60 | 61 | kb = KBHit() 62 | 63 | print('Hit any key, or ESC to exit') 64 | 65 | while True: 66 | if kb.kbhit(): 67 | c = kb.getch() 68 | if ord(c) == 27: # ESC 69 | break 70 | print(ord(c), c, chr(ord(c))) 71 | # else: 72 | # print("Nothing") 73 | 74 | # kb.set_normal_term() 75 | -------------------------------------------------------------------------------- /mujoco_panda/utils/tf.py: -------------------------------------------------------------------------------- 1 | import mujoco_py 2 | import quaternion 3 | import numpy as np 4 | from scipy.spatial.transform import Rotation as R 5 | 6 | def euler2quat(euler, degrees=True, flip=False): 7 | r = R.from_euler('xyz', euler, degrees=degrees) 8 | q = r.as_quat() 9 | if flip: 10 | return np.hstack([q[-1], q[:3]]) 11 | return q 12 | 13 | def quat2euler(quat, degrees=False, flip=False): 14 | if flip: 15 | quat = np.hstack([quat[1:4], quat[0]]) 16 | r = R.from_quat(quat) 17 | return r.as_euler('xyz', degrees=degrees) 18 | 19 | def euler2mat(euler, degrees=True): 20 | r = R.from_euler('xyz', euler, degrees=degrees) 21 | return r.as_matrix() 22 | 23 | def quat2mat(quat, flip=False): 24 | if flip: 25 | quat = np.hstack([quat[1:4], quat[0]]) 26 | r = R.from_quat(quat) 27 | return r.as_matrix() 28 | 29 | 30 | def quatdiff_in_euler(quat_curr, quat_des): 31 | """ 32 | Compute difference between quaternions and return 33 | Euler angles as difference 34 | """ 35 | curr_mat = quaternion.as_rotation_matrix( 36 | quaternion.from_float_array(quat_curr)) 37 | des_mat = quaternion.as_rotation_matrix( 38 | quaternion.from_float_array(quat_des)) 39 | rel_mat = des_mat.T.dot(curr_mat) 40 | rel_quat = quaternion.from_rotation_matrix(rel_mat) 41 | vec = quaternion.as_float_array(rel_quat)[1:] 42 | if rel_quat.w < 0.0: 43 | vec = -vec 44 | 45 | return -des_mat.dot(vec) 46 | 47 | identity_quat = np.array([1., 0., 0., 0.]) 48 | 49 | 50 | def mat2Quat(mat): 51 | ''' 52 | Convenience function for mju_mat2Quat. 53 | ''' 54 | res = np.zeros(4) 55 | mujoco_py.functions.mju_mat2Quat(res, mat.flatten()) 56 | return res 57 | 58 | 59 | def quat2Mat(quat): 60 | ''' 61 | Convenience function for mju_quat2Mat. 62 | ''' 63 | res = np.zeros(9) 64 | mujoco_py.functions.mju_quat2Mat(res, quat) 65 | res = res.reshape(3, 3) 66 | return res 67 | 68 | 69 | def quat2Vel(quat): 70 | ''' 71 | Convenience function for mju_quat2Vel. 72 | ''' 73 | res = np.zeros(3) 74 | mujoco_py.functions.mju_quat2Vel(res, quat, 1.) 75 | return res 76 | 77 | 78 | def axisAngle2Quat(axis, angle): 79 | ''' 80 | Convenience function for mju_quat2Vel. 81 | ''' 82 | res = np.zeros(4) 83 | mujoco_py.functions.mju_axisAngle2Quat(res, axis, angle) 84 | return res 85 | 86 | 87 | def subQuat(qb, qa): 88 | ''' 89 | Convenience function for mju_subQuat. 90 | ''' 91 | # Allocate memory 92 | qa_t = np.zeros(4) 93 | q_diff = np.zeros(4) 94 | res = np.zeros(3) 95 | 96 | # Compute the subtraction 97 | mujoco_py.functions.mju_negQuat(qa_t, qa) 98 | mujoco_py.functions.mju_mulQuat(q_diff, qb, qa_t) 99 | mujoco_py.functions.mju_quat2Vel(res, q_diff, 1.) 100 | 101 | # Mujoco 1.50 doesn't support the subQuat function. Uncomment this when 102 | # mujoco_py upgrades to Mujoco 2.0 103 | # res = np.zeros(3) 104 | # mujoco_py.functions.mju_subQuat(res, qa, qb) 105 | return res 106 | 107 | 108 | def mulQuat(qa, qb): 109 | res = np.zeros(4) 110 | mujoco_py.functions.mju_mulQuat(res, qa, qb) 111 | return res 112 | 113 | 114 | def random_quat(): 115 | q = np.random.random(4) 116 | q = q/np.linalg.norm(q) 117 | return q 118 | 119 | 120 | def quatIntegrate(q, v, dt=1.): 121 | res = q.copy() 122 | mujoco_py.functions.mju_quatIntegrate(res, v, 1.) 123 | return res 124 | 125 | 126 | def quatAdd(q1, v): 127 | qv = quatIntegrate(identity_quat, v) 128 | res = mulQuat(qv, q1) 129 | return res 130 | 131 | 132 | def rotVecQuat(v, q): 133 | res = np.zeros(3) 134 | mujoco_py.functions.mju_rotVecQuat(res, v, q) 135 | return res 136 | -------------------------------------------------------------------------------- /mujoco_panda/utils/viewer_utils.py: -------------------------------------------------------------------------------- 1 | from mujoco_py.generated import const 2 | from .tf import quat2Mat 3 | 4 | def render_frame(viewer, pos, quat, scale = 0.1, alpha = 1.): 5 | """ 6 | Visualise a 3D coordinate frame. 7 | """ 8 | viewer.add_marker(pos=pos, 9 | label='', 10 | type=const.GEOM_SPHERE, 11 | size=[.01, .01, .01]) 12 | mat = quat2Mat(quat) 13 | cylinder_half_height = scale 14 | pos_cylinder = pos + mat.dot([0.0, 0.0, cylinder_half_height]) 15 | viewer.add_marker(pos=pos_cylinder, 16 | label='', 17 | type=const.GEOM_CYLINDER, 18 | size=[.005, .005, cylinder_half_height], 19 | rgba=[0.,0.,1.,alpha], 20 | mat=mat) 21 | pos_cylinder = pos + mat.dot([cylinder_half_height, 0.0, 0.]) 22 | viewer.add_marker(pos=pos_cylinder, 23 | label='', 24 | type=const.GEOM_CYLINDER, 25 | size=[cylinder_half_height, .005, .005 ], 26 | rgba=[1., 0., 0., alpha], 27 | mat=mat) 28 | pos_cylinder = pos + mat.dot([0.0, cylinder_half_height, 0.0]) 29 | viewer.add_marker(pos=pos_cylinder, 30 | label='', 31 | type=const.GEOM_CYLINDER, 32 | size=[.005, cylinder_half_height, .005], 33 | rgba=[0., 1., 0., alpha], 34 | mat=mat) 35 | 36 | 37 | def render_point(viewer, pos): 38 | viewer.add_marker(pos=pos, 39 | label='', 40 | type=const.GEOM_SPHERE, 41 | size=[.01, .01, .01]) 42 | -------------------------------------------------------------------------------- /set_env.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | export MJ_PANDA_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 4 | export PYTHONPATH=$MJ_PANDA_PATH:$PYTHONPATH 5 | 6 | echo -e "Setting MJ_PANDA_PATH=$MJ_PANDA_PATH\n" 7 | --------------------------------------------------------------------------------