Motion planning for stretch robot

[Entongsu]

Hello,

I've been working on implementing inverse kinematics (IK) reachability and motion planning for the stretch robot. I've successfully generated collision spheres, which work well for IK reachability. However, I've encountered challenges with motion generation. Regardless of how I adjust the target position or orientation, it fails to determine a viable path. Could you kindly offer some guidance or hints to help me overcome this issue?

Your assistance would be greatly appreciated. Thank you!

[Entongsu]

I have fixed this problem. Thank you!

[balakumar-s]

What was the issue?

[Entongsu]

Thank you for your reply. The solver failed to find a path for the stretch, but I have fixed this problem by modifying the pdf file. But I have found another problem. For the revolute joints, the rotation of the real robot and spheres is opposite, and how can I change that?

[balakumar-s]

Can you share the urdf, meshes, and the robot configuration file?

[Entongsu]

Sure. Here is the link.https://drive.google.com/drive/folders/1iepJdC8bh6P1aPDGA_zSlWB2OjDVZjbu?usp=sharing. The major problem is that if you rotate the target grasp center point, the gripper will rotate in the opposite direction.

[balakumar-s]

Can you share the commit tag of cuRobo you are using? We added support for negative axis only in a recent commit.

[balakumar-s]

I was able to run your code after making a few changes:

1. Removed base_joint_z that was adding a height joint which is not required.
2. Changed the retract config so that the joints are not at a limit or cause self collision.

I could not see the issue you had where the gripper was rotating in the opposite direction. Can you try the below yaml file and check if the issue still exists?

Here is the updated yaml file:

robot_cfg:
   kinematics:
    usd_path: robot/stretch_robot/stretch_description_RE2V0_tool_dex_wrist.usd
    usd_robot_root: /robot
    isaac_usd_path: 
    use_usd_kinematics: False
   
    usd_flip_joint_limits: []
    urdf_path: robot/stretch_robot/stretch_description_RE2V0_tool_dex_wrist.urdf
    asset_root_path: robot/stretch_robot/RE2V0
    base_link: link_ground
    ee_link: link_grasp_center
    link_names: null
    lock_joints: null
    extra_links: null
    usd_flip_joints: ["joint_wrist_yaw"]
    usd_flip_joints: {
    "joint_wrist_yaw": "Z",
  
    }
    collision_link_names: ["base_link","link_mast","link_arm_l4","link_arm_l3","link_arm_l2","link_arm_l1","link_arm_l0","link_gripper_finger_left","link_gripper_finger_right"]
    collision_spheres:
      base_link:
        - "center": [0.0, 0.12, 0.06]
          "radius": 0.12
        - "center": [0.0, -0.12, 0.06]
          "radius": 0.12
        - "center": [0.0, 0.04, 0.06]
          "radius": 0.12
        - "center": [0.0, -0.04, 0.06]
          "radius": 0.12
        - "center": [-0.24, -0.12, 0.06]
          "radius": 0.12
        - "center": [-0.24, 0.12, 0.06]
          "radius": 0.12
        - "center": [-0.24, -0.04, 0.06]
          "radius": 0.12
        - "center": [-0.16, -0.12, 0.06]
          "radius": 0.12
        - "center": [-0.08, -0.12, 0.06]
          "radius": 0.12
        - "center": [-0.08, 0.12, 0.06]
          "radius": 0.12
        - "center": [-0.16, 0.12, 0.06]
          "radius": 0.12
      link_mast:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.1
        - "center": [0.0, 1.3, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.081, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.162, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.244, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.325, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.406, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.487, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.569, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.65, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.731, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.812, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.894, 0.0]
          "radius": 0.1
        - "center": [0.0, 0.975, 0.0]
          "radius": 0.1
        - "center": [0.0, 1.056, 0.0]
          "radius": 0.1
        - "center": [0.0, 1.137, 0.0]
          "radius": 0.1
        - "center": [0.0, 1.219, 0.0]
          "radius": 0.1
      link_arm_l0:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.13]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.043]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.087]
          "radius": 0.06
      link_arm_l1:
        - "center": [0.0, 0.0, -0.13]
          "radius": 0.06
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.087]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.043]
          "radius": 0.06
      link_arm_l3:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.13]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.043]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.087]
          "radius": 0.06
      link_arm_l2:
        - "center": [0.0, 0.0, -0.13]
          "radius": 0.06
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.087]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.043]
          "radius": 0.06
      link_arm_l4:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.13]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.043]
          "radius": 0.06
        - "center": [0.0, 0.0, -0.087]
          "radius": 0.06
      link_gripper_finger_left:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.03
        - "center": [0.169, -0.0, 0.0]
          "radius": 0.03
        - "center": [0.028, -0.0, 0.0]
          "radius": 0.03
        - "center": [0.056, -0.0, 0.0]
          "radius": 0.03
        - "center": [0.085, -0.0, 0.0]
          "radius": 0.03
        - "center": [0.113, -0.0, 0.0]
          "radius": 0.03
        - "center": [0.141, -0.0, 0.0]
          "radius": 0.03
      link_gripper_finger_right:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.03
        - "center": [-0.179, 0.0, 0.005]
          "radius": 0.03
        - "center": [-0.03, 0.0, 0.001]
          "radius": 0.03
        - "center": [-0.06, 0.0, 0.002]
          "radius": 0.03
        - "center": [-0.09, 0.0, 0.002]
          "radius": 0.03
        - "center": [-0.12, 0.0, 0.003]
          "radius": 0.03
        - "center": [-0.149, 0.0, 0.004]
          "radius": 0.03


    collision_sphere_buffer: 0.005
    extra_collision_spheres: {}
    self_collision_ignore: {
      base_link: [link_mast,link_arm_l4,link_arm_l3,link_arm_l2,link_arm_l1,link_arm_l0,link_gripper_finger_left,link_gripper_finger_right], #[link_ground, link_mast, link_arm_l4, link_arm_l3, link_arm_l2, link_arm_l1, link_arm_l0],
      link_mast: [link_arm_l4,link_arm_l3,link_arm_l2,link_arm_l1,link_arm_l0,link_gripper_finger_left,link_gripper_finger_right],
      link_arm_l4: [link_arm_l3,link_arm_l2,link_arm_l1,link_arm_l0,link_gripper_finger_left,link_gripper_finger_right],
      link_arm_l3: [link_arm_l2,link_arm_l1,link_arm_l0,link_gripper_finger_left,link_gripper_finger_right],
      link_arm_l2: [link_arm_l1,link_arm_l0,link_gripper_finger_left,link_gripper_finger_right],
      link_arm_l1: [link_arm_l0,link_gripper_finger_left,link_gripper_finger_right],
      link_arm_l0: [link_gripper_finger_left,link_gripper_finger_right],
      link_gripper_finger_left: [link_gripper_finger_right],
      

      # link_ground: [link_mast,link_arm_l4,link_arm_l3, link_arm_l2, link_arm_l1, link_arm_l0],
      # link_mast: [link_arm_l4,link_arm_l3, link_arm_l2, link_arm_l1, link_arm_l0 ],
      # link_arm_l4: [link_arm_l3, link_arm_l2, link_arm_l1, link_arm_l0],
      # link_arm_l3: [link_arm_l2, link_arm_l1, link_arm_l0],
      # link_arm_l2: [link_arm_l1, link_arm_l0],
      # link_arm_l1: [link_arm_l0],

      }
    self_collision_buffer: {'base_link': 0, "link_mast": 0,"link_arm_l4": 0 ,"link_arm_l3": 0, "link_arm_l2": 0, "link_arm_l1": 0, "link_arm_l0": 0,"link_gripper_finger_left": 0,"link_gripper_finger_right": 0
     
      } 
      
    use_global_cumul: True
    mesh_link_names: null # List[str]

    cspace:
      joint_names: [
        "base_x", "base_y","rotation_z","joint_lift","joint_arm_l3","joint_arm_l2","joint_arm_l1","joint_arm_l0","joint_wrist_yaw","joint_wrist_pitch","joint_wrist_roll","joint_gripper_finger_right","joint_gripper_finger_left"]
      retract_config: [0,0,0,0.5,0.05,0.05,0.05,0.05,0,0,0,0,0]
      null_space_weight: [1,1,1,1,1,1,1,1,1,1,1,1,1]
      cspace_distance_weight: [1,1,1,1,1,1,1,1,1,1,1,1,1]
      max_acceleration: 15.0 #15.0
      max_jerk: 500.0