Merge code for dynamixel

robohive/envs/arms/franka/assets/franka_bin_pick_rp03_v0.config

@@ -0,0 +1,89 @@
+{
+    # device1: sensors, actuators
+    'franka':{
+        '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'},
+            {'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'},
+        ]
+    },
+
+    # device1: sensors, actuators
+    '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_831612072257/color/image_raw',
+                      'd_topic': 'realsense_831612072257/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        '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':0.05, 'offset':0, 'name':'/depth_mono/image_raw'},
+        ],
+        'actuator':[]
+    },
+
+    'left_cam':{
+        'interface': {'type': 'realsense',
+                      'rgb_topic':'realsense_815412070729/color/image_raw',
+                      'd_topic':'realsense_815412070729/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        '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':0.05, 'offset':0, 'name':'/depth_mono/image_raw'},
+        ],
+        'actuator':[]
+    },
+
+    'top_cam':{
+        'interface': {'type': 'realsense',
+                      'rgb_topic':'realsense_831612071119/color/image_raw',
+                      'd_topic': 'realsense_831612071119/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        'sensor':[],
+        'cam': [
+            {'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_814412072030/color/image_raw',
+                      'd_topic':  'realsense_814412072030/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        'sensor':[],
+        'cam': [
+            {'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':[]
+    },
+}

robohive/envs/arms/franka/assets/franka_bin_pick_rp05_v0.config

@@ -0,0 +1,89 @@
+{
+    # device1: sensors, actuators
+    'franka':{
+        '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'},
+            {'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'},
+        ]
+    },
+
+    # device1: sensors, actuators
+    '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_818312071351/color/image_raw',
+                      'd_topic': 'realsense_818312071351/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        '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':0.05, 'offset':0, 'name':'/depth_mono/image_raw'},
+        ],
+        'actuator':[]
+    },
+
+    'left_cam':{
+        'interface': {'type': 'realsense',
+                      'rgb_topic':'realsense_745412071685/color/image_raw',
+                      'd_topic':'realsense_745412071685/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        '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':0.05, 'offset':0, 'name':'/depth_mono/image_raw'},
+        ],
+        'actuator':[]
+    },
+
+    'top_cam':{
+        'interface': {'type': 'realsense',
+                      'rgb_topic':'realsense_815412070225/color/image_raw',
+                      'd_topic': 'realsense_815412070225/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        'sensor':[],
+        'cam': [
+            {'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_135122076787/color/image_raw',
+                      'd_topic':  'realsense_135122076787/depth_uncolored/image_raw',
+                      'data_type':'rgbd'},
+        'sensor':[],
+        'cam': [
+            {'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':[]
+    },
+}

robohive/envs/arms/franka/assets/franka_bin_pick_v0.config

@@ -0,0 +1,89 @@
+{
+    # device1: sensors, actuators
+    'franka':{
+        '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'},
+            {'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'},
+        ]
+    },
+
+    # device1: sensors, actuators
+    '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':[],
+        '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':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':[],
+        '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':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':[],
+        'cam': [
+            {'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':[],
+        'cam': [
+            {'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':[]
+    },
+}

robohive/envs/arms/franka/assets/franka_bin_pick_v0.xml

@@ -0,0 +1,63 @@
+<mujoco model="RoboPen-BinPick">
+<!-- =================================================
+    Copyright 2022 Vikash Kumar
+    Model   :: BinPick (MuJoCoV2.0)
+    Author  :: Vikash Kumar (vikashplus@gmail.com)
+    source  :: https://github.com/vikashplus/robohive
+    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.
+====================================================== -->
+
+    <size njmax='1000' nconmax='1000'/>
+
+    <include file="../../../../sims/scene_sim/topfloor_scene.xml"/>
+    <include file="../../../../sims/furniture_sim/simpleTable/simpleTable_asset.xml"/>
+    <include file="../../../../sims/furniture_sim/ventionTable/ventionTable_asset.xml"/>
+    <include file="../../../../sims/franka_sim/assets/assets.xml"/>
+    <include file="../../../../sims/franka_sim/assets/actuator0.xml"/>
+    <include file="../../../../sims/robotiq_sim/assets/assets.xml"/>
+    <include file="../../../../sims/furniture_sim/bin/bin_asset.xml"/>
+
+    <compiler inertiafromgeom="auto" inertiagrouprange="3 4" angle="radian" meshdir="../../../../sims/franka_sim" texturedir="../../../../sims/franka_sim"/>
+
+    <worldbody>
+
+        <camera name='left_cam' pos='0.16 0.68 1.35' quat='-0.2 -0.11 0.45 0.86'/>
+        <camera name='right_cam' pos='0.78 -0.68 1.35' quat='0.87 0.45 0.09 0.17'/>
+        <camera name='top_cam' pos='0.5 0 2.2' euler='0 0 -1.57'/>
+        <site name='workspace' type='box' size='.6 .375 .25' pos='0 0.475 1.0' group='3' rgba='0 0 .4 .2'/>
+
+        <!-- Franka Arm-->
+        <body pos='0 0 .823' euler='0 0 0'>
+            <include file="../../../../sims/franka_sim/assets/chain0_nogripper.xml"/>
+        </body>
+
+        <!-- Robotiq Hand-->
+        <body name="ee_mount" pos="0 0 .11" euler="0 0 0">
+            <include file="../../../../sims/robotiq_sim/assets/chain.xml"/>
+        </body>
+
+        <!-- Tables -->
+        <body name="ventiontable" pos='-0.3 0 0' euler='0 0 1.57'>
+            <include file="../../../../sims/furniture_sim/ventionTable/ventionTable_body.xml"/>
+        </body>
+        <body name="scenetable" pos='0.49 0 0.065' euler="0 0 1.57">
+            <include file="../../../../sims/furniture_sim/simpleTable/simpleMarbleTable_body.xml"/>
+        </body>
+
+        <!-- tasks details added here via include-->
+        <body name="busbin1" pos='0.5 0.0 0.825'  euler="0 0 1.57">
+            <include file="../../../../sims/furniture_sim/bin/busbin3_body.xml"/>
+        </body>
+
+        <site name='drop_target' type='sphere' size='.075' pos='0.0 0.5 1.1' group='1' rgba='.1 .8 .2 0.0'/>
+
+        <body name="obj0" pos="0.5 0.0 0.855">
+            <inertial pos="0 0 0" mass="0.088" diaginertia="0.0001 0.0001 0.0001" />
+            <geom type="box" size="0.0762 0.0254 0.0254" pos="0.0 0.0 0.0" euler="0 0 0" group="1" rgba="0.94 0.9 0.55 1" mass=".100"/>
+            <site name="obj0" size="0.01" pos="0 0 0"/>
+            <freejoint name="obj0"/>
+        </body>   
+
+    </worldbody>
+
+</mujoco>