prodmp box pushing works
This commit is contained in:
Hongyi Zhou
parent
187c5f5bb2
commit
eec171e04a
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -87,8 +87,7 @@ DEFAULT_BB_DICT_ProDMP = {
|
||||
},
|
||||
"black_box_kwargs": {
|
||||
'replanning_schedule': None,
|
||||
'verbose': 2,
|
||||
'enable_traj_level_reward': False,
|
||||
'verbose': 2
|
||||
}
|
||||
}
|
||||
|
||||
@ -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']
|
||||
|
||||
@ -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("========================================")
|
||||
@ -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")
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
|
||||
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -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>
|
||||
@ -1 +0,0 @@
|
||||
<!--todo-->
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -1 +0,0 @@
|
||||
<!--todo-->
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user