prodmp box pushing works

fancy_gym/black_box/black_box_wrapper.py

@@ -9,6 +9,7 @@ from fancy_gym.black_box.controller.base_controller import BaseController
 from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
 from fancy_gym.utils.utils import get_numpy
 
+import torch
 
 class BlackBoxWrapper(gym.ObservationWrapper):
 
@@ -80,7 +81,9 @@ class BlackBoxWrapper(gym.ObservationWrapper):
         bc_time = np.array(0 if not self.do_replanning else self.current_traj_steps * self.dt)
         # TODO we could think about initializing with the previous desired value in order to have a smooth transition
         #  at least from the planning point of view.
-        self.traj_gen.set_boundary_conditions(bc_time, self.current_pos, self.current_vel)
+        self.traj_gen.set_boundary_conditions(torch.as_tensor(bc_time),
+                                              torch.as_tensor(self.current_pos),
+                                              torch.as_tensor(self.current_vel))
         duration = None if self.learn_sub_trajectories else self.duration
         self.traj_gen.set_duration(duration, self.dt)
         # traj_dict = self.traj_gen.get_trajs(get_pos=True, get_vel=True)

fancy_gym/black_box/factory/basis_generator_factory.py

@@ -1,4 +1,4 @@
-from mp_pytorch.basis_gn import NormalizedRBFBasisGenerator, ZeroPaddingNormalizedRBFBasisGenerator
+from mp_pytorch.basis_gn import NormalizedRBFBasisGenerator, ZeroPaddingNormalizedRBFBasisGenerator, ProDMPBasisGenerator
 from mp_pytorch.phase_gn import PhaseGenerator
 
 ALL_TYPES = ["rbf", "zero_rbf", "rhythmic"]
@@ -10,6 +10,8 @@ def get_basis_generator(basis_generator_type: str, phase_generator: PhaseGenerat
         return NormalizedRBFBasisGenerator(phase_generator, **kwargs)
     elif basis_generator_type == "zero_rbf":
         return ZeroPaddingNormalizedRBFBasisGenerator(phase_generator, **kwargs)
+    elif basis_generator_type == "prodmp":
+        return ProDMPBasisGenerator(phase_generator, **kwargs)
     elif basis_generator_type == "rhythmic":
         raise NotImplementedError()
         # return RhythmicBasisGenerator(phase_generator, **kwargs)

fancy_gym/envs/__init__.py

@@ -87,8 +87,7 @@ DEFAULT_BB_DICT_ProDMP = {
     },
     "black_box_kwargs": {
         'replanning_schedule': None,
-        'verbose': 2,
+        'verbose': 2
-        'enable_traj_level_reward': False,
     }
 }
 
@@ -500,6 +499,16 @@ for _v in _versions:
     _env_id = f'{_name[0]}ProDMP-{_name[1]}'
     kwargs_dict_box_pushing_prodmp = deepcopy(DEFAULT_BB_DICT_ProDMP)
     kwargs_dict_box_pushing_prodmp['wrappers'].append(mujoco.box_pushing.MPWrapper)
+    kwargs_dict_box_pushing_prodmp['name'] = _v
+    kwargs_dict_box_pushing_prodmp['controller_kwargs']['p_gains'] = 0.01 * np.array([120., 120., 120., 120., 50., 30., 10.])
+    kwargs_dict_box_pushing_prodmp['controller_kwargs']['d_gains'] = 0.01 * np.array([10., 10., 10., 10., 6., 5., 3.])
+
+    register(
+        id=_env_id,
+        entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
+        kwargs=kwargs_dict_box_pushing_prodmp
+    )
+    ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProDMP"].append(_env_id)
 #
 # ## Walker2DJump
 # _versions = ['Walker2DJump-v0']

fancy_gym/envs/mujoco/box_pushing/box_push_env.py

@@ -1,416 +0,0 @@
-import os
-import time
-
-import numpy as np
-import mujoco_py
-from gym import utils, spaces
-from gym.envs.mujoco import MujocoEnv
-# from alr_envs.alr.mujoco.box_pushing.box_push_utils import PushBoxReward, rot_to_quat, q_max, q_min, get_quaternion_error
-# from alr_envs.alr.mujoco.box_pushing.box_push_utils import q_dot_dot_max, rotation_distance
-# from alr_envs.alr.mujoco.box_pushing.box_push_utils import TrajectoryRecoder
-from fancy_gym.envs.mujoco.box_pushing.box_push_utils import PushBoxReward, rot_to_quat, q_max, q_min, get_quaternion_error
-from fancy_gym.envs.mujoco.box_pushing.box_push_utils import q_dot_dot_max, rotation_distance
-from fancy_gym.envs.mujoco.box_pushing.box_push_utils import TrajectoryRecoder
-
-MAX_EPISODE_STEPS = 1000
-
-INIT_BOX_POS_BOUND = np.array([[0.3, -0.45, -0.01], [0.6, 0.45, -0.01]])
-
-
-class BOX_PUSH_Env_Gym(MujocoEnv, utils.EzPickle):
-    def __init__(self, enable_gravity_comp=False, frame_skip=1, reward_type="Dense"):
-        model_path = os.path.join(os.path.dirname(__file__), "assets", "box_pushing.xml")
-        self.reward_type = reward_type
-        assert reward_type in ["Dense", "Sparse1", "Sparse2"], "reward_type must be one of Dense, Sparse1, Sparse2"
-        self.enable_gravity_comp = enable_gravity_comp
-        self.frame_skip = frame_skip
-        self.max_episode_steps = MAX_EPISODE_STEPS // self.frame_skip
-
-        self.time_steps = 0
-        self.init_qpos_box_push = np.array([
-            0., 0., 0., -1.5, 0., 1.5, 0., 0., 0., 0.6, 0.45, 0.0, 1., 0., 0.,
-            0.
-        ])
-        self.init_qvel_box_push = np.zeros(15)
-        self._id_set = False
-        self.reward = PushBoxReward()
-
-        # utilities for IK
-        self.J_reg = 1e-6
-        self.W = np.diag([1, 1, 1, 1, 1, 1, 1])
-        self.target_th_null = np.array([
-            3.57795216e-09,
-            1.74532920e-01,
-            3.30500960e-08,
-            -8.72664630e-01,
-            -1.14096181e-07,
-            1.22173047e00,
-            7.85398126e-01,
-        ])
-
-        self.torque_bound_low = -q_dot_dot_max
-        self.torque_bound_high = q_dot_dot_max
-
-        self.episode_energy = 0.
-        self.episode_end_pos_dist = 0.
-        self.episode_end_rot_dist = 0.
-        # self.trajectory_recorder = TrajectoryRecoder(None, max_length=100)
-        # end of IK utilities
-        super(BOX_PUSH_Env_Gym, self).__init__(model_path=model_path,
-                                               frame_skip=self.frame_skip,
-                                               mujoco_bindings="mujoco_py")
-        utils.EzPickle.__init__(self)
-
-        action_space_low = np.array([-1.0] * 7)
-        action_space_high = np.array([1.0] * 7)
-        self.action_space = spaces.Box(low=action_space_low,
-                                       high=action_space_high,
-                                       dtype='float32')
-        # self.trajectory_recorder.update_sim(self.sim)
-
-    def _set_ids(self):
-        self.box_id = self.sim.model._body_name2id["box_0"]
-        self.target_id = self.sim.model._body_name2id["target_pos"]
-        self.rod_tip_site_id = self.sim.model.site_name2id("rod_tip")
-        self._id_set = True
-
-    def sample_context(self):
-        # return np.random.uniform(INIT_BOX_POS_BOUND[0],
-        #                          INIT_BOX_POS_BOUND[1],
-        #                          size=INIT_BOX_POS_BOUND[0].shape)
-        # pos = np.random.uniform(INIT_BOX_POS_BOUND[0],
-        #                         INIT_BOX_POS_BOUND[1],
-        #                         size=INIT_BOX_POS_BOUND[0].shape)
-        pos = np.array([0.4, 0.3, -0.01]) # was 0.45 0.4
-        # theta = np.random.uniform(0, np.pi * 2)
-        theta = 0.0
-        quat = rot_to_quat(theta, np.array([0, 0, 1]))
-        return np.concatenate((pos, quat))
-
-    def generate_specified_context(self, goal_x, goal_y, goal_rot):
-        pos = np.array([goal_x, goal_y, -0.01])
-        quat = rot_to_quat(goal_rot, np.array([0, 0, 1]))
-        return np.concatenate((pos, quat))
-
-    def reset_model(self):
-        self.reward.reset()
-
-        self.episode_energy = 0.
-        self.episode_end_pos_dist = 0.
-        self.episode_end_rot_dist = 0.
-
-        self.set_state(self.init_qpos_box_push, self.init_qvel_box_push)
-        box_init_pos = self.sample_context()
-        box_init_pos[0] = 0.4
-        box_init_pos[1] = 0.3
-        box_init_pos[-4:] = np.array([0, 0, 0, 1.])
-
-        box_target_pos = self.sample_context()
-
-        # if both box and target are in the same position, sample again
-        # while np.linalg.norm(box_init_pos[:3] - box_target_pos[:3]) < 0.3:
-        #     box_target_pos = self.sample_context()
-        box_target_pos[0] = 0.4  # was 0.4
-        box_target_pos[1] = -0.3  # was -0.3
-        # box_target_pos = self.generate_specified_context(0.45, -0.25, np.pi)
-
-        self.sim.model.body_pos[2] = box_target_pos[:3]
-        self.sim.model.body_quat[2] = box_target_pos[-4:]
-
-        self.sim.model.body_pos[3] = box_target_pos[:3]
-        self.sim.model.body_quat[3] = box_target_pos[-4:]
-
-        desired_ee_pos = box_init_pos[:3].copy()
-        desired_ee_pos[2] += 0.15
-        desired_ee_quat = np.array([0, 1, 0, 0])
-        desired_joint_pos = self.findJointPosition(desired_ee_pos,
-                                                   desired_ee_quat)
-        self.sim.data.qpos[:7] = desired_joint_pos
-        self.sim.data.set_joint_qpos('box_joint', box_init_pos)
-        # self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
-        self.sim.forward()
-        self.time_steps = 0
-
-        # reset trajectory recorder
-        # self.trajectory_recorder.plot_trajectories()
-        # self.trajectory_recorder.save_trajectory("ppo_dense")
-        # self.trajectory_recorder.reset()
-
-        return self._get_obs()
-
-    def _get_obs(self):
-        box_pos = self.sim.data.get_body_xpos("box_0")
-        box_quat = self.sim.data.get_body_xquat("box_0")
-        target_pos = self.sim.data.get_body_xpos("replan_target_pos")
-        target_quat = self.sim.data.get_body_xquat("replan_target_pos")
-        rod_tip_pos = self.sim.data.site_xpos[self.rod_tip_site_id]
-        rod_quat = self.sim.data.get_body_xquat("push_rod")
-        obs = np.concatenate([
-            self.sim.data.qpos[:7].copy(),
-            self.sim.data.qvel[:7].copy(),
-            self.sim.data.qfrc_bias[:7].copy(),
-            # self.sim.data.qfrc_actuator[:7].copy(),
-            rod_tip_pos,
-            rod_quat,
-            box_pos,
-            box_quat,
-            target_pos,
-            target_quat
-            # np.array([self.time_steps / 100.])
-        ])
-        return obs
-
-    def step(self, action):
-        if not self._id_set:
-            self._set_ids()
-        invalid_flag = False
-        done = False
-        self.time_steps += 1
-
-        action = np.clip(action, self.action_space.low, self.action_space.high)
-
-        if self.enable_gravity_comp:
-            action = action * 10.  # rescale action
-            resultant_action = action + self.sim.data.qfrc_bias[:7].copy()
-        else:
-            resultant_action = action * 10.
-
-        resultant_action = np.clip(resultant_action, self.torque_bound_low,
-                                   self.torque_bound_high)
-
-        # the replan_target_pos was align with the target_pos before 800
-        # if self.time_steps == 20:
-        #     new_target_pos = np.array([1., 1., -0.01])
-        #     new_target_quat = np.array([0, 1, 0, 0])
-        #     while new_target_pos[0] < 0.4 or new_target_pos[0] > 0.6 or abs(
-        #             new_target_pos[1]) > 0.45:
-        #         pos_change = np.random.uniform(-0.25, 0.2, 3)
-        #         pos_change[-1] = 0.
-        #         # pos_change[-2] = 0.
-        #         # self.sim.model.body_pos[3] = self.sim.data.get_body_xpos("target_pos") + pos_change
-        #         # self.sim.model.body_quat[3] = self.sim.data.get_body_xquat("target_pos")
-        #         new_target_pos = self.sim.data.get_body_xpos(
-        #             "target_pos") + pos_change
-        #         # new_target_quat = self.sim.data.get_body_xquat("target_pos")
-        #     old_target_quat = self.sim.data.get_body_xquat("target_pos")
-        #     new_target_quat = None
-        #     while new_target_quat is None or rotation_distance(
-        #             new_target_quat, old_target_quat) > np.pi / 2.:
-        #         theta = np.random.uniform(0, np.pi * 2)
-        #         new_target_quat = rot_to_quat(theta, np.array([0, 0, 1]))
-        #
-        #     self.sim.model.body_pos[3] = new_target_pos
-        #     self.sim.model.body_quat[3] = new_target_quat
-        #     self.sim.forward()
-
-        try:
-            self.do_simulation(resultant_action, self.frame_skip)
-        except mujoco_py.MujocoException as e:
-            print(e)
-            invalid_flag = True
-
-        # record the trajectory
-        # if self.time_steps % (10//self.frame_skip) == 0:
-        # self.trajectory_recorder.record()
-
-        box_pos = self.sim.data.get_body_xpos("box_0")
-        box_quat = self.sim.data.get_body_xquat("box_0")
-        target_pos = self.sim.data.get_body_xpos("replan_target_pos")
-        target_quat = self.sim.data.get_body_xquat("replan_target_pos")
-        rod_tip_pos = self.sim.data.site_xpos[self.rod_tip_site_id].copy()
-        rod_quat = self.sim.data.get_body_xquat("push_rod")
-        qpos = self.sim.data.qpos[:7].copy()
-        qvel = self.sim.data.qvel[:7].copy()
-
-
-        episode_end = False
-        self.episode_energy += np.sum(np.square(action))
-        if self.time_steps >= 100 - 1:
-            episode_end = True
-            done = True
-            self.episode_end_pos_dist = np.linalg.norm(box_pos - target_pos)
-            self.episode_end_rot_dist = rotation_distance(
-                box_quat, target_quat)
-
-        if self.reward_type == "Dense":
-            reward = self.reward.step_reward(box_pos, box_quat, target_pos,
-                                            target_quat, rod_tip_pos, rod_quat,
-                                            qpos, qvel, action)
-        elif self.reward_type == "Sparse1":
-            reward = self.reward.sparse1_reward(episode_end, box_pos, box_quat,
-                                                target_pos, target_quat,
-                                                rod_tip_pos, rod_quat, qpos, qvel,
-                                                action)
-        elif self.reward_type == "Sparse2":
-            reward = self.reward.sparse2_reward(episode_end, box_pos, box_quat,
-                                                target_pos, target_quat, rod_tip_pos,
-                                                rod_quat, qpos, qvel, action)
-        else:
-            raise NotImplementedError("Unknown reward type: {}".format(
-                self.reward_type))
-
-        if invalid_flag:
-            reward = -25
-
-        obs = self._get_obs()
-        infos = {
-            'episode_end':
-            episode_end,
-            'box_goal_pos_dist':
-            self.episode_end_pos_dist,
-            'box_goal_rot_dist':
-            self.episode_end_rot_dist,
-            'episode_energy':
-            self.episode_energy if episode_end else 0.,
-            'is_success':
-            True if episode_end and self.episode_end_pos_dist < 0.05
-            and self.episode_end_rot_dist < 0.5 else False,
-            'num_steps':
-            self.time_steps
-        }
-        return obs, reward, done, infos
-
-    def getJacobian(self, body_id="tcp", q=np.zeros(7)):
-
-        self.sim.data.qpos[:7] = q
-        self.sim.forward()
-        jacp = self.sim.data.get_body_jacp(body_id).reshape(3, -1)[:, :7]
-        jacr = self.sim.data.get_body_jacr(body_id).reshape(3, -1)[:, :7]
-        jac = np.concatenate((jacp, jacr), axis=0)
-        return jac
-
-    def findJointPosition(self, desiredPos=None, desiredQuat=None):
-
-        eps = 1e-5
-        IT_MAX = 1000
-        DT = 1e-3
-
-        i = 0
-        self.pgain = [
-            33.9403713446798,
-            30.9403713446798,
-            33.9403713446798,
-            27.69370238555632,
-            33.98706171459314,
-            30.9185531893281,
-        ]
-        self.pgain_null = 5 * np.array([
-            7.675519770796831,
-            2.676935478437176,
-            8.539040163444975,
-            1.270446361314313,
-            8.87752182480855,
-            2.186782233762969,
-            4.414432577659688,
-        ])
-        self.pgain_limit = 20
-
-        q = self.sim.data.qpos[:7].copy()
-        qd_d = np.zeros(q.shape)
-        oldErrNorm = np.inf
-
-        # if (desiredPos is None):
-        #     desiredPos = self.desiredTaskPosition[:3]
-        #
-        # if (desiredQuat is None):
-        #     desiredQuat = self.desiredTaskPosition[3:]
-
-        while True:
-            oldQ = q
-            q = q + DT * qd_d
-
-            q = np.clip(q, q_min, q_max)
-
-            # cartPos, orient = self.sim.data.site_xpos[self.rod_tip_site_id]
-            cartPos = self.sim.data.get_body_xpos("tcp")
-            orient = self.sim.data.get_body_xquat("tcp")
-            cpos_err = desiredPos - cartPos
-
-            if np.linalg.norm(orient -
-                              desiredQuat) > np.linalg.norm(orient +
-                                                            desiredQuat):
-                orient = -orient
-
-            cpos_err = np.clip(cpos_err, -0.1, 0.1)
-            cquat_err = np.clip(
-                get_quaternion_error(orient, desiredQuat),
-                -0.5,
-                0.5,
-            )
-            err = np.hstack((cpos_err, cquat_err))
-
-            errNorm = np.sum(cpos_err**2) + np.sum((orient - desiredQuat)**2)
-
-            if errNorm > oldErrNorm:
-                q = oldQ
-                DT = DT * 0.7
-                continue
-            else:
-                DT = DT * 1.025
-
-            if errNorm < eps:
-                success = True
-                break
-            if i >= IT_MAX:
-                success = False
-                break
-
-            # if not i % 1:
-            #print('%d: error = %s, %s, %s' % (i, errNorm, oldErrNorm, DT))
-
-            oldErrNorm = errNorm
-
-            J = self.getJacobian(q=q)
-
-            Jw = J.dot(self.W)
-
-            # J *  W * J' + reg * I
-            JwJ_reg = Jw.dot(J.T) + self.J_reg * np.eye(J.shape[0])
-
-            # Null space movement
-            qd_null = self.pgain_null * (self.target_th_null - q)
-
-            margin_to_limit = 0.1
-            qd_null_limit = np.zeros(qd_null.shape)
-            qd_null_limit_max = self.pgain_limit * (q_max - margin_to_limit -
-                                                    q)
-            qd_null_limit_min = self.pgain_limit * (q_min + margin_to_limit -
-                                                    q)
-            qd_null_limit[q > q_max - margin_to_limit] += qd_null_limit_max[
-                q > q_max - margin_to_limit]
-            qd_null_limit[q < q_min + margin_to_limit] += qd_null_limit_min[
-                q < q_min + margin_to_limit]
-
-            qd_null += qd_null_limit
-            # W J.T (J W J' + reg I)^-1 xd_d + (I - W J.T (J W J' + reg I)^-1 J qd_null
-            qd_d = np.linalg.solve(JwJ_reg, self.pgain * err - J.dot(qd_null))
-            # qd_d = self.pgain * err
-            qd_d = self.W.dot(J.transpose()).dot(qd_d) + qd_null
-
-            i += 1
-
-        # print("Final IK error (%d iterations):  %s" % (i, errNorm))
-        return q
-
-
-if __name__ == "__main__":
-    env = BOX_PUSH_Env_Gym(enable_gravity_comp=True, frame_skip=10)
-    env.reset()
-    for j in range(60):
-        old_obs = env.reset()
-        done = False
-        for _ in range(100):
-            env.render(mode='human')
-            # action = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
-            # action = np.array([0.0] * 7)
-            action = env.action_space.sample()
-            obs, reward, done, info = env.step(action)
-            qpos = env.sim.data.qpos
-            # qpos[:7] = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]
-            # qpos[:7] = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]
-            # qvel = env.sim.data.qvel
-            # qvel[:7] = [0, 0, 0, 0, 0, 0, 0]
-            # env.set_state(qpos, qvel)
-            # print("diff between old and new obs: ", np.linalg.norm(obs - old_obs))
-            old_obs = obs
-    print("========================================")

fancy_gym/envs/mujoco/box_pushing/box_push_utils.py

@@ -1,342 +0,0 @@
-import matplotlib.pyplot as plt
-
-import numpy as np
-import pandas as pd
-
-from gym.envs.mujoco.mujoco_env import MujocoEnv
-
-from scipy.interpolate import make_interp_spline
-
-# joint constraints for Franka robot
-q_max = np.array([2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973])
-q_min = np.array(
-    [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973])
-
-q_dot_max = np.array([2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100])
-q_dot_dot_max = np.array([90., 90., 90., 90., 12., 12., 12.])
-
-desired_rod_quat = np.array([0.0, 1.0, 0.0, 0.0])
-
-
-def get_quaternion_error(curr_quat, des_quat):
-    """
-    Calculates the difference between the current quaternion and the desired quaternion.
-    See Siciliano textbook page 140 Eq 3.91
-
-    :param curr_quat: current quaternion
-    :param des_quat: desired quaternion
-    :return: difference between current quaternion and desired quaternion
-    """
-    quatError = np.zeros((3, ))
-
-    quatError[0] = (curr_quat[0] * des_quat[1] - des_quat[0] * curr_quat[1] -
-                    curr_quat[3] * des_quat[2] + curr_quat[2] * des_quat[3])
-
-    quatError[1] = (curr_quat[0] * des_quat[2] - des_quat[0] * curr_quat[2] +
-                    curr_quat[3] * des_quat[1] - curr_quat[1] * des_quat[3])
-
-    quatError[2] = (curr_quat[0] * des_quat[3] - des_quat[0] * curr_quat[3] -
-                    curr_quat[2] * des_quat[1] + curr_quat[1] * des_quat[2])
-
-    return quatError
-
-
-def rotation_distance(p: np.array, q: np.array):
-    """
-    p: quaternion
-    q: quaternion
-    theta: rotation angle between p and q (rad)
-    """
-    assert p.shape == q.shape, "p and q should be quaternion"
-    product = p[0] * q[0] + p[1] * q[1] + p[2] * q[2] + p[3] * q[3]
-    theta = 2 * np.arccos(abs(product))
-    return theta
-
-
-def joint_limit_violate_penalty(joint_pos,
-                                joint_vel,
-                                enable_pos_limit=False,
-                                enable_vel_limit=False):
-    penalty = 0.
-    p_coeff = 1.
-    v_coeff = 1.
-    # q_limit
-    if enable_pos_limit:
-        higher_indice = np.where(joint_pos > q_max)
-        lower_indice = np.where(joint_pos < q_min)
-        higher_error = joint_pos - q_max
-        lower_error = q_min - joint_pos
-        penalty -= p_coeff * (abs(np.sum(higher_error[higher_indice])) +
-                              abs(np.sum(lower_error[lower_indice])))
-    # q_dot_limit
-    if enable_vel_limit:
-        q_dot_error = abs(joint_vel) - abs(q_dot_max)
-        q_dot_violate_idx = np.where(q_dot_error > 0.)
-        penalty -= v_coeff * abs(np.sum(q_dot_error[q_dot_violate_idx]))
-    return penalty
-
-
-def rot_to_quat(theta, axis):
-    quant = np.zeros(4)
-    quant[0] = np.sin(theta / 2.)
-    quant[1] = np.cos(theta / 2.) * axis[0]
-    quant[2] = np.cos(theta / 2.) * axis[1]
-    quant[3] = np.cos(theta / 2.) * axis[2]
-    return quant
-
-
-class PushBoxReward:
-    def __init__(self):
-        self.box_size = np.array([0.05, 0.05, 0.045])
-        self.step_reward_joint_penalty = []
-        self.step_reward_tcp = []
-        self.step_reward_pos = []
-        self.step_reward_rot = []
-        self.step_reward_flip = []
-        self.energy_cost = []
-
-    def get_reward_trajectory(self):
-        return np.array(self.step_reward_pos.copy()), np.array(
-            self.step_reward_rot.copy())
-
-    def reset(self):
-        self.step_reward_pos = []
-        self.step_reward_rot = []
-        self.step_reward_tcp = []
-        self.step_reward_joint_penalty = []
-        self.step_reward_flip = []
-        self.energy_cost = []
-
-    def step_reward(self, box_pos, box_quat, target_pos, target_quat,
-                    rod_tip_pos, rod_quat, qpos, qvel, action):
-
-        joint_penalty = joint_limit_violate_penalty(qpos,
-                                                    qvel,
-                                                    enable_pos_limit=True,
-                                                    enable_vel_limit=True)
-        tcp_box_dist_reward = -2 * np.clip(np.linalg.norm(box_pos - rod_tip_pos), 0.05, 100)
-        box_goal_pos_dist_reward = -3.5 * np.linalg.norm(box_pos - target_pos)
-        box_goal_rot_dist_reward = -rotation_distance(box_quat, target_quat) / np.pi
-        energy_cost = -0.0005 * np.sum(np.square(action))
-
-        reward = joint_penalty + tcp_box_dist_reward + \
-                 box_goal_pos_dist_reward + box_goal_rot_dist_reward + energy_cost
-
-        rod_inclined_angle = rotation_distance(rod_quat, desired_rod_quat)
-        if rod_inclined_angle > np.pi / 4:
-            reward -= rod_inclined_angle / (np.pi)
-
-        # self.step_reward_joint_penalty.append(joint_penalty)
-        # self.step_reward_tcp.append(tcp_box_dist_reward)
-        # self.step_reward_pos.append(box_goal_pos_dist_reward)
-        # self.step_reward_rot.append(box_goal_rot_dist_reward)
-        # self.energy_cost.append(energy_cost)
-        return reward
-
-    def sparse1_reward(self, episodic_end, box_pos, box_quat, target_pos,
-                        target_quat, rod_tip_pos, rod_quat, qpos, qvel, action):
-
-        reward = 0.
-        joint_penalty = joint_limit_violate_penalty(qpos, qvel, enable_pos_limit=True, enable_vel_limit=True)
-        energy_cost = -0.0005 * np.sum(np.square(action))
-        tcp_box_dist_reward = -2 * np.clip(np.linalg.norm(box_pos - rod_tip_pos), 0.05, 100)
-        reward += joint_penalty + tcp_box_dist_reward + energy_cost
-        rod_inclined_angle = rotation_distance(rod_quat, desired_rod_quat)
-
-        if rod_inclined_angle > np.pi / 4:
-            reward -= rod_inclined_angle / (np.pi)
-
-        if not episodic_end:
-            return reward
-
-        box_goal_dist = np.linalg.norm(box_pos - target_pos)
-
-        box_goal_pos_dist_reward = -3.5 * box_goal_dist * 100
-        box_goal_rot_dist_reward = -rotation_distance(box_quat, target_quat) / np.pi * 100
-
-        reward += box_goal_pos_dist_reward + box_goal_rot_dist_reward
-
-        return reward
-
-    def sparse2_reward(self, episodic_end, box_pos, box_quat, target_pos,
-                        target_quat, rod_tip_pos, rod_quat, qpos, qvel, action):
-
-        reward = 0.
-        joint_penalty = joint_limit_violate_penalty(qpos, qvel, enable_pos_limit=True, enable_vel_limit=True)
-        energy_cost = -0.0005 * np.sum(np.square(action))
-        tcp_box_dist_reward = -2 * np.clip(np.linalg.norm(box_pos - rod_tip_pos), 0.05, 100)
-        reward += joint_penalty + tcp_box_dist_reward + energy_cost
-        rod_inclined_angle = rotation_distance(rod_quat, desired_rod_quat)
-
-        if rod_inclined_angle > np.pi / 4:
-            reward -= rod_inclined_angle / (np.pi)
-
-        if not episodic_end:
-            return reward
-
-        box_goal_dist = np.linalg.norm(box_pos - target_pos)
-
-        if box_goal_dist < 0.1:
-            reward += 300
-            box_goal_pos_dist_reward = np.clip(- 3.5 * box_goal_dist * 100 * 3, -100, 0)
-            box_goal_rot_dist_reward = np.clip(- rotation_distance(box_quat, target_quat)/np.pi * 100 * 1.5, -100, 0)
-            reward += box_goal_pos_dist_reward + box_goal_rot_dist_reward
-
-        return reward
-
-    def plotRewards(self):
-        length = np.array(self.step_reward_pos).shape[0]
-        t = np.arange(0, length)
-        fig, axs = plt.subplots(1, 1)
-        axs.plot(t, self.step_reward_pos, label='pos')
-        axs.plot(t, self.step_reward_rot, label='rot')
-        axs.plot(t, self.step_reward_tcp, label='tcp')
-        axs.plot(t,
-                 np.array(self.step_reward_joint_penalty) / 0.5,
-                 label='joint_penalty')
-        # axs.plot(t, self.step_reward_flip, label='flip')
-        axs.plot(t, self.energy_cost, label='energy_cost')
-        plt.legend(loc='upper right')
-        plt.xlabel('Steps')
-        plt.title('Reward')
-        plt.show()
-
-class TrajectoryRecoder(object):
-    def __init__(self, sim, prefix="episodic", max_length=1000):
-        self.sim = sim
-        self.prefix = prefix
-        self.max_length = max_length
-        self.reset()
-
-    def update_sim(self, sim):
-        self.sim = sim
-
-    def reset(self):
-        self.trajectory = []
-        self.box_pos_dist_trajectory = []  # trajectory of box position distance to goal
-        self.box_rot_dist_trajectory = []  # trajectory of box rotation distance to goal
-        self.joint_pos_trajectory = []  # trajectory of joint position
-        self.joint_vel_trajectory = []  # trajectory of joint velocity
-        self.joint_torque_trajectory = []   # trajectory of joint torque
-        self.length = 0
-
-    def record(self):
-        if self.sim is None:
-            return
-
-        self.joint_pos_trajectory.append(self.sim.data.qpos[:7].copy())
-        self.joint_vel_trajectory.append(self.sim.data.qvel[:7].copy())
-        self.joint_torque_trajectory.append(self.sim.data.qfrc_actuator[:7].copy())
-
-        box_pos = self.sim.data.get_body_xpos("box_0")
-        box_quat = self.sim.data.get_body_xquat("box_0")
-        target_pos = self.sim.data.get_body_xpos("replan_target_pos")
-        target_quat = self.sim.data.get_body_xquat("replan_target_pos")
-
-        self.box_pos_dist_trajectory.append(np.linalg.norm(box_pos - target_pos))
-        self.box_rot_dist_trajectory.append(rotation_distance(box_quat, target_quat))
-
-        self.length += 1
-        if self.length > self.max_length:
-            self.joint_vel_trajectory.pop(0)
-            self.joint_pos_trajectory.pop(0)
-            self.joint_torque_trajectory.pop(0)
-            self.box_pos_dist_trajectory.pop(0)
-            self.box_rot_dist_trajectory.pop(0)
-            self.length -= 1
-
-    def get_trajectory(self):
-        return self.trajectory
-
-    def get_length(self):
-        return self.length
-
-    def plot_trajectories(self):
-        self.plot_trajectory(self.joint_pos_trajectory,
-                                   "joint_pos_trajectory")
-        self.plot_trajectory(self.joint_vel_trajectory,
-                                   "joint_vel_trajectory")
-        self.plot_trajectory(self.joint_torque_trajectory,
-                                   "joint_acc_trajectory")
-        self.plot_trajectory(self.box_pos_dist_trajectory,
-                                   "box_pos_dist_trajectory")
-        self.plot_trajectory(self.box_rot_dist_trajectory,
-                                   "box_rot_dist_trajectory")
-
-    def plot_trajectory(self, trajectory, title: str):
-        if len(trajectory) == 0:
-            return
-        trajectory = np.array(trajectory)
-        length = trajectory.shape[0]
-        t = np.arange(0, length)
-        dim = trajectory.shape[1] if len(trajectory.shape) > 1 else 1
-        fig, axs = plt.subplots(dim, 1, sharex=True)
-        if dim == 1:
-            axs.plot(t, trajectory)
-        else:
-            for i in range(dim):
-                axs[i].plot(t, trajectory[:, i])
-        # plt.legend(loc='upper right')
-        plt.xlabel('Steps')
-        plt.title(self.prefix + ": " + title)
-        plt.show()
-
-    def plot_box_trajectory(self):
-        pass
-
-    def save_trajectory(self, path: str):
-
-        joint_pos_trajectory = np.array(self.joint_pos_trajectory)
-        joint_vel_trajectory = np.array(self.joint_vel_trajectory)
-        joint_torque_trajectory = np.array(self.joint_torque_trajectory)
-        box_pos_dist_trajectory = np.array(self.box_pos_dist_trajectory)
-        box_rot_dist_trajectory = np.array(self.box_rot_dist_trajectory)
-        pd_dict = {}
-        if joint_pos_trajectory.shape[0] > 0:
-            for i in range(7):
-                pd_dict["qpos_" + str(i)] = joint_pos_trajectory[:, i]
-                pd_dict["qvel_" + str(i)] = joint_vel_trajectory[:, i]
-                pd_dict["qfrc_" + str(i)] = joint_torque_trajectory[:, i]
-            pd_dict["box_pos_dist"] = box_pos_dist_trajectory
-            pd_dict["box_rot_dist"] = box_rot_dist_trajectory
-
-            df = pd.DataFrame(pd_dict)
-            folder_path = "/home/i53/student/hongyi_zhou/py_ws/alr_envs/alr_envs/alr/mujoco/box_pushing/recorded_trajectory/"
-            save_path = folder_path + path + ".csv"
-            df.to_csv(save_path)
-
-def plot_trajectories(traj_dict:dict, traj_name:str):
-
-    fig, axs = plt.subplots(1, 1, sharex=True)
-
-    for key in traj_dict.keys():
-        t = traj_dict[key][traj_name].shape[0]
-        t = np.arange(0, t)
-        spline = make_interp_spline(t, traj_dict[key][traj_name])
-        t_ = np.linspace(t.min(), t.max(), 1000)
-        y_ = spline(t_)
-        axs.plot(t_, y_, label=key)
-
-    plt.legend(loc='upper right')
-    plt.title(traj_name)
-    plt.xlabel('Steps')
-    plt.show()
-
-
-if __name__ == "__main__":
-
-    ## sparse1 nodistcheck sparse2 withdistcheck
-    folder_path = "/home/i53/student/hongyi_zhou/py_ws/alr_envs/alr_envs/alr/mujoco/box_pushing/recorded_trajectory/"
-    exp_name = ["ppo_dense", "promp_dense_reward", "promp_sparse_nodistcheck", "promp_sparse_withdistcheck"]
-    # exp_name = ["promp_sparse_nodistcheck"]
-    trajectory_dict = { }
-    for name in exp_name:
-        data_path = folder_path + name + ".csv"
-        df = pd.read_csv(data_path)
-        trajectory_dict[name] = df
-
-    # plot_trajectories(trajectory_dict, "box_pos_dist")
-    # plot_trajectories(trajectory_dict, "box_rot_dist")
-    plot_trajectories(trajectory_dict, "qpos_6")
-    plot_trajectories(trajectory_dict, "qvel_6")
-    plot_trajectories(trajectory_dict, "qfrc_6")

fancy_gym/envs/mujoco/box_pushing/model/objects/assets/box.xml

@@ -1,9 +0,0 @@
-<mujocoinclude>
- <worldbody>
-  <body name="box" pos="0.5 0 0">
-   <freejoint name="box:joint"/>
-   <geom condim="4" name="box:geom1" pos="0 0 0" rgba="0.32 0.32 0.32 1" size="0.02 0.02 0.02" solimp="0.99 0.99 0.001"
-         solref="0.002 0.1" type="box" density="500"/>
-  </body>
- </worldbody>
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/objects/assets/box_with_lid.xml

@@ -1,14 +0,0 @@
-<mujoco model="box_with_lid">
-  <worldbody>
-     <body name="chest" pos="0.6 0.45 0.5" quat="1 0 0 1">
-         <geom pos="0 0 0.1" size="0.01 0.22 0.1" type="box" />
-         <geom pos="-0.22 0 0.1" size="0.01 0.22 0.1" type="box" />
-         <geom pos="-0.11 0.21 0.1" size="0.1 0.01 0.1" type="box" />
-         <geom pos="-0.11 -0.21 0.1" size="0.1 0.01 0.1" type="box" />
-         <body name="lid" pos="0 0 0.205">
-              <geom pos="-0.13 0 0.01" rgba="1 1 1 0.1" size="0.14 0.22 0.01" type="box" />
-              <joint axis="0 1 0" pos="0 0 0.01" type="hinge" />
-         </body>
-     </body>
-  </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/objects/assets/cubic_with_sites.xml

@@ -1,9 +0,0 @@
-<mujoco model="cubic_with_sites">
-  <worldbody>
-     <body name="cubic_with_sites" pos="0.5 0.0 0.0" quat="0 0 0 0">
-         <joint damping="0.01" name="cubic_with_sites:joint" type="free" />
-         <inertial diaginertia="3 3 3" mass=".1" pos="0 0 0 " />
-         <geom conaffinity="3" contype="3" density="0" name="cubic_with_sites:geom1" pos="0 0 0" rgba="0.4 0.65 0.89 1" size="0.02 0.02 0.02" type="box" />
-     </body>
-  </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/objects/assets/goal.xml

@@ -1,7 +0,0 @@
-<mujocoinclude>
- <worldbody>
-  <body name="goal" pos="0.5 0 0.1">
-   <site name="goal:site1" pos="0 0 0" rgba="1 0 0 1" size="0.01 0.01 0.01" type="sphere"/>
-  </body>
- </worldbody>
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/objects/assets/push_box.xml

@@ -1,12 +0,0 @@
-<mujoco model="push_box">
-    <worldbody>
-        <body name="box_0" pos="0.6 0.15 0.0" quat="1 0 0 0">
-            <geom pos="0 0 0" size="0.05 0.05 0.01" rgba="1 0 0 1.0" type="box" mass="2.0" friction="0.3 0.001 0.0001" priority="1"/>
-            <geom pos="0.05 0 0.0485" size="0.005 0.05 0.045" rgba="0 0 1 1.0" type="box" mass="0.001"/>
-            <geom pos="0 0.05 0.0485" size="0.05 0.005 0.045" rgba="1 0 0 1.0" type="box" mass="0.001"/>
-            <geom pos="-0.05 0 0.0485" size="0.005 0.05 0.045" rgba="1 0 0 1.0" type="box" mass="0.001"/>
-            <geom pos="0 -0.05 0.0485" size="0.05 0.005 0.045" rgba="1 0 0 1.0" type="box" mass="0.001"/>
-            <joint name="box_joint" type="free"/>
-        </body>
-    </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/objects/assets/tray.xml

@@ -1,11 +0,0 @@
-<mujoco model="tray">
-  <worldbody>
-         <body name="tray_body" pos="0.5 0 0.25" quat="0 0 0 0">
-             <geom mass="2000" pos="0 0 0.02" size="0.3 0.6 0.01" type="box" />
-             <geom pos="0 0.61 0.06" size="0.3 0.01 0.06" type="box" />
-             <geom pos="0 -0.61 0.06" size="0.3 0.01 0.06" type="box" />
-             <geom pos="0.31 0 0.06" size="0.01 0.62 0.06" type="box" />
-             <geom pos="-0.31 0 0.06" size="0.01 0.62 0.06" type="box" />
-         </body>
-  </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/robots/controller/panda_mocap_control.xml

@@ -1,15 +0,0 @@
-<mujocoinclude>
-<worldbody>
-    <body mocap="true" name="panda:mocap" pos="0 0 0">
-      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7" size="0.005 0.005 0.005" type="box" />
-<!--      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="1 0 0 0.3" size="1 0.005 0.005" type="box" />-->
-<!--      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 1 0 0.3" size="0.005 1 0.005" type="box" />-->
-<!--      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0 1 0.3" size="0.005 0.005 1" type="box" />-->
-    </body>
-  </worldbody>
-    <equality>
-		<weld body1="panda:mocap" body2="tcp" solref="0.001 1"/>
-    </equality>
-</mujocoinclude>
-
-

fancy_gym/envs/mujoco/box_pushing/model/robots/controller/panda_position_control.xml

@@ -1,50 +0,0 @@
-<mujocoinclude>
- <actuator>
-  <position class="panda" ctrlrange="-2.9671 2.9671" forcerange="-87 87" joint="panda_joint1" kp="8700.0"
-            name="panda_joint1"/>
-  <position class="panda" ctrlrange="-1.8326 1.8326" forcerange="-87 87" joint="panda_joint2" kp="8700.0"
-            name="panda_joint2"/>
-  <position class="panda" ctrlrange="-2.9671 2.9671" forcerange="-87 87" joint="panda_joint3" kp="8700.0"
-            name="panda_joint3"/>
-  <position class="panda" ctrlrange="-3.1416 0.0" forcerange="-87 87" joint="panda_joint4" kp="8700.0"
-            name="panda_joint4"/>
-  <position class="panda" ctrlrange="-2.9671 2.9671" forcerange="-12 12" joint="panda_joint5" kp="1200.0"
-            name="panda_joint5"/>
-  <position class="panda" ctrlrange="-3.7525 2.1817" forcerange="-12 12" joint="panda_joint6" kp="1200.0"
-            name="panda_joint6"/>
-  <position class="panda" ctrlrange="-2.9671 2.9671" forcerange="-12 12" joint="panda_joint7" kp="1200.0"
-            name="panda_joint7"/>
- </actuator>
-
- <default>
-  <default class="panda">
-   <joint axis="0 0 1" limited="true" pos="0 0 0"/>
-   <position ctrllimited="true" forcelimited="true" user="1002 40 2001 -0.005 0.005"/>
-   <default class="panda_viz">
-    <geom conaffinity="0" contype="0" group="0" mass="0" rgba=".95 .99 .92 1" type="mesh"/>
-   </default>
-
-   <default class="panda_col">
-    <geom conaffinity="1" contype="1" group="3" rgba=".5 .6 .7 1" type="mesh"/>
-   </default>
-   <default class="panda_arm">
-    <joint damping="100"/>
-   </default>
-   <default class="panda_forearm">
-    <joint damping="10"/>
-   </default>
-   <default class="panda_finger">
-    <joint armature="5" damping="100"/>
-    <geom conaffinity="0" condim="6" contype="1" friction="1 0.5 0.0001" group="3" margin="0.001" rgba="0.5 0.6 0.7 .4"
-          solimp="0.8 0.9 0.001" solref="0.01 1" user="0"/>
-    <position user="1002 40 2001 -0.0001 0.0001"/>
-   </default>
-  </default>
-
-  <default class="panda_overlay">
-   <joint armature="1" damping="1000" frictionloss="10" limited="false"/>
-   <geom conaffinity="0" contype="0" group="2" rgba=".42 0.42 0.42 .5" type="mesh"/>
-  </default>
- </default>
-
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/robots/controller/panda_torque_control.xml

@@ -1,11 +0,0 @@
-<mujoco model="torque">
- <actuator>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint1" name="panda_joint1_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint2" name="panda_joint2_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint3" name="panda_joint3_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint4" name="panda_joint4_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint5" name="panda_joint5_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint6" name="panda_joint6_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint7" name="panda_joint7_act"/>
- </actuator>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/robots/controller/panda_velocity_control.xml

@@ -1,13 +0,0 @@
-<mujocoinclude>
- <actuator>
-  <velocity ctrlrange="-2.1750 2.1750" forcerange="-87 87" joint="panda_joint1" kv="0.8" name="panda_joint1"/>
-  <velocity ctrlrange="-2.1750 2.1750" forcerange="-87 87" joint="panda_joint2" kv="0.8" name="panda_joint2"/>
-  <velocity ctrlrange="-2.1750 2.1750" forcerange="-87 87" joint="panda_joint3" kv="0.8" name="panda_joint3"/>
-  <velocity ctrlrange="-2.1750 2.1750" forcerange="-87 87" joint="panda_joint4" kv="0.8" name="panda_joint4"/>
-  <velocity ctrlrange="-2.6100 2.6100" forcerange="-12 12" joint="panda_joint5" kv="0.8" name="panda_joint5"/>
-  <velocity ctrlrange="-2.6100 2.6100" forcerange="-12 12" joint="panda_joint6" kv="0.8" name="panda_joint6"/>
-  <velocity ctrlrange="-2.9671 2.9671" forcerange="-12 12" joint="panda_joint7" kv="0.8" name="panda_joint7"/>
- </actuator>
-
-
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/robots/depr/panda_gym.xml

@@ -1,157 +0,0 @@
-<mujoco model="panda">
- <compiler angle="radian" discardvisual="false" meshdir="../meshes_panda/for_mujoco/"/>
- <option cone="elliptic" impratio="20" timestep="0.002">
- </option>
- <size nconmax="2000" njmax="2000"/>
- <asset>
-  <mesh file="link0v.stl" name="link0v"/>
-  <mesh file="link1v.stl" name="link1v"/>
-  <mesh file="link2v.stl" name="link2v"/>
-  <mesh file="link3v.stl" name="link3v"/>
-  <mesh file="link4v.stl" name="link4v"/>
-  <mesh file="link5v.stl" name="link5v"/>
-  <mesh file="link6v.stl" name="link6v"/>
-  <mesh file="link7v.stl" name="link7v"/>
-  <mesh file="handv.stl" name="handv"/>
-  <mesh file="fingerv.stl" name="fingerv"/>
-  <texture builtin="gradient" height="32" rgb1="0.26 0.58 0.51" rgb2="0.26 0.58 0.51" type="skybox" width="32"/>
- </asset>
- <contact>
-  <exclude body1="panda_link0" body2="panda_link1"/>
-  <exclude body1="panda_link1" body2="panda_link2"/>
-  <exclude body1="panda_link2" body2="panda_link3"/>
-  <exclude body1="panda_link3" body2="panda_link4"/>
-  <exclude body1="panda_link4" body2="panda_link5"/>
-  <exclude body1="panda_link5" body2="panda_link6"/>
-  <exclude body1="panda_link6" body2="panda_link7"/>
-  <exclude body1="panda_link7" body2="panda_link8"/>
-  <exclude body1="panda_link8" body2="panda_hand"/>
- </contact>
- <visual>
-  <map zfar="1000" znear="0.001"/>
- </visual>
- <worldbody>
-  <light castshadow="false" cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3"
-         specular=".1 .1 .1"/>
-  <body name="ground" pos="0 0 -0.94">
-   <geom name="ground:geom1" rgba="0.26 0.58 0.51 1" size="0 0 1" type="plane"/>
-  </body>
-  <body name="panda_link0" pos="0 0 0">
-   <inertial diaginertia="0.0126801 0.0117603 0.00856656" mass="3.01399" pos="-0.0291898 -0.000879465 0.0566032"
-             quat="0.00411744 0.564916 0.0132875 0.825031"/>
-   <geom conaffinity="0" contype="0" group="1" mesh="link0v" name="panda_link0:geom1" rgba="1 1 1 1" type="mesh"/>
-   <geom mesh="link0v" name="panda_link0:geom2" rgba="1 1 1 1" type="mesh"/>
-   <body childclass="panda:body" name="panda_link1" pos="0 0 0.333">
-    <inertial diaginertia="0.0164224 0.0153969 0.00546286" mass="2.77281" pos="1.1399e-05 -0.0312655 -0.0693733"
-              quat="0.98466 0.174481 -0.000101815 0.000347662"/>
-    <joint axis="0 0 1"  limited="true" name="panda_joint1" pos="0 0 0" range="-2.9671 2.9671"/>
-    <geom conaffinity="0" contype="0" group="1" mesh="link1v" name="panda_joint1:geom1" rgba="1 1 1 1" type="mesh"/>
-    <geom mesh="link1v" name="panda_joint1:geom2" rgba="1 1 1 1" type="mesh"/>
-    <body childclass="panda:body" name="panda_link2" pos="0 0 0" quat="0.707107 -0.707107 0 0">
-     <inertial diaginertia="0.016787 0.0157415 0.00553027" mass="2.7996" pos="-1.31766e-05 -0.0703216 0.0311782"
-               quat="0.57484 0.818266 -6.05764e-05 -6.61626e-05"/>
-     <joint axis="0 0 1"  limited="true" name="panda_joint2" pos="0 0 0"
-            range="-1.8326                     1.8326"/>
-     <geom conaffinity="0" contype="0" group="1" mesh="link2v" name="panda_joint2:geom1" rgba="1 1 1 1" type="mesh"/>
-     <geom mesh="link2v" name="panda_joint2:geom2" rgba="1 1 1 1" type="mesh"/>
-     <body childclass="panda:body" name="panda_link3" pos="0 -0.316 0" quat="0.707107 0.707107 0 0">
-      <inertial diaginertia="0.00915257 0.00896477 0.00384742" mass="2.14603" pos="0.0443483 0.0249283 -0.03813"
-                quat="0.0615263 0.349824 0.234291 0.904956"/>
-      <joint axis="0 0 1"  limited="true" name="panda_joint3" pos="0 0 0" range="-2.9671 2.9671"/>
-      <geom conaffinity="0" contype="0" group="1" mesh="link3v" name="panda_joint3:geom1" rgba="1 1 1 1" type="mesh"/>
-      <geom mesh="link3v" name="panda_joint3:geom2" rgba="1 1 1 1" type="mesh"/>
-      <body childclass="panda:body" name="panda_link4" pos="0.0825 0 0" quat="0.707107 0.707107 0 0">
-       <inertial diaginertia="0.00946899 0.00928491 0.00396694" mass="2.18807" pos="-0.0385503 0.0395256 0.0247162"
-                 quat="0.813566 0.465041 0.309792 0.160858"/>
-       <joint axis="0 0 1"  limited="true" name="panda_joint4" pos="0 0 0" range="-3.1416 0.0873"/>
-       <geom conaffinity="0" contype="0" group="1" mesh="link4v" name="panda_link4:geom1" rgba="1 1 1 1" type="mesh"/>
-       <geom mesh="link4v" name="panda_link4:geom2" rgba="1 1 1 1" type="mesh"/>
-       <body childclass="panda:body" name="panda_link5" pos="-0.0825 0.384 0" quat="0.707107 -0.707107 0 0">
-        <inertial diaginertia="0.0278873 0.0268823 0.00569569" mass="3.19545" pos="-6.36776e-05 0.0384124 -0.10997"
-                  quat="0.990767 -0.135571 0.000963106 0.000694406"/>
-        <joint axis="0 0 1"  limited="true" name="panda_joint5" pos="0 0 0" range="-2.9671 2.9671"/>
-        <geom conaffinity="0" contype="0" group="1" mesh="link5v" name="panda_link5:geom1" rgba="1 1 1 1" type="mesh"/>
-        <geom mesh="link5v" name="panda_link5:geom2" rgba="1 1 1 1" type="mesh"/>
-        <body childclass="panda:body" name="panda_link6" pos="0 0 0" quat="0.707107 0.707107 0 0">
-         <inertial diaginertia="0.00412168 0.0033698 0.00213304" mass="1.35761" pos="0.0510023 0.00693267 0.00616899"
-                   quat="-0.0460841 0.754362 0.044494 0.653325"/>
-         <joint axis="0 0 1"  limited="true" name="panda_joint6" pos="0 0 0" range="-0.0873 3.8223"/>
-         <geom conaffinity="0" contype="0" group="1" mesh="link6v" name="panda_link6:geom1" rgba="1 1 1 1" type="mesh"/>
-         <geom mesh="link6v" name="panda_link6:geom2" rgba="1 1 1 1" type="mesh"/>
-         <body childclass="panda:body" name="panda_link7" pos="0.088 0 0" quat="0.707107 0.707107 0 0">
-          <inertial diaginertia="0.000637671 0.000528056 0.000279577" mass="0.417345"
-                    pos="0.0103614 0.0103596 0.0791078" quat="0.63547 0.278021 -0.670462 0.263369"/>
-          <joint axis="0 0 1"  limited="true" name="panda_joint7" pos="0 0 0" range="-2.9671 2.9671"/>
-          <geom conaffinity="0" contype="0" group="1" mesh="link7v" name="panda_link7:geom1" rgba="1 1 1 1"
-                type="mesh"/>
-          <geom mesh="link7v" name="panda_link7:geom2" rgba="1 1 1 1" solimp="1.998 1.999 0" type="mesh"/>
-          <body name="panda_link8" pos="0 0 0.107">
-           <inertial diaginertia="0.1 0.1 0.1" mass="0.1" pos="0 0 0"/>
-           <body name="panda_hand" pos="0 0 0" quat="0.92388 0 0 -0.382683">
-            <inertial diaginertia="0.00227632 0.00206087 0.000456542" mass="0.670782"
-                      pos="-2.76618e-06 -0.00061547 0.0239295" quat="0.697945 0.716151 -0.000242485 8.47563e-05"/>
-            <geom conaffinity="0" contype="0" group="1" mesh="handv" name="panda_hand:geom1" type="mesh"/>
-            <geom mesh="handv" name="panda_hand:geom2" solimp="1.998 1.999 0" type="mesh"/>
-            <camera euler="0 3.14 0" fovy="60" ipd="0.0" mode="fixed" name="rgbd" pos="0.1 0.0 0."/>
-            <body name="tcp" pos="0 0 0.1">
-             <site name="tcp" rgba="0 0 1 0" size="0.001"/>
-            </body>
-            <body childclass="panda:gripper" name="panda_leftfinger" pos="0 0 0.0584">
-               <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754" pos="-2.42335e-05 0.0119585 0.0237816" quat="0.996177 0.0868631 -2.79377e-05 -0.00926642" />
-               <joint axis="0 1 0" name="panda_finger_joint1" />
-               <site name="panda_leftfinger:site" pos="0 0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012" type="box" />
-               <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_leftfinger:geom1" type="mesh" />
-               <geom mesh="fingerv" name="panda_leftfinger:geom2" type="mesh" />
-               <body name="finger_joint1_tip" pos="0 0.0085 0.056">
-                 <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1" />
-                 <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger1_tip_collision" pos="0 -0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5" type="box" />
-                 <geom conaffinity="1" contype="1" name="touch_left" rgba="0 1 0 1" pos="0 -0.005 -0.012"
-                       quat="1 1 0 0" size="0.004 0.004" type="cylinder"/>
-               </body>
-             </body>
-             <body childclass="panda:gripper" name="panda_rightfinger" pos="0 0 0.0584">
-               <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754" pos="2.42335e-05 -0.0119585 0.0237816" quat="0.996177 -0.0868631 2.79377e-05 -0.00926642" />
-               <joint axis="0 -1 0" name="panda_finger_joint2" />
-               <site name="panda_rightfinger:site" pos="0 -0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012" type="box" />
-               <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_rightfinger:geom1" quat="0 0 0 1" type="mesh" />
-               <geom mesh="fingerv" name="panda_rightfinger:geom2" quat="0 0 0 1" type="mesh" />
-               <body name="finger_joint2_tip" pos="0 -0.0085 0.056">
-                 <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1" />
-                 <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger2_tip_collision" pos="0 0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5" type="box" />
-                 <geom conaffinity="1" contype="1" name="touch_right" rgba="0 1 0 1" pos="0 0.005 -0.012"
-                       quat="1 1 0 0" size="0.004 0.004" type="cylinder"/>
-               </body>
-            </body>
-           </body>
-          </body>
-         </body>
-        </body>
-       </body>
-      </body>
-     </body>
-    </body>
-   </body>
-  </body>
-  <camera euler="0 0 0" fovy="45" ipd="0.0" mode="fixed" name="rgbd_cage" pos="0.7 0.1 0.9"/>
- </worldbody>
- <sensor>
-  <touch name="touchsensor:left" site="panda_leftfinger:site"/>
-  <touch name="touchsensor:right" site="panda_rightfinger:site"/>
- </sensor>
- <default>
-  <default class="panda:body">
-   <joint damping="0" />
-  </default>
-  <default class="panda:gripper">
-   <geom condim="4" solimp="0.999 0.999 0.0001" type="box"/>
-   <joint armature="10" damping="300" limited="true" range="0.001 0.04" type="slide"/>
-  </default>
- </default>
- <actuator>
-  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"
-            joint="panda_finger_joint1" kp="1000000" name="panda_finger_joint1_act"/>
-  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"
-            joint="panda_finger_joint2" kp="1000000" name="panda_finger_joint2_act"/>
- </actuator>
- <include file="../assets/panda_frame.xml"/>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/robots/panda.xml

@@ -1,155 +0,0 @@
-<mujocoinclude>
-    <compiler angle="radian" discardvisual="false" meshdir="../robots/meshes/panda/"/>
- <asset>
-  <mesh file="link0v.stl" name="link0v"/>
-  <mesh file="link1v.stl" name="link1v"/>
-  <mesh file="link2v.stl" name="link2v"/>
-  <mesh file="link3v.stl" name="link3v"/>
-  <mesh file="link4v.stl" name="link4v"/>
-  <mesh file="link5v.stl" name="link5v"/>
-  <mesh file="link6v.stl" name="link6v"/>
-  <mesh file="link7v.stl" name="link7v"/>
-  <mesh file="handv.stl" name="handv"/>
-  <mesh file="fingerv.stl" name="fingerv"/>
- </asset>
- <contact>
-  <exclude body1="panda_link0" body2="panda_link1"/>
-  <exclude body1="panda_link1" body2="panda_link2"/>
-  <exclude body1="panda_link2" body2="panda_link3"/>
-  <exclude body1="panda_link3" body2="panda_link4"/>
-  <exclude body1="panda_link4" body2="panda_link5"/>
-  <exclude body1="panda_link5" body2="panda_link6"/>
-  <exclude body1="panda_link6" body2="panda_link7"/>
-  <exclude body1="panda_link7" body2="panda_link8"/>
-  <exclude body1="panda_link8" body2="panda_hand"/>
- </contact>
- <worldbody>
-  <body name="panda_link0" pos="0 0 0">
-   <inertial diaginertia="0.0126801 0.0117603 0.00856656" mass="3.01399" pos="-0.0291898 -0.000879465 0.0566032"
-             quat="0.00411744 0.564916 0.0132875 0.825031"/>
-   <geom conaffinity="0" contype="0" group="1" mesh="link0v" name="panda_link0:geom1" rgba="1 1 1 1" type="mesh"/>
-   <geom mesh="link0v" name="panda_link0:geom2" rgba="1 1 1 1" type="mesh"/>
-   <body name="panda_link1" pos="0 0 0.333">
-    <inertial diaginertia="0.0164224 0.0153969 0.00546286" mass="2.77281" pos="1.1399e-05 -0.0312655 -0.0693733"
-              quat="0.98466 0.174481 -0.000101815 0.000347662"/>
-    <joint axis="0 0 1" limited="true" name="panda_joint1" pos="0 0 0" range="-2.9671 2.9671"/>
-    <geom conaffinity="0" contype="0" group="1" mesh="link1v" name="panda_joint1:geom1" rgba="1 1 1 1" type="mesh"/>
-    <geom mesh="link1v" name="panda_joint1:geom2" rgba="1 1 1 1" type="mesh"/>
-    <body name="panda_link2" pos="0 0 0" quat="0.707107 -0.707107 0 0">
-     <inertial diaginertia="0.016787 0.0157415 0.00553027" mass="2.7996" pos="-1.31766e-05 -0.0703216 0.0311782"
-               quat="0.57484 0.818266 -6.05764e-05 -6.61626e-05"/>
-     <joint axis="0 0 1" limited="true" name="panda_joint2" pos="0 0 0" range="-1.8326
-                    1.8326"/>
-     <geom conaffinity="0" contype="0" group="1" mesh="link2v" name="panda_joint2:geom1" rgba="1 1 1 1" type="mesh"/>
-     <geom mesh="link2v" name="panda_joint2:geom2" rgba="1 1 1 1" type="mesh"/>
-     <body name="panda_link3" pos="0 -0.316 0" quat="0.707107 0.707107 0 0">
-      <inertial diaginertia="0.00915257 0.00896477 0.00384742" mass="2.14603" pos="0.0443483 0.0249283 -0.03813"
-                quat="0.0615263 0.349824 0.234291 0.904956"/>
-      <joint axis="0 0 1" limited="true" name="panda_joint3" pos="0 0 0" range="-2.9671 2.9671"/>
-      <geom conaffinity="0" contype="0" group="1" mesh="link3v" name="panda_joint3:geom1" rgba="1 1 1 1" type="mesh"/>
-      <geom mesh="link3v" name="panda_joint3:geom2" rgba="1 1 1 1" type="mesh"/>
-      <body name="panda_link4" pos="0.0825 0 0" quat="0.707107 0.707107 0 0">
-       <inertial diaginertia="0.00946899 0.00928491 0.00396694" mass="2.18807" pos="-0.0385503 0.0395256 0.0247162"
-                 quat="0.813566 0.465041 0.309792 0.160858"/>
-       <joint axis="0 0 1" limited="true" name="panda_joint4" pos="0 0 0" range="-3.1416 0.0873"/>
-       <geom conaffinity="0" contype="0" group="1" mesh="link4v" name="panda_link4:geom1" rgba="1 1 1 1" type="mesh"/>
-       <geom mesh="link4v" name="panda_link4:geom2" rgba="1 1 1 1" type="mesh"/>
-       <body name="panda_link5" pos="-0.0825 0.384 0" quat="0.707107 -0.707107 0 0">
-        <inertial diaginertia="0.0278873 0.0268823 0.00569569" mass="3.19545" pos="-6.36776e-05 0.0384124 -0.10997"
-                  quat="0.990767 -0.135571 0.000963106 0.000694406"/>
-        <joint axis="0 0 1" limited="true" name="panda_joint5" pos="0 0 0" range="-2.9671 2.9671"/>
-        <geom conaffinity="0" contype="0" group="1" mesh="link5v" name="panda_link5:geom1" rgba="1 1 1 1" type="mesh"/>
-        <geom mesh="link5v" name="panda_link5:geom2" rgba="1 1 1 1" type="mesh"/>
-        <body name="panda_link6" pos="0 0 0" quat="0.707107 0.707107 0 0">
-         <inertial diaginertia="0.00412168 0.0033698 0.00213304" mass="1.35761" pos="0.0510023 0.00693267 0.00616899"
-                   quat="-0.0460841 0.754362 0.044494 0.653325"/>
-         <joint axis="0 0 1" limited="true" name="panda_joint6" pos="0 0 0" range="-0.0873 3.8223"/>
-         <geom conaffinity="0" contype="0" group="1" mesh="link6v" name="panda_link6:geom1" rgba="1 1 1 1" type="mesh"/>
-         <geom mesh="link6v" name="panda_link6:geom2" rgba="1 1 1 1" type="mesh"/>
-         <body name="panda_link7" pos="0.088 0 0" quat="0.707107 0.707107 0 0">
-          <inertial diaginertia="0.000637671 0.000528056 0.000279577" mass="0.417345"
-                    pos="0.0103614 0.0103596 0.0791078" quat="0.63547 0.278021 -0.670462 0.263369"/>
-          <joint axis="0 0 1" limited="true" name="panda_joint7" pos="0 0 0" range="-2.9671 2.9671"/>
-          <geom conaffinity="0" contype="0" group="1" mesh="link7v" name="panda_link7:geom1" rgba="1 1 1 1"
-                type="mesh"/>
-          <geom mesh="link7v" name="panda_link7:geom2" rgba="1 1 1 1" solimp="1.998 1.999 0" type="mesh"/>
-          <body name="panda_link8" pos="0 0 0.107">
-           <inertial diaginertia="0.1 0.1 0.1" mass="0.1" pos="0 0 0"/>
-           <body name="panda_hand" pos="0 0 0" quat="0.92388 0 0 -0.382683">
-            <inertial diaginertia="0.00227632 0.00206087 0.000456542" mass="0.670782"
-                      pos="-2.76618e-06 -0.00061547 0.0239295" quat="0.697945 0.716151 -0.000242485 8.47563e-05"/>
-            <geom conaffinity="0" contype="0" group="1" mesh="handv" name="panda_hand:geom1" type="mesh"/>
-            <geom mesh="handv" name="panda_hand:geom2" solimp="1.998 1.999 0" type="mesh"/>
-            <camera euler="0 3.14 0" fovy="60" ipd="0.0" mode="fixed" name="rgbd" pos="0.1 0.0 0."/>
-            <body name="tcp" pos="0 0 0.105">
-             <site name="tcp" rgba="'0 0 1 0'" size='0.001'/>
-            </body>
-            <body childclass="panda:gripper" name="panda_leftfinger" pos="0 0 0.0584">
-             <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754"
-                       pos="-2.42335e-05 0.0119585 0.0237816" quat="0.996177 0.0868631 -2.79377e-05 -0.00926642"/>
-             <joint axis="0 1 0" name="panda_finger_joint1"/>
-             <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_leftfinger:geom1" type="mesh"/>
-             <geom mesh="fingerv" name="panda_leftfinger:geom2" type="mesh"/>
-             <site name="panda_leftfinger:site" pos="0 0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012" type="box"/>
-             <body name="finger_joint1_tip" pos="0 0.0085 0.056">
-              <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1"/>
-              <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger1_tip_collision"
-                    pos="0 -0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5"
-                    type="box"/>
-             </body>
-            </body>
-            <body childclass="panda:gripper" name="panda_rightfinger" pos="0 0 0.0584">
-             <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754"
-                       pos="2.42335e-05 -0.0119585 0.0237816" quat="0.996177 -0.0868631 2.79377e-05 -0.00926642"/>
-             <joint axis="0 -1 0" name="panda_finger_joint2"/>
-             <site name="panda_rightfinger:site" pos="0 -0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012"
-                   type="box"/>
-             <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_rightfinger:geom1" quat="0 0 0 1"
-                   type="mesh"/>
-             <geom condim="4" mesh="fingerv" name="panda_rightfinger:geom2" quat="0 0 0 1" type="mesh"/>
-             <body name="finger_joint2_tip" pos="0 -0.0085 0.056">
-              <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1"/>
-              <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger2_tip_collision"
-                    pos="0 0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5"
-                    type="box"/>
-             </body>
-            </body>
-           </body>
-          </body>
-         </body>
-        </body>
-       </body>
-      </body>
-     </body>
-    </body>
-   </body>
-  </body>
-  <camera euler="0 0 0" fovy="45" ipd="0.0" mode="fixed" name="rgbd_cage" pos="0.7 0.1 0.9"/>
- </worldbody>
- <sensor>
-  <touch name="touchsensor:left" site="panda_leftfinger:site"/>
-  <touch name="touchsensor:right" site="panda_rightfinger:site"/>
- </sensor>
- <default>
-  <default class="panda:gripper">
-   <geom condim="4" friction="1 0.005 0.0001" margin="0.001" solimp="0.998 0.999 0.001" solref="0.02 1" type="box"/>
-   <joint damping="10" limited="true" range="0 0.04" type="slide"/>
-  </default>
- </default>
-  <actuator>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint1" name="panda_joint1_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint2" name="panda_joint2_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint3" name="panda_joint3_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint4" name="panda_joint4_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint5" name="panda_joint5_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint6" name="panda_joint6_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint7" name="panda_joint7_act"/>
-  <motor forcelimited="true" forcerange="-70 70" joint="panda_finger_joint1" name="panda_finger_joint1_act"/>
-  <motor forcelimited="true" forcerange="-70 70" joint="panda_finger_joint2" name="panda_finger_joint2_act"/>
-  <!--<position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"
-            joint="panda_finger_joint1" kp="1000000" name="panda_finger_joint1_act"/>
-  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"
-            joint="panda_finger_joint2" kp="1000000" name="panda_finger_joint2_act"/>-->
-  
- </actuator>
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/robots/panda_bimanual.xml

@@ -1 +0,0 @@
-<!--todo-->

fancy_gym/envs/mujoco/box_pushing/model/robots/panda_mocap.xml

@@ -1,140 +0,0 @@
-<mujocoinclude>
- <compiler angle="radian" discardvisual="false" meshdir="../robots/meshes/panda/"/>
- <asset>
-  <mesh file="link0v.stl" name="link0v"/>
-  <mesh file="link1v.stl" name="link1v"/>
-  <mesh file="link2v.stl" name="link2v"/>
-  <mesh file="link3v.stl" name="link3v"/>
-  <mesh file="link4v.stl" name="link4v"/>
-  <mesh file="link5v.stl" name="link5v"/>
-  <mesh file="link6v.stl" name="link6v"/>
-  <mesh file="link7v.stl" name="link7v"/>
-  <mesh file="handv.stl" name="handv"/>
-  <mesh file="fingerv.stl" name="fingerv"/>
- </asset>
- <contact>
-  <exclude body1="panda_link0" body2="panda_link1"/>
-  <exclude body1="panda_link1" body2="panda_link2"/>
-  <exclude body1="panda_link2" body2="panda_link3"/>
-  <exclude body1="panda_link3" body2="panda_link4"/>
-  <exclude body1="panda_link4" body2="panda_link5"/>
-  <exclude body1="panda_link5" body2="panda_link6"/>
-  <exclude body1="panda_link6" body2="panda_link7"/>
-  <exclude body1="panda_link7" body2="panda_link8"/>
-  <exclude body1="panda_link8" body2="panda_hand"/>
- </contact>
- <visual>
-  <map zfar="1000" znear="0.001"/>
- </visual>
- <worldbody>
-  <body name="panda_link0" pos="0 0 0">
-   <inertial diaginertia="0.0126801 0.0117603 0.00856656" mass="3.01399" pos="-0.0291898 -0.000879465 0.0566032" quat="0.00411744 0.564916 0.0132875 0.825031"/>
-   <geom conaffinity="0" contype="0" group="1" mesh="link0v" name="panda_link0:geom1" rgba="1 1 1 1" type="mesh"/>
-   <geom mesh="link0v" name="panda_link0:geom2" rgba="1 1 1 1" type="mesh"/>
-   <body name="panda_link1" pos="0 0 0.333">
-    <inertial diaginertia="0.0164224 0.0153969 0.00546286" mass="2.77281" pos="1.1399e-05 -0.0312655 -0.0693733" quat="0.98466 0.174481 -0.000101815 0.000347662"/>
-    <joint axis="0 0 1" damping="300" limited="true" name="panda_joint1" pos="0 0 0" range="-2.9671 2.9671"/>
-    <geom conaffinity="0" contype="0" group="1" mesh="link1v" name="panda_joint1:geom1" rgba="1 1 1 1" type="mesh"/>
-    <geom mesh="link1v" name="panda_joint1:geom2" rgba="1 1 1 1" type="mesh"/>
-    <body name="panda_link2" pos="0 0 0" quat="0.707107 -0.707107 0 0">
-     <inertial diaginertia="0.016787 0.0157415 0.00553027" mass="2.7996" pos="-1.31766e-05 -0.0703216 0.0311782" quat="0.57484 0.818266 -6.05764e-05 -6.61626e-05"/>
-     <joint axis="0 0 1" damping="300" limited="true" name="panda_joint2" pos="0 0 0" range="-1.8326                     1.8326"/>
-     <geom conaffinity="0" contype="0" group="1" mesh="link2v" name="panda_joint2:geom1" rgba="1 1 1 1" type="mesh"/>
-     <geom mesh="link2v" name="panda_joint2:geom2" rgba="1 1 1 1" type="mesh"/>
-     <body name="panda_link3" pos="0 -0.316 0" quat="0.707107 0.707107 0 0">
-      <inertial diaginertia="0.00915257 0.00896477 0.00384742" mass="2.14603" pos="0.0443483 0.0249283 -0.03813" quat="0.0615263 0.349824 0.234291 0.904956"/>
-      <joint axis="0 0 1" damping="300" limited="true" name="panda_joint3" pos="0 0 0" range="-2.9671 2.9671"/>
-      <geom conaffinity="0" contype="0" group="1" mesh="link3v" name="panda_joint3:geom1" rgba="1 1 1 1" type="mesh"/>
-      <geom mesh="link3v" name="panda_joint3:geom2" rgba="1 1 1 1" type="mesh"/>
-      <body name="panda_link4" pos="0.0825 0 0" quat="0.707107 0.707107 0 0">
-       <inertial diaginertia="0.00946899 0.00928491 0.00396694" mass="2.18807" pos="-0.0385503 0.0395256 0.0247162" quat="0.813566 0.465041 0.309792 0.160858"/>
-       <joint axis="0 0 1" damping="300" limited="true" name="panda_joint4" pos="0 0 0" range="-3.1416 0.0873"/>
-       <geom conaffinity="0" contype="0" group="1" mesh="link4v" name="panda_link4:geom1" rgba="1 1 1 1" type="mesh"/>
-       <geom mesh="link4v" name="panda_link4:geom2" rgba="1 1 1 1" type="mesh"/>
-       <body name="panda_link5" pos="-0.0825 0.384 0" quat="0.707107 -0.707107 0 0">
-        <inertial diaginertia="0.0278873 0.0268823 0.00569569" mass="3.19545" pos="-6.36776e-05 0.0384124 -0.10997" quat="0.990767 -0.135571 0.000963106 0.000694406"/>
-        <joint axis="0 0 1" damping="300" limited="true" name="panda_joint5" pos="0 0 0" range="-2.9671 2.9671"/>
-        <geom conaffinity="0" contype="0" group="1" mesh="link5v" name="panda_link5:geom1" rgba="1 1 1 1" type="mesh"/>
-        <geom mesh="link5v" name="panda_link5:geom2" rgba="1 1 1 1" type="mesh"/>
-        <body name="panda_link6" pos="0 0 0" quat="0.707107 0.707107 0 0">
-         <inertial diaginertia="0.00412168 0.0033698 0.00213304" mass="1.35761" pos="0.0510023 0.00693267 0.00616899" quat="-0.0460841 0.754362 0.044494 0.653325"/>
-         <joint axis="0 0 1" damping="300" limited="true" name="panda_joint6" pos="0 0 0" range="-0.0873 3.8223"/>
-         <geom conaffinity="0" contype="0" group="1" mesh="link6v" name="panda_link6:geom1" rgba="1 1 1 1" type="mesh"/>
-         <geom mesh="link6v" name="panda_link6:geom2" rgba="1 1 1 1" type="mesh"/>
-         <body name="panda_link7" pos="0.088 0 0" quat="0.707107 0.707107 0 0">
-          <inertial diaginertia="0.000637671 0.000528056 0.000279577" mass="0.417345" pos="0.0103614 0.0103596 0.0791078" quat="0.63547 0.278021 -0.670462 0.263369"/>
-          <joint axis="0 0 1" damping="300" limited="true" name="panda_joint7" pos="0 0 0" range="-2.9671 2.9671"/>
-          <geom conaffinity="0" contype="0" group="1" mesh="link7v" name="panda_link7:geom1" rgba="1 1 1 1" type="mesh"/>
-          <geom mesh="link7v" name="panda_link7:geom2" rgba="1 1 1 1" solimp="1.998 1.999 0" type="mesh"/>
-          <body name="panda_link8" pos="0 0 0.107">
-           <inertial diaginertia="0.1 0.1 0.1" mass="0.1" pos="0 0 0"/>
-           <body name="panda_hand" pos="0 0 0" quat="0.92388 0 0 -0.382683">
-            <inertial diaginertia="0.00227632 0.00206087 0.000456542" mass="0.670782" pos="-2.76618e-06 -0.00061547 0.0239295" quat="0.697945 0.716151 -0.000242485 8.47563e-05"/>
-            <geom conaffinity="0" contype="0" group="1" mesh="handv" name="panda_hand:geom1" type="mesh"/>
-            <geom mesh="handv" name="panda_hand:geom2" solimp="1.998 1.999 0" type="mesh"/>
-            <camera euler="0 3.14 0" fovy="60" ipd="0.0" mode="fixed" name="rgbd" pos="0.1 0.0 0."/>
-            <body name="tcp" pos="0 0 0.105">
-             <site name="tcp" rgba="'0 0 1 0'" size='0.001'/>
-            </body>
-            <body childclass="panda:gripper" name="panda_leftfinger" pos="0 0 0.0584">
-             <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754" pos="-2.42335e-05 0.0119585 0.0237816" quat="0.996177 0.0868631 -2.79377e-05 -0.00926642"/>
-             <!--                          <joint axis="0 1 0" limited="true" name="panda_finger_joint1" pos="0 0 0" range="0 0.04"-->
-             <!-- type="slide" />-->
-             <joint axis="0 1 0" name="panda_finger_joint1"/>
-             <site name="panda_leftfinger:site" pos="0 0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012" type="box"/>
-             <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_leftfinger:geom1" type="mesh"/>
-             <geom mesh="fingerv" name="panda_leftfinger:geom2" type="mesh"/>
-             <body name="finger_joint1_tip" pos="0 0.0085 0.056">
-              <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1"/>
-              <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger1_tip_collision" pos="0 -0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5" type="box"/>
-             </body>
-            </body>
-            <body childclass="panda:gripper" name="panda_rightfinger" pos="0 0 0.0584">
-             <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754" pos="2.42335e-05 -0.0119585 0.0237816" quat="0.996177 -0.0868631 2.79377e-05 -0.00926642"/>
-             <joint axis="0 -1 0" name="panda_finger_joint2"/>
-             <site name="panda_rightfinger:site" pos="0 -0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012" type="box"/>
-             <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_rightfinger:geom1" quat="0 0 0 1" type="mesh"/>
-             <geom condim="4" mesh="fingerv" name="panda_rightfinger:geom2" quat="0 0 0 1" type="mesh"/>
-             <body name="finger_joint2_tip" pos="0 -0.0085 0.056">
-              <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1"/>
-              <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger2_tip_collision" pos="0 0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5" type="box"/>
-             </body>
-            </body>
-           </body>
-          </body>
-         </body>
-        </body>
-       </body>
-      </body>
-     </body>
-    </body>
-   </body>
-  </body>
-  <camera euler="0 0 0" fovy="45" ipd="0.0" mode="fixed" name="rgbd_cage" pos="0.7 0.1 0.9"/>
-  <body mocap="true" name="panda:mocap" pos="0 0 0">
-   <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7" size="0.005 0.005 0.005" type="box"/>
-   <!--      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="1 0 0 0.3" size="1 0.005 0.005" type="box" />-->
-   <!--      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 1 0 0.3" size="0.005 1 0.005" type="box" />-->
-   <!--      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0 1 0.3" size="0.005 0.005 1" type="box" />-->
-  </body>
- </worldbody>
- <equality>
-  <weld body1="panda:mocap" body2="tcp" solref="0.001 1"/>
- </equality>
- <sensor>
-  <touch name="touchsensor:left" site="panda_leftfinger:site"/>
-  <touch name="touchsensor:right" site="panda_rightfinger:site"/>
- </sensor>
- <default>
-  <default class="panda:gripper">
-   <geom condim="4" friction="1 0.005 0.0001" margin="0.001" solimp="0.998 0.999 0.001" solref="0.02 1" type="box"/>
-   <joint damping="10" limited="true" range="0 0.04" type="slide"/>
-  </default>
- </default>
- <actuator>
-  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"
-            joint="panda_finger_joint1" kp="1000000" name="panda_finger_joint1_act"/>
-  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"
-            joint="panda_finger_joint2" kp="1000000" name="panda_finger_joint2_act"/>
- </actuator>
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/robots/panda_rod.xml

@@ -1,159 +0,0 @@
-<mujocoinclude>
-    <compiler angle="radian" discardvisual="false" meshdir="../robots/meshes/panda/"/>
- <asset>
-  <mesh file="link0v.stl" name="link0v"/>
-  <mesh file="link1v.stl" name="link1v"/>
-  <mesh file="link2v.stl" name="link2v"/>
-  <mesh file="link3v.stl" name="link3v"/>
-  <mesh file="link4v.stl" name="link4v"/>
-  <mesh file="link5v.stl" name="link5v"/>
-  <mesh file="link6v.stl" name="link6v"/>
-  <mesh file="link7v.stl" name="link7v"/>
-  <mesh file="handv.stl" name="handv"/>
-  <mesh file="fingerv.stl" name="fingerv"/>
- </asset>
- <contact>
-  <exclude body1="panda_link0" body2="panda_link1"/>
-  <exclude body1="panda_link1" body2="panda_link2"/>
-  <exclude body1="panda_link2" body2="panda_link3"/>
-  <exclude body1="panda_link3" body2="panda_link4"/>
-  <exclude body1="panda_link4" body2="panda_link5"/>
-  <exclude body1="panda_link5" body2="panda_link6"/>
-  <exclude body1="panda_link6" body2="panda_link7"/>
-  <exclude body1="panda_link7" body2="panda_link8"/>
-  <exclude body1="panda_link8" body2="panda_hand"/>
- </contact>
- <worldbody>
-  <body name="panda_link0" pos="0 0 0">
-   <inertial diaginertia="0.0126801 0.0117603 0.00856656" mass="3.01399" pos="-0.0291898 -0.000879465 0.0566032"
-             quat="0.00411744 0.564916 0.0132875 0.825031"/>
-   <geom conaffinity="0" contype="0" group="1" mesh="link0v" name="panda_link0:geom1" rgba="1 1 1 1" type="mesh"/>
-   <geom mesh="link0v" name="panda_link0:geom2" rgba="1 1 1 1" type="mesh"/>
-   <body name="panda_link1" pos="0 0 0.333">
-    <inertial diaginertia="0.0164224 0.0153969 0.00546286" mass="2.77281" pos="1.1399e-05 -0.0312655 -0.0693733"
-              quat="0.98466 0.174481 -0.000101815 0.000347662"/>
-    <joint axis="0 0 1" limited="true" name="panda_joint1" pos="0 0 0" range="-2.9671 2.9671"/>
-    <geom conaffinity="0" contype="0" group="1" mesh="link1v" name="panda_joint1:geom1" rgba="1 1 1 1" type="mesh"/>
-    <geom mesh="link1v" name="panda_joint1:geom2" rgba="1 1 1 1" type="mesh"/>
-    <body name="panda_link2" pos="0 0 0" quat="0.707107 -0.707107 0 0">
-     <inertial diaginertia="0.016787 0.0157415 0.00553027" mass="2.7996" pos="-1.31766e-05 -0.0703216 0.0311782"
-               quat="0.57484 0.818266 -6.05764e-05 -6.61626e-05"/>
-     <joint axis="0 0 1" limited="true" name="panda_joint2" pos="0 0 0" range="-1.8326
-                    1.8326"/>
-     <geom conaffinity="0" contype="0" group="1" mesh="link2v" name="panda_joint2:geom1" rgba="1 1 1 1" type="mesh"/>
-     <geom mesh="link2v" name="panda_joint2:geom2" rgba="1 1 1 1" type="mesh"/>
-     <body name="panda_link3" pos="0 -0.316 0" quat="0.707107 0.707107 0 0">
-      <inertial diaginertia="0.00915257 0.00896477 0.00384742" mass="2.14603" pos="0.0443483 0.0249283 -0.03813"
-                quat="0.0615263 0.349824 0.234291 0.904956"/>
-      <joint axis="0 0 1" limited="true" name="panda_joint3" pos="0 0 0" range="-2.9671 2.9671"/>
-      <geom conaffinity="0" contype="0" group="1" mesh="link3v" name="panda_joint3:geom1" rgba="1 1 1 1" type="mesh"/>
-      <geom mesh="link3v" name="panda_joint3:geom2" rgba="1 1 1 1" type="mesh"/>
-      <body name="panda_link4" pos="0.0825 0 0" quat="0.707107 0.707107 0 0">
-       <inertial diaginertia="0.00946899 0.00928491 0.00396694" mass="2.18807" pos="-0.0385503 0.0395256 0.0247162"
-                 quat="0.813566 0.465041 0.309792 0.160858"/>
-       <joint axis="0 0 1" limited="true" name="panda_joint4" pos="0 0 0" range="-3.1416 0.0873"/>
-       <geom conaffinity="0" contype="0" group="1" mesh="link4v" name="panda_link4:geom1" rgba="1 1 1 1" type="mesh"/>
-       <geom mesh="link4v" name="panda_link4:geom2" rgba="1 1 1 1" type="mesh"/>
-       <body name="panda_link5" pos="-0.0825 0.384 0" quat="0.707107 -0.707107 0 0">
-        <inertial diaginertia="0.0278873 0.0268823 0.00569569" mass="3.19545" pos="-6.36776e-05 0.0384124 -0.10997"
-                  quat="0.990767 -0.135571 0.000963106 0.000694406"/>
-        <joint axis="0 0 1" limited="true" name="panda_joint5" pos="0 0 0" range="-2.9671 2.9671"/>
-        <geom conaffinity="0" contype="0" group="1" mesh="link5v" name="panda_link5:geom1" rgba="1 1 1 1" type="mesh"/>
-        <geom mesh="link5v" name="panda_link5:geom2" rgba="1 1 1 1" type="mesh"/>
-        <body name="panda_link6" pos="0 0 0" quat="0.707107 0.707107 0 0">
-         <inertial diaginertia="0.00412168 0.0033698 0.00213304" mass="1.35761" pos="0.0510023 0.00693267 0.00616899"
-                   quat="-0.0460841 0.754362 0.044494 0.653325"/>
-         <joint axis="0 0 1" limited="true" name="panda_joint6" pos="0 0 0" range="-0.0873 3.8223"/>
-         <geom conaffinity="0" contype="0" group="1" mesh="link6v" name="panda_link6:geom1" rgba="1 1 1 1" type="mesh"/>
-         <geom mesh="link6v" name="panda_link6:geom2" rgba="1 1 1 1" type="mesh"/>
-         <body name="panda_link7" pos="0.088 0 0" quat="0.707107 0.707107 0 0">
-          <inertial diaginertia="0.000637671 0.000528056 0.000279577" mass="0.417345"
-                    pos="0.0103614 0.0103596 0.0791078" quat="0.63547 0.278021 -0.670462 0.263369"/>
-          <joint axis="0 0 1" limited="true" name="panda_joint7" pos="0 0 0" range="-2.9671 2.9671"/>
-          <geom conaffinity="0" contype="0" group="1" mesh="link7v" name="panda_link7:geom1" rgba="1 1 1 1"
-                type="mesh"/>
-          <geom mesh="link7v" name="panda_link7:geom2" rgba="1 1 1 1" solimp="1.998 1.999 0" type="mesh"/>
-          <body name="panda_link8" pos="0 0 0.107">
-           <inertial diaginertia="0.1 0.1 0.1" mass="0.1" pos="0 0 0"/>
-           <body name="panda_hand" pos="0 0 0" quat="0.92388 0 0 -0.382683">
-            <inertial diaginertia="0.00227632 0.00206087 0.000456542" mass="0.670782"
-                      pos="-2.76618e-06 -0.00061547 0.0239295" quat="0.697945 0.716151 -0.000242485 8.47563e-05"/>
-            <geom conaffinity="0" contype="0" group="1" mesh="handv" name="panda_hand:geom1" type="mesh"/>
-            <geom mesh="handv" name="panda_hand:geom2" solimp="1.998 1.999 0" type="mesh"/>
-            <camera euler="0 3.14 0" fovy="60" ipd="0.0" mode="fixed" name="rgbd" pos="0.1 0.0 0."/>
-            <body name="tcp" pos="0 0 0.105">
-             <site name="tcp" rgba="'0 0 1 0'" size='0.001'/>
-            </body>
-
-            <!-- PUSHROD -->
-            <body name="push_rod">
-             <geom type="cylinder" size="0.01 0.15" pos="0 0 0.075"/>
-             <site name="rod_tip" type="box" rgba="0 0 1 1." size="0.01 0.01 0.01" pos="0 0 0.2"/>
-            </body>
-
-            <body childclass="panda:gripper" name="panda_leftfinger" pos="0 0 0.0584">
-             <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754"
-                       pos="-2.42335e-05 0.0119585 0.0237816" quat="0.996177 0.0868631 -2.79377e-05 -0.00926642"/>
-             <joint axis="0 1 0" name="panda_finger_joint1"/>
-             <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_leftfinger:geom1" type="mesh"/>
-             <geom mesh="fingerv" name="panda_leftfinger:geom2" type="mesh"/>
-             <site name="panda_leftfinger:site" pos="0 0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012" type="box"/>
-             <body name="finger_joint1_tip" pos="0 0.0085 0.056">
-              <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1"/>
-              <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger1_tip_collision"
-                    pos="0 -0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5"
-                    type="box"/>
-             </body>
-            </body>
-            <body childclass="panda:gripper" name="panda_rightfinger" pos="0 0 0.0584">
-             <inertial diaginertia="5.69661e-06 5.56035e-06 1.55183e-06" mass="0.0218754"
-                       pos="2.42335e-05 -0.0119585 0.0237816" quat="0.996177 -0.0868631 2.79377e-05 -0.00926642"/>
-             <joint axis="0 -1 0" name="panda_finger_joint2"/>
-             <site name="panda_rightfinger:site" pos="0 -0.012 0.045" rgba="0 0 1 0" size="0.012 0.015 0.012"
-                   type="box"/>
-             <geom conaffinity="0" contype="0" group="1" mesh="fingerv" name="panda_rightfinger:geom1" quat="0 0 0 1"
-                   type="mesh"/>
-             <geom condim="4" mesh="fingerv" name="panda_rightfinger:geom2" quat="0 0 0 1" type="mesh"/>
-             <body name="finger_joint2_tip" pos="0 -0.0085 0.056">
-              <inertial diaginertia="0.01 0.01 0.01" mass="0.01" pos="0 0 0" quat="0 0 0 1"/>
-              <geom conaffinity="1" condim="4" contype="1" friction="2 0.05 0.0001" name="finger2_tip_collision"
-                    pos="0 0.005 -0.012" quat="0 0 0 1" rgba="1 0 0 1" size="0.008 0.004 0.008" solref="0.01 0.5"
-                    type="box"/>
-             </body>
-            </body>
-           </body>
-          </body>
-         </body>
-        </body>
-       </body>
-      </body>
-     </body>
-    </body>
-   </body>
-  </body>
-  <camera euler="0 0 0" fovy="45" ipd="0.0" mode="fixed" name="rgbd_cage" pos="0.7 0.1 0.9"/>
- </worldbody>
- <sensor>
-  <touch name="touchsensor:left" site="panda_leftfinger:site"/>
-  <touch name="touchsensor:right" site="panda_rightfinger:site"/>
- </sensor>
- <default>
-  <default class="panda:gripper">
-   <geom condim="4" friction="1 0.005 0.0001" margin="0.001" solimp="0.998 0.999 0.001" solref="0.02 1" type="box"/>
-   <joint damping="10" limited="true" range="0 0.04" type="slide"/>
-  </default>
- </default>
-  <actuator>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint1" name="panda_joint1_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint2" name="panda_joint2_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint3" name="panda_joint3_act"/>
-  <motor forcelimited="true" forcerange="-87 87" joint="panda_joint4" name="panda_joint4_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint5" name="panda_joint5_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint6" name="panda_joint6_act"/>
-  <motor forcelimited="true" forcerange="-12 12" joint="panda_joint7" name="panda_joint7_act"/>
-<!--  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"-->
-<!--            joint="panda_finger_joint1" kp="1000000" name="panda_finger_joint1_act"/>-->
-<!--  <position ctrllimited="true" ctrlrange="0.001 0.04" forcelimited="true" forcerange="-70 70"-->
-<!--            joint="panda_finger_joint2" kp="1000000" name="panda_finger_joint2_act"/>-->
- </actuator>
-</mujocoinclude>

fancy_gym/envs/mujoco/box_pushing/model/robots/pandas_kit_lab.xml

@@ -1 +0,0 @@
-<!--todo-->

fancy_gym/envs/mujoco/box_pushing/model/surroundings/assembled_scene.xml

@@ -1,42 +0,0 @@
-<mujoco model="base_surrounding">
-  <compiler angle="radian" discardvisual="false" />
-  <option collision="all" cone="elliptic" gravity="0 0 -9.81" impratio="3" solver="Newton" timestep="0.002" tolerance="1e-10" />
-  <size nconmax="2000" njmax="2000" />
-  <asset>
-    <texture builtin="gradient" height="32" rgb1="0.26 0.58 0.51" rgb2="0.26 0.58 0.51" type="skybox" width="32" />
-  </asset>
-  <visual>
-    <map zfar="1000" znear="0.001" />
-  </visual>
-  <worldbody>
-    <light castshadow="true" cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1" />
-    <body name="ground" pos="0 0 -0.94">
-      <geom name="ground:geom1" rgba="0.26 0.58 0.51 1" size="0 0 1" type="plane" />
-    </body>
-<!--    <body name="goal" pos="0.5 0 0.2" quat="1 0 0 0">-->
-<!--      <geom type="sphere" mass="0.1" size="0.01" rgba="1 0 0 1" />-->
-<!--    </body>-->
-<!--    <body name="box" pos="0.5 0 0.1" quat="1 0 0 0">-->
-<!--      <geom type="box" mass="1" size="0.02 0.02 0.02" rgba="0.32 0.32 0.32 1" />-->
-<!--    </body>-->
-    <body name="target_pos" pos="0.5 0.5 0.0">
-      <site type="box" pos="0 0 0" rgba="0 1 0 0.3" size="0.05 0.05 0.01" />
-      <site pos="0.05 0 0.0485" size="0.005 0.05 0.045" rgba="0 0 1 0.5" type="box"/>
-      <site pos="0 0.05 0.0485" size="0.05 0.005 0.045" rgba="0 1 0 0.5" type="box"/>
-      <site pos="-0.05 0 0.0485" size="0.005 0.05 0.045" rgba="0 1 0 0.5" type="box"/>
-      <site pos="0 -0.05 0.0485" size="0.05 0.005 0.045" rgba="0 1 0 0.5" type="box"/>
-    </body>
-
-    <body name="replan_target_pos" pos="0.5 0.5 -0.01">
-      <site type="box" pos="0 0 0" rgba="1 1 0 0.3" size="0.05 0.05 0.01" />
-      <site pos="0.05 0 0.0485" size="0.005 0.05 0.045" rgba="0 0 1 0.5" type="box"/>
-      <site pos="0 0.05 0.0485" size="0.05 0.005 0.045" rgba="1 1 0 0.5" type="box"/>
-      <site pos="-0.05 0 0.0485" size="0.005 0.05 0.045" rgba="1 1 0 0.5" type="box"/>
-      <site pos="0 -0.05 0.0485" size="0.05 0.005 0.045" rgba="1 1 0 0.5" type="box"/>
-    </body>
-  </worldbody>
-
-  <include file="kit_lab_surrounding.xml" />
-  <include file="../robots/panda_rod.xml" />
-  <include file="../objects/assets/push_box.xml" />
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/surroundings/base.xml

@@ -1,19 +0,0 @@
-<mujoco model="base_surrounding">
- <compiler angle="radian" discardvisual="false"/>
-    <option collision="all" cone="elliptic" gravity="0 0 -9.81" impratio="3" solver="Newton" timestep="0.001"
-            tolerance="1e-10"/>
- <size nconmax="2000" njmax="2000"/>
-    <asset>
-        <texture builtin="gradient" height="32" rgb1="0.26 0.58 0.51" rgb2="0.26 0.58 0.51" type="skybox" width="32"/>
-    </asset>
-    <visual>
-         <map zfar="1000" znear="0.001"/>
-    </visual>
-    <worldbody>
-        <light castshadow="true" cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3"
-             specular=".1 .1 .1"/>
-          <body name="ground" pos="0 0 -0.94">
-            <geom name="ground:geom1" rgba="0.26 0.58 0.51 1" size="0 0 1" type="plane"/>
-          </body>
-    </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/surroundings/kit_lab_surrounding.xml

@@ -1,118 +0,0 @@
-<mujoco model="PandaFrame">
-    <worldbody>
-        <body name="table_plane" pos="0.2 0 -0.02">
-            <geom type="box" size="0.49 0.98 0.001" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
-                  solref="0.002 1"/>
-
-            <body name="panda_ground" pos="-0.24 0 0.01">
-            <geom type="box" size="0.18 0.18 0.01" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="front_upper" pos="0.49 0 -0.02">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="front_mid" pos="0.49 0 -0.445">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="front_lower" pos="0.49 0 -0.87">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="back_upper" pos="-0.49 0 -0.02">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="back_mid" pos="-0.49 0 -0.445">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="back_lower" pos="-0.49 0 -0.87">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="vert_front_left" pos="0.49 0.98 -0.445">
-            <geom type="box" size="0.02 0.02 0.445" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="vert_front_right" pos="0.49 -0.98 -0.445">
-            <geom type="box" size="0.02 0.02 0.445" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="vert_back_left" pos="-0.49 0.98 -0.445">
-            <geom type="box" size="0.02 0.02 0.445" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="vert_back_right" pos="-0.49 -0.98 -0.445">
-            <geom type="box" size="0.02 0.02 0.445" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="side_upper_right" pos="0 -0.98 -0.02">
-            <geom type="box" size="0.47 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="side_lower_right" pos="0 -0.98 -0.87">
-            <geom type="box" size="0.47 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="side_upper_left" pos="0 0.98 -0.02">
-            <geom type="box" size="0.47 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="side_lower_left" pos="0 0.98 -0.87">
-            <geom type="box" size="0.47 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="foot_front_left" pos="0.49 0.98 -0.9">
-            <geom type="cylinder" size="0.02 0.02 0.04" rgba="0 0 0 1"/>
-            </body>
-            <body name="foot_front_mid" pos="0.49 0 -0.9">
-            <geom type="cylinder" size="0.02 0.02 0.04" rgba="0 0 0 1"/>
-            </body>
-            <body name="foot_front_right" pos="0.49 -0.98 -0.9">
-            <geom type="cylinder" size="0.02 0.02 0.04" rgba="0 0 0 1"/>
-            </body>
-
-            <body name="foot_back_left" pos="-0.49 0.98 -0.9">
-            <geom type="cylinder" size="0.02 0.02 0.04" rgba="0 0 0 1"/>
-            </body>
-            <body name="foot_back_mid" pos="-0.49 0 -0.9">
-            <geom type="cylinder" size="0.02 0.02 0.04" rgba="0 0 0 1"/>
-            </body>
-            <body name="foot_back_right" pos="-0.49 -0.98 -0.9">
-            <geom type="cylinder" size="0.02 0.02 0.04" rgba="0 0 0 1"/>
-            </body>
-
-            <body name="top_back_left" pos="-0.49 0.98 0.65">
-            <geom type="box" size="0.02 0.02 0.65" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="top_back_right" pos="-0.49 -0.98 0.65">
-            <geom type="box" size="0.02 0.02 0.65" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="top_front" pos="0.49 0 1.28">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="top_back" pos="-0.49 0 1.28">
-            <geom type="box" size="0.02 0.96 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="top_side_right" pos="0 -0.98 1.28">
-            <geom type="box" size="0.51 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="top_side_mid" pos="0 0 1.28">
-            <geom type="box" size="0.51 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="top_side_left" pos="0 0.98 1.28">
-            <geom type="box" size="0.51 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="rot_lower_right" pos="-0.31 0.98 0.22" quat="0.2 0 0.1 0">
-            <geom type="box" size="0.3 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="rot_lower_left" pos="-0.31 -0.98 0.22" quat="0.2 0 0.1 0">
-            <geom type="box" size="0.3 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="rot_upper_right" pos="-0.31 0.98 1.04" quat="0.1 0 0.2 0">
-            <geom type="box" size="0.3 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-            <body name="rot_upper_left" pos="-0.31 -0.98 1.04" quat="0.1 0 0.2 0">
-            <geom type="box" size="0.3 0.02 0.02" rgba=".9 .9 .9 1"/>
-            </body>
-
-            <body name="support_body" pos="0 0 -0.4" >
-            <geom type="box" size="0.49 0.98 0.4" rgba="0 0 0 0" solimp="0.999  0.999 0.001" solref="0.002 1"/>
-            </body>
-        </body>
-    </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/workspace/large_workspace.xml

@@ -1,13 +0,0 @@
-<mujoco model="PandaFrame">
-   <worldbody>
-    <body name="workspace" pos="0.42 0 0.26" >
-        <site name="workspace_box" type="box" size="0.28 0.35 0.26" rgba="0 0 0 0"/>
-        <site name="x_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="-0.28 0 0"/>
-        <site name="x_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0.28 0 0"/>
-        <site name="y_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 -0.35 0"/>
-        <site name="y_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0.35 0"/>
-        <site name="z_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0 -0.26"/>
-        <site name="z_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0 0.26"/>
-    </body>
-    </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/workspace/medium_workspace.xml

@@ -1,13 +0,0 @@
-<mujoco model="PandaFrame">
-   <worldbody>
-    <body name="workspace" pos="0.42 0 0.22" >
-        <site name="workspace_box" type="box" size="0.2 0.25 0.22" rgba="0 0 0 0"/>
-        <site name="x_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="-0.2 0 0"/>
-        <site name="x_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0.2 0 0"/>
-        <site name="y_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 -0.25 0"/>
-        <site name="y_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0.25 0"/>
-        <site name="z_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0 -0.23"/>
-        <site name="z_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0 0.22"/>
-    </body>
-    </worldbody>
-</mujoco>

fancy_gym/envs/mujoco/box_pushing/model/workspace/small_workspace.xml

@@ -1,13 +0,0 @@
-<mujoco model="PandaFrame">
-   <worldbody>
-    <body name="workspace" pos="0.5 0 0.2" >
-        <site name="workspace_box" type="box" size="0.001 0.001 0.2" rgba="0 0 0 0.5"/>
-        <site name="x_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="-0.001 0 0"/>
-        <site name="x_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0.001 0 0"/>
-        <site name="y_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 -0.001 0"/>
-        <site name="y_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0.001 0"/>
-        <site name="z_constrain_low" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0 -0.22"/>
-        <site name="z_constrain_high" type="sphere" size="0.01 0.01 0.01" rgba="1 0 0 0" pos="0 0 0.2"/>
-    </body>
-    </worldbody>
-</mujoco>

fancy_gym/examples/examples_movement_primitives.py

@@ -163,6 +163,9 @@ if __name__ == '__main__':
     # example_mp("HoleReacherProMP-v0", seed=10, iterations=5, render=render)
     example_mp("BoxPushingDenseProMP-v0", seed=10, iterations=50, render=render)
 
+    # ProDMP
+    # example_mp("BoxPushingDenseProDMP-v0", seed=10, iterations=50, render=render)
+
     # Altered basis functions
     # obs1 = example_custom_mp("Reacher5dProMP-v0", seed=10, iterations=1, render=render)