From 646cf0ec472c59578cce7109a1ca08c708224acf Mon Sep 17 00:00:00 2001 From: Patrick Lancaster Date: Mon, 1 May 2023 14:33:46 -0400 Subject: [PATCH 1/2] Update push environment to use robotiq gripper and to have cartesian action space --- .../arms/franka/assets/franka_ycb_v0.config | 72 ++++--- .../envs/arms/franka/assets/franka_ycb_v0.xml | 27 ++- robohive/envs/arms/push_base_v0.py | 191 +++++++++++++++++- 3 files changed, 251 insertions(+), 39 deletions(-) diff --git a/robohive/envs/arms/franka/assets/franka_ycb_v0.config b/robohive/envs/arms/franka/assets/franka_ycb_v0.config index 54b23efe..3bea08bc 100644 --- a/robohive/envs/arms/franka/assets/franka_ycb_v0.config +++ b/robohive/envs/arms/franka/assets/franka_ycb_v0.config @@ -1,7 +1,7 @@ { # device1: sensors, actuators 'franka':{ - 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, + 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale': 1.5}, 'sensor':[ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, @@ -10,18 +10,7 @@ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(0.00, .04), 'noise':0.05, 'hdr_id':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, - {'range':(0.00, .04), 'noise':0.05, 'hdr_id':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, + ], 'actuator':[ @@ -32,20 +21,55 @@ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, - {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, - 'object':{ - 'interface':{}, + + # device1: sensors, actuators + 'robotiq':{ + 'interface': {'type': 'robotiq', 'ip_address':'172.16.0.3'}, 'sensor':[ - {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Tx'}, - {'range':(-0.5, 0.5), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Ty'}, - {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Tz'}, - {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Rx'}, - {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Ry'}, - {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Rz'}, + {'range':(0, 0.834), 'noise':0.0, 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, + ], + 'actuator':[ + {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, + ] + }, + + 'right_cam':{ + 'interface': {'type': 'realsense', 'rgb_topic':'realsense_817612071438/color/image_raw', 'd_topic':'realsense_817612071438/depth_uncolored/image_raw'}, + 'sensor':[], + 'cam': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'}, + ], + 'actuator':[] + }, + + 'left_cam':{ + 'interface': {'type': 'realsense', 'rgb_topic':'realsense_923322071682/color/image_raw', 'd_topic':'realsense_923322071682/depth_uncolored/image_raw'}, + 'sensor':[], + 'cam': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + ], + 'actuator':[] + }, + + 'top_cam':{ + 'interface': {'type': 'realsense', 'rgb_topic':'realsense_831612071935/color/image_raw', 'd_topic':'realsense_831612071935/depth_uncolored/image_raw'}, + 'sensor':[], + 'cam': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] - } + }, + + 'Franka_wrist_cam':{ + 'interface': {'type': 'realsense', 'rgb_topic':'realsense_936322070059/color/image_raw', 'd_topic':'realsense_936322070059/depth_uncolored/image_raw'}, + 'sensor':[], + 'cam': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + ], + 'actuator':[] + }, + } \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_ycb_v0.xml b/robohive/envs/arms/franka/assets/franka_ycb_v0.xml index d18ca51a..853d945a 100644 --- a/robohive/envs/arms/franka/assets/franka_ycb_v0.xml +++ b/robohive/envs/arms/franka/assets/franka_ycb_v0.xml @@ -10,12 +10,12 @@ + - + - @@ -24,27 +24,36 @@ - - + + + - - + + - + + + + + + - + + + - + + diff --git a/robohive/envs/arms/push_base_v0.py b/robohive/envs/arms/push_base_v0.py index 56f9985b..3e5f2723 100644 --- a/robohive/envs/arms/push_base_v0.py +++ b/robohive/envs/arms/push_base_v0.py @@ -5,16 +5,21 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ +import os import collections import gym import numpy as np from robohive.envs import env_base +from robohive.physics.sim_scene import SimScene +from robohive.utils.xml_utils import reassign_parent +from robohive.utils.quat_math import mat2euler, euler2quat, mat2quat +from robohive.utils.inverse_kinematics import qpos_from_site_pose class PushBaseV0(env_base.MujocoEnv): DEFAULT_OBS_KEYS = [ - 'qp', 'qv', 'object_err', 'target_err' + 'qp', 'qv', 'grasp_pos', 'grasp_rot', 'object_err', 'target_err' ] DEFAULT_RWD_KEYS_AND_WEIGHTS = { "object_dist": -1.0, @@ -25,6 +30,27 @@ class PushBaseV0(env_base.MujocoEnv): def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): + # Process model to use Robotiq/DManus as end effector + raw_sim = SimScene.get_sim(model_path) + raw_xml = raw_sim.model.get_xml() + processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount") + processed_model_path = model_path[:-4]+"_processed.xml" + with open(processed_model_path, 'w') as file: + file.write(processed_xml) + + # Process model to use DManus as end effector + if obsd_model_path == model_path: + processed_obsd_model_path = processed_model_path + elif obsd_model_path: + raw_sim = SimScene.get_sim(obsd_model_path) + raw_xml = raw_sim.model.get_xml() + processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount") + processed_obsd_model_path = obsd_model_path[:-4]+"_processed.xml" + with open(processed_obsd_model_path, 'w') as file: + file.write(processed_xml) + else: + processed_obsd_model_path = None + # EzPickle.__init__(**locals()) is capturing the input dictionary of the init method of this class. # In order to successfully capture all arguments we need to call gym.utils.EzPickle.__init__(**locals()) # at the leaf level, when we do inheritance like we do here. @@ -37,16 +63,26 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): # first construct the inheritance chain, which is just __init__ calls all the way down, with env_base # creating the sim / sim_obsd instances. Next we run through "setup" which relies on sim / sim_obsd # created in __init__ to complete the setup. - super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed) + super().__init__(model_path=processed_model_path, obsd_model_path=processed_obsd_model_path, seed=seed) - self._setup(**kwargs) + self._setup(processed_model_path, **kwargs) + + os.remove(processed_model_path) + if processed_obsd_model_path and processed_obsd_model_path!=processed_model_path: + os.remove(processed_obsd_model_path) def _setup(self, + model_path, robot_site_name, object_site_name, target_site_name, target_xyz_range, + init_qpos=None, + pos_limits=None, + vel_limits=None, + obj_pos_limits=None, + max_ik=3, frame_skip=40, reward_mode="dense", obs_keys=DEFAULT_OBS_KEYS, @@ -56,21 +92,51 @@ def _setup(self, # ids self.grasp_sid = self.sim.model.site_name2id(robot_site_name) + self.object_site_name = object_site_name self.object_sid = self.sim.model.site_name2id(object_site_name) self.target_sid = self.sim.model.site_name2id(target_site_name) self.target_xyz_range = target_xyz_range + self.pos_limits = pos_limits + self.vel_limits = vel_limits + self.obj_pos_limits = obj_pos_limits + self.max_ik = max_ik + + self.ik_sim = SimScene.get_sim(model_path) + self.last_eef_cmd = None + + assert(self.pos_limits is None or + ('eef_high' in self.pos_limits and 'eef_low' in self.pos_limits )) + assert(self.vel_limits is None or + ('jnt' in self.vel_limits and 'eef' in self.vel_limits)) + assert(self.obj_pos_limits is None or + ('low' in self.obj_pos_limits and 'high' in self.obj_pos_limits)) + + if pos_limits is not None: + for key in pos_limits.keys(): + pos_limits[key] = np.array(pos_limits[key]) + if vel_limits is not None: + for key in vel_limits.keys(): + vel_limits[key] = np.array(vel_limits[key]) + if obj_pos_limits is not None: + for key in obj_pos_limits.keys(): + obj_pos_limits[key] = np.array(obj_pos_limits[key]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, **kwargs) + if init_qpos is not None: + self.init_qpos[:len(init_qpos)] = np.array(init_qpos)[:] def get_obs_dict(self, sim): obs_dict = {} obs_dict['time'] = np.array([self.sim.data.time]) obs_dict['qp'] = sim.data.qpos.copy() obs_dict['qv'] = sim.data.qvel.copy() + obs_dict['grasp_pos'] = sim.data.site_xpos[self.grasp_sid].copy() + obs_dict['grasp_rot'] = mat2quat(self.sim.data.site_xmat[self.grasp_sid].reshape(3, 3).transpose()) obs_dict['object_err'] = sim.data.site_xpos[self.object_sid]-sim.data.site_xpos[self.grasp_sid] obs_dict['target_err'] = sim.data.site_xpos[self.target_sid]-sim.data.site_xpos[self.object_sid] return obs_dict @@ -88,15 +154,128 @@ def get_reward_dict(self, obs_dict): ('bonus', (object_dist<.1) + (target_dist<.1) + (target_dist<.05)), ('penalty', (object_dist>far_th)), # Must keys - ('sparse', -1.0*target_dist), + ('sparse', -1.0*(target_dist<.050)), ('solved', target_dist<.050), ('done', object_dist > far_th), )) rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) return rwd_dict - def reset(self): + def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): self.sim.model.site_pos[self.target_sid] = self.np_random.uniform(high=self.target_xyz_range['high'], low=self.target_xyz_range['low']) self.sim_obsd.model.site_pos[self.target_sid] = self.sim.model.site_pos[self.target_sid] - obs = super().reset(self.init_qpos, self.init_qvel) + + if reset_qpos is None: + reset_qpos = self.init_qpos.copy() + if self.obj_pos_limits is not None: + obj_jid = self.sim.model.joint_name2id(self.object_site_name) + reset_qpos[obj_jid:obj_jid+3] = self.np_random.uniform(low=self.obj_pos_limits['low'], high=self.obj_pos_limits['high']) + + obs = super().reset(reset_qpos, reset_qvel, **kwargs) + + cur_pos = self.sim.data.site_xpos[self.grasp_sid] + cur_rot = mat2euler(self.sim.data.site_xmat[self.grasp_sid].reshape(3,3).transpose()) + if self.pos_limits is not None: + cur_rot[cur_rot < self.pos_limits['eef_low'][3:6]] += 2*np.pi + cur_rot[cur_rot > self.pos_limits['eef_high'][3:6]] -= 2 * np.pi + + self.last_eef_cmd = np.concatenate([cur_pos, + cur_rot, + [self.sim.data.qpos[7]]]) + return obs + + def get_ik_action(self, eef_pos, eef_quat): + for i in range(self.max_ik): + + self.ik_sim.data.qpos[:7] = np.random.normal(self.sim.data.qpos[:7], i*0.1) + + self.ik_sim.data.qpos[2] = 0.0 + self.ik_sim.forward() + + ik_result = qpos_from_site_pose(physics = self.ik_sim, + site_name = self.sim.model.site_id2name(self.grasp_sid), + target_pos= eef_pos, + target_quat= eef_quat, + inplace=False, + regularization_strength=1.0) + + if ik_result.success: + break + return ik_result + + def step(self, a, **kwargs): + assert(len(a.shape) == 1) + assert(a.shape[0] == self.sim.model.nu or a.shape[0] == 7) + + if a.shape[0] == self.sim.model.nu: + action = a + + if self.vel_limits is not None: + act_low = self.sim.model.actuator_ctrlrange[:,0].copy() + act_high = self.sim.model.actuator_ctrlrange[:,1].copy() + # Enforce joint velocity limits + if self.normalize_act: + action = (0.5 * action + 0.5) * (act_high - act_low) + act_low + action = np.clip(action, + self.sim.data.qpos[:self.sim.model.nu] - self.vel_limits['jnt'], + self.sim.data.qpos[:self.sim.model.nu] + self.vel_limits['jnt']) + if self.normalize_act: + action = 2 * (((action - act_low) / ( + act_high - act_low)) - 0.5) + else: + # Un-normalize cmd + eef_cmd = a + if self.normalize_act: + eef_cmd = (0.5 * eef_cmd + 0.5) * (self.pos_limits['eef_high'] - self.pos_limits['eef_low']) + \ + self.pos_limits['eef_low'] + + if self.pos_limits is not None: + eef_cmd = np.clip(eef_cmd, + self.pos_limits['eef_low'], + self.pos_limits['eef_high']) + + # Get current position and rotation of eef + cur_pose = self.sim.data.site_xpos[self.grasp_sid] + + # Enforce cartesian velocity limits + if self.vel_limits is not None: + eef_cmd[:3] = np.clip(eef_cmd[:3], + cur_pose - self.vel_limits['eef'], + cur_pose + self.vel_limits['eef']) + + # Exponential moving average to limit jerk + assert self.last_eef_cmd is not None + eef_cmd[:3] = 0.25 * eef_cmd[:3] + 0.75 * self.last_eef_cmd[:3] + + # Prepare for IK, execute last successful command if IK fails + eef_pos = eef_cmd[:3] + eef_elr = eef_cmd[3:6] + eef_quat = euler2quat(eef_elr) + + ik_result = self.get_ik_action(eef_pos, eef_quat) + ik_success = ik_result.success + if not ik_success: + eef_cmd = self.last_eef_cmd + eef_pos = eef_cmd[:3] + eef_elr = eef_cmd[3:6] + eef_quat = euler2quat(eef_elr) + ik_result = self.get_ik_action(eef_pos, eef_quat) + + action = ik_result.qpos[:self.sim.model.nu] + action[7:self.sim.model.nu] = eef_cmd[6] + + self.last_eef_cmd = eef_cmd + + # Enforce joint velocity limits + # if self.robot.is_hardware: + if self.vel_limits is not None: + action = np.clip(action, + self.sim.data.qpos[:self.sim.model.nu] - self.vel_limits['jnt'], + self.sim.data.qpos[:self.sim.model.nu] + self.vel_limits['jnt']) + + if self.normalize_act: + action = 2 * (((action - self.sim.model.actuator_ctrlrange[:,0].copy()) / ( + self.sim.model.actuator_ctrlrange[:,1].copy() - self.sim.model.actuator_ctrlrange[:,0].copy())) - 0.5) + + return super().step(action, **kwargs) \ No newline at end of file From c91f588fe56f426e941785cefc4caa048a2cbb61 Mon Sep 17 00:00:00 2001 From: Patrick Lancaster Date: Mon, 1 May 2023 14:47:05 -0400 Subject: [PATCH 2/2] Add new pushing envs --- robohive/envs/arms/__init__.py | 76 +++++++++++++++- .../franka/assets/franka_bin_push_v0.config | 89 +++++++++++++++++++ .../arms/franka/assets/franka_bin_push_v0.xml | 74 +++++++++++++++ .../franka/assets/franka_hang_push_v0.config | 89 +++++++++++++++++++ .../franka/assets/franka_hang_push_v0.xml | 68 ++++++++++++++ .../assets/franka_planar_push_v0.config | 89 +++++++++++++++++++ .../franka/assets/franka_planar_push_v0.xml | 59 ++++++++++++ 7 files changed, 542 insertions(+), 2 deletions(-) create mode 100644 robohive/envs/arms/franka/assets/franka_bin_push_v0.config create mode 100644 robohive/envs/arms/franka/assets/franka_bin_push_v0.xml create mode 100644 robohive/envs/arms/franka/assets/franka_hang_push_v0.config create mode 100644 robohive/envs/arms/franka/assets/franka_hang_push_v0.xml create mode 100644 robohive/envs/arms/franka/assets/franka_planar_push_v0.config create mode 100644 robohive/envs/arms/franka/assets/franka_planar_push_v0.xml diff --git a/robohive/envs/arms/__init__.py b/robohive/envs/arms/__init__.py index e9f1dfb0..ddab8f04 100644 --- a/robohive/envs/arms/__init__.py +++ b/robohive/envs/arms/__init__.py @@ -76,7 +76,7 @@ def register_visual_envs(encoder_type): 'robot_site_name': "end_effector", 'object_site_name': "sugarbox", 'target_site_name': "target", - 'target_xyz_range': {'high':[-.4, 0.5, 0.78], 'low':[-.4, 0.5, 0.78]} + 'target_xyz_range': {'high': [0.5, 0.4, 0.78], 'low': [0.5, 0.4, 0.78]} } ) @@ -91,7 +91,79 @@ def register_visual_envs(encoder_type): 'robot_site_name': "end_effector", 'object_site_name': "sugarbox", 'target_site_name': "target", - 'target_xyz_range': {'high':[0.4, 0.5, 0.78], 'low':[-.4, .4, 0.78]} + 'target_xyz_range': {'high': [0.5, 0.4, 0.78], 'low': [0.4, -0.4, 0.78]} + } +) + +register( + # Init position: [0.51,0.23,0.98] + # Init euler: [-np.pi/4+0.3, np.pi+0.3, -3*np.pi/4,] + id='FrankaBinPush-v0', + entry_point='robohive.envs.arms.push_base_v0:PushBaseV0', + max_episode_steps=50, #50steps*40Skip*2ms = 4s + kwargs={ + 'model_path': curr_dir+'/franka/assets/franka_bin_push_v0.xml', + 'config_path': curr_dir+'/franka/assets/franka_bin_push_v0.config', + 'robot_site_name': "end_effector", + 'object_site_name': "obj0", + 'obj_pos_limits': {'high':[0.52, 0.126, 0.955], 'low':[0.48, 0.125, 0.945]}, + 'target_site_name': "target", + 'target_xyz_range': {'high':[0.5, -0.22, 1.085], 'low':[0.5, -0.22, 1.085]}, + 'pos_limits': {'eef_low': [0.315, -0.22, 0.89, -0.485, 3.14, -2.36, 0.4], + 'eef_high': [0.695, 0.275, 1.125, -0.485, 3.14, -2.36, 0.4] + }, + 'vel_limits': {'eef':[0.15, 0.15, 0.15], + 'jnt': [0.15, 0.25, 0.1, 0.25, 0.1, 0.25, 0.2, 1.0] + }, + 'init_qpos': [0.534, 0.401, 0.0, -1.971, -0.457, 0.490, 1.617, 0.4, 0.4], + } +) + +register( + # Init position: [0.36, 0.0, 1.34] + # Init euler: [3.14, 0.5, -0.8,] + id='FrankaHangPush-v0', + entry_point='robohive.envs.arms.push_base_v0:PushBaseV0', + max_episode_steps=50, #50steps*40Skip*2ms = 4s + kwargs={ + 'model_path': curr_dir+'/franka/assets/franka_hang_push_v0.xml', + 'config_path': curr_dir+'/franka/assets/franka_hang_push_v0.config', + 'robot_site_name': "end_effector", + 'object_site_name': "obj0", + 'obj_pos_limits': {'high':[0.46, 0.01, 1.31], 'low':[0.44, -0.01, 1.3]}, + 'target_site_name': "target", + 'target_xyz_range': {'high':[0.621, 0.0, 1.333], 'low':[0.621, 0.0, 1.333]}, + 'pos_limits': {'eef_low': [0.3, -0.1, 1.25, 3.14, 0.5, -0.8, 0.2], + 'eef_high': [0.8, 0.1, 1.5, 3.14, 0.5, -0.8, 0.2] + }, + 'vel_limits': {'eef':[0.15, 0.15, 0.15], + 'jnt': [0.15, 0.25, 0.1, 0.25, 0.1, 0.25, 0.2, 1.0] + }, + 'init_qpos': [0.0227, -0.9291, -0.014, -2.5568, -0.029, 0.5052, -0.783, 0.2, 0.2], + } +) + +register( + # Init position: [0.36, 0.0, 1.34] + # Init euler: [3.14, 0.5, -0.8,] + id='FrankaPlanarPush-v0', + entry_point='robohive.envs.arms.push_base_v0:PushBaseV0', + max_episode_steps=50, #50steps*40Skip*2ms = 4s + kwargs={ + 'model_path': curr_dir+'/franka/assets/franka_planar_push_v0.xml', + 'config_path': curr_dir+'/franka/assets/franka_planar_push_v0.config', + 'robot_site_name': "end_effector", + 'object_site_name': "obj0", + 'obj_pos_limits': {'high':[0.50, 0.31, 0.855], 'low':[0.46, 0.29, 0.845]}, + 'target_site_name': "target", + 'target_xyz_range': {'high':[0.48, -0.1, 0.845], 'low':[0.48, -0.1, 0.845]}, + 'pos_limits': {'eef_low': [0.3, -0.4, 0.865, 3.14, 0.0, 0.28, 0.2], + 'eef_high': [0.8, 0.4, 0.965, 3.14, 0.0, 1.28, 0.2] + }, + 'vel_limits': {'eef':[0.15, 0.15, 0.15], + 'jnt': [0.15, 0.25, 0.1, 0.25, 0.1, 0.25, 0.2, 1.0] + }, + 'init_qpos': [0.665, 0.567, 0.0, -1.999, 0.0, 0.9172, 1.4541, 0.2, 0.2], } ) diff --git a/robohive/envs/arms/franka/assets/franka_bin_push_v0.config b/robohive/envs/arms/franka/assets/franka_bin_push_v0.config new file mode 100644 index 00000000..f6ad5db2 --- /dev/null +++ b/robohive/envs/arms/franka/assets/franka_bin_push_v0.config @@ -0,0 +1,89 @@ +{ + # device1: sensors, actuators + 'franka':{ + 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale': 0.5}, + 'sensor':[ + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + + ], + + 'actuator':[ + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + ] + }, + + 'robotiq':{ + 'interface': {'type': 'robotiq', 'ip_address':'172.16.0.3'}, + 'sensor':[ + {'range':(0, 0.834), 'noise':0.0, 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, + ], + 'actuator':[ + {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, + ] + }, + + 'right_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_819112073358/color/image_raw', + 'd_topic': 'realsense_819112073358/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'left_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_948522071060/color/image_raw', + 'd_topic':'realsense_948522071060/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'top_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_815412071252/color/image_raw', + 'd_topic': 'realsense_815412071252/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_color'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'Franka_wrist_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_137222072789/color/image_raw', + 'd_topic': 'realsense_137222072789/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_color'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + +} \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_bin_push_v0.xml b/robohive/envs/arms/franka/assets/franka_bin_push_v0.xml new file mode 100644 index 00000000..87d50059 --- /dev/null +++ b/robohive/envs/arms/franka/assets/franka_bin_push_v0.xml @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_hang_push_v0.config b/robohive/envs/arms/franka/assets/franka_hang_push_v0.config new file mode 100644 index 00000000..eb986017 --- /dev/null +++ b/robohive/envs/arms/franka/assets/franka_hang_push_v0.config @@ -0,0 +1,89 @@ +{ + # device1: sensors, actuators + 'franka':{ + 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale': 0.5}, + 'sensor':[ + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + + ], + + 'actuator':[ + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + ] + }, + + 'robotiq':{ + 'interface': {'type': 'robotiq', 'ip_address':'172.16.0.3'}, + 'sensor':[ + {'range':(0, 0.834), 'noise':0.0, 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, + ], + 'actuator':[ + {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, + ] + }, + + 'right_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_817612071438/color/image_raw', + 'd_topic': 'realsense_817612071438/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'left_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_923322071682/color/image_raw', + 'd_topic':'realsense_923322071682/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'top_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_831612071935/color/image_raw', + 'd_topic': 'realsense_831612071935/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_color'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'Franka_wrist_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_936322070059/color/image_raw', + 'd_topic': 'realsense_936322070059/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_color'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + +} \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_hang_push_v0.xml b/robohive/envs/arms/franka/assets/franka_hang_push_v0.xml new file mode 100644 index 00000000..958ae6cf --- /dev/null +++ b/robohive/envs/arms/franka/assets/franka_hang_push_v0.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_planar_push_v0.config b/robohive/envs/arms/franka/assets/franka_planar_push_v0.config new file mode 100644 index 00000000..659aaf37 --- /dev/null +++ b/robohive/envs/arms/franka/assets/franka_planar_push_v0.config @@ -0,0 +1,89 @@ +{ + # device1: sensors, actuators + 'franka':{ + 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale': 0.5}, + 'sensor':[ + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + + ], + + 'actuator':[ + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + ] + }, + + 'robotiq':{ + 'interface': {'type': 'robotiq', 'ip_address':'172.16.0.3'}, + 'sensor':[ + {'range':(0, 0.834), 'noise':0.0, 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, + ], + 'actuator':[ + {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, + ] + }, + + 'right_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_138422075994/color/image_raw', + 'd_topic': 'realsense_138422075994/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'left_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_815412070347/color/image_raw', + 'd_topic':'realsense_815412070347/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'top_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_815412070804/color/image_raw', + 'd_topic': 'realsense_815412070804/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_color'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + + 'Franka_wrist_cam':{ + 'interface': {'type': 'realsense', + 'rgb_topic':'realsense_827112071730/color/image_raw', + 'd_topic': 'realsense_827112071730/depth_uncolored/image_raw', + 'data_type':'rgbd'}, + 'sensor':[], + 'cams': [ + {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_color'}, + {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':0.05, 'offset':0, 'name':'/depth_mono/image_raw'}, + ], + 'actuator':[] + }, + +} \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_planar_push_v0.xml b/robohive/envs/arms/franka/assets/franka_planar_push_v0.xml new file mode 100644 index 00000000..b3e5e0d8 --- /dev/null +++ b/robohive/envs/arms/franka/assets/franka_planar_push_v0.xml @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file