check start state validity and minor fixes

src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml

@@ -19,28 +19,28 @@ robot_cfg:
 
     urdf_path: "robot/ur_description/ur5e_robotiq_2f_140.urdf"
     asset_root_path: "robot/ur_description"
-    
+
     base_link: "base_link"
     ee_link: "grasp_frame"
     link_names: null
-    lock_joints: {'finger_joint': 0.3}
+    lock_joints: {'finger_joint': 0.0}
 
-    extra_links: null #{"attached_object":{"parent_link_name": "grasp_frame" , 
+    extra_links: null #{"attached_object":{"parent_link_name": "grasp_frame" ,
     #"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
     #"joint_name": "attach_joint" }}
     extra_collision_spheres: null #{"attached_object": 4}
 
-    collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 
+    collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
     'wrist_2_link' ,'wrist_3_link', 'tool0', 'robotiq_arg2f_base_link',
     'left_outer_knuckle',
-    'left_inner_knuckle', 
-    'left_outer_finger', 
-    'left_inner_finger', 
-    'left_inner_finger_pad', 
+    'left_inner_knuckle',
+    'left_outer_finger',
+    'left_inner_finger',
+    'left_inner_finger_pad',
     'right_outer_knuckle',
-    'right_inner_knuckle', 
+    'right_inner_knuckle',
     'right_outer_finger' ,
-    'right_inner_finger', 
+    'right_inner_finger',
     'right_inner_finger_pad',
     ] # List[str]
     collision_spheres:
@@ -117,7 +117,7 @@ robot_cfg:
             "radius": 0.01
           - "center": [0.0, 0.015, -0.01]
             "radius": 0.01
-        
+
       left_inner_finger_pad:
           - "center": [0.0, -0.0, 0.005]
             "radius": 0.01
@@ -135,7 +135,7 @@ robot_cfg:
       left_inner_finger:
           - "center": [0.0, -0.0, -0.0]
             "radius": 0.01
-         
+
       left_outer_knuckle:
           - "center": [0.0, 0.055, 0.01]
             "radius": 0.01
@@ -148,7 +148,7 @@ robot_cfg:
             "radius": 0.01
           - "center": [0.0, 0.015, -0.01]
             "radius": 0.01
-      
+
       right_inner_finger_pad:
           - "center": [0.0, -0.0, 0.005]
             "radius": 0.01
@@ -166,7 +166,7 @@ robot_cfg:
       right_inner_finger:
           - "center": [0.0, -0.0, -0.0]
             "radius": 0.01
-         
+
       right_outer_knuckle:
           - "center": [0.0, 0.055, 0.01]
             "radius": 0.01
@@ -175,25 +175,25 @@ robot_cfg:
 
 
     collision_sphere_buffer: 0.005
-   
+
     self_collision_ignore: {
       "upper_arm_link": ["forearm_link", "shoulder_link"],
       "forearm_link": ["wrist_1_link", "wrist_2_link", "wrist_3_link"],
       "wrist_1_link": ["wrist_2_link","wrist_3_link","tool0", "robotiq_arg2f_base_link"],
       "wrist_2_link": ["wrist_3_link", "tool0",  "robotiq_arg2f_base_link"],
       "wrist_3_link": ["tool0", "robotiq_arg2f_base_link"],
-      
-      "tool0": ['robotiq_arg2f_base_link', 'left_outer_finger', 'left_inner_finger_pad', 
+
+      "tool0": ['robotiq_arg2f_base_link', 'left_outer_finger', 'left_inner_finger_pad',
       'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
       'right_outer_finger', 'right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
 
-      "robotiq_arg2f_base_link": ['left_outer_finger', 'left_inner_finger_pad', 
+      "robotiq_arg2f_base_link": ['left_outer_finger', 'left_inner_finger_pad',
       'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
       'right_outer_finger', 'right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
-      
-      "left_outer_finger": ['left_inner_finger_pad', 
+
+      "left_outer_finger": ['left_inner_finger_pad',
       'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
       'right_outer_finger', 'right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
@@ -216,7 +216,7 @@ robot_cfg:
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
 
 
-      
+
       "right_outer_finger": ['right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
 
@@ -227,36 +227,36 @@ robot_cfg:
 
       "right_inner_finger": [ 'right_outer_knuckle'],
 
-      
+
 
       }
     self_collision_buffer: {
       'shoulder_link': 0.01,
-      'upper_arm_link': 0, 
-      'forearm_link': 0, 
-      'wrist_1_link': 0, 
+      'upper_arm_link': 0,
+      'forearm_link': 0,
+      'wrist_1_link': 0,
       'wrist_2_link': 0,
       'wrist_3_link' : 0,
       'tool0': 0.02,
-      } 
-      
+      }
+
     use_global_cumul: True
-    mesh_link_names:  ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 
+    mesh_link_names:  ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
     'wrist_2_link' ,'wrist_3_link', 'robotiq_arg2f_base_link',
     'left_outer_knuckle',
-    'left_inner_knuckle', 
+    'left_inner_knuckle',
     'left_outer_finger',
-    'left_inner_finger', 
+    'left_inner_finger',
     'right_outer_knuckle',
-    'right_inner_knuckle', 
+    'right_inner_knuckle',
     'right_outer_finger' ,
-    'right_inner_finger', 
+    'right_inner_finger',
     ] # List[str]
 
     cspace:
-      joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 
+      joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                     'wrist_2_joint', 'wrist_3_joint',
-                    'finger_joint'] 
+                    'finger_joint']
       retract_config:  [0.0, -2.2, 1.0, -1.383, -1.57, 0.00, 0.6]
       null_space_weight:  [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,1.0]
       cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]