NVlabs / curobo

CUDA Accelerated Robot Library
https://curobo.org
Other
796 stars 125 forks source link

Motion Generation seems not use my specified EEF link #400

Open FlyCole opened 1 month ago

FlyCole commented 1 month ago

Hi, I'm trying to do motion generation based on moving_gripper setup, in the case that there's no whole robot arm planning, so the final generated trajectory should look like a straight line. The issue is, if I choose to use panda_hand as my eef_link (which does have a collision body), the final trajectory looks good to me as in the first image; However, if I choose to use panda_hand_tcp as my eef_link (which doesn't have a collision body), the final trajectory looks really weird as in the second one. Then if I still use panda_hand_tcp as my eef_link and double the distance between panda_hand_tcp and panda_hand in my urdf file, the trajectory is even more twisted as in the third figure. From observation, it seems all three of them are planned based on panda_hand rather than the EEF I specified, and then because there are some twisting during the trajectory, so the fixed distance between the panda_hand and the EEF I specified will enlarge this twist and that's why it looks like that. That's why I'm wondering if the motion generation really changes when I specify the ee_link in the .yml file. I also attached my moving_gripper.yml and moving_gripper.urdf files here. Could you please let me know is there any problem with it? Thank you! Screenshot from 2024-10-05 14-33-16 Screenshot from 2024-10-05 14-32-28 Screenshot from 2024-10-05 14-40-29

moving_gripper.yml

 robot_cfg:
    kinematics:
        usd_path: null
        usd_robot_root: null
        isaac_usd_path: ""
        usd_flip_joints: {}
        usd_flip_joint_limits: []

        urdf_path: "robot/franka_description/moving_gripper_2.urdf"
        asset_root_path: "robot/franka_description"

        base_link: "base"
        ee_link: "panda_hand_tcp"
        link_names: null
        lock_joints: null
        extra_links: null

        collision_link_names: ["panda_hand"] # List[str]
        collision_spheres:
            panda_hand:
                - "center": [0.0, -0.075, 0.01]
                  "radius": 0.023
                - "center": [0.0, -0.045, 0.01]
                  "radius": 0.023
                - "center": [0.0, -0.015, 0.01]
                  "radius": 0.023
                - "center": [0.0, 0.015, 0.01]
                  "radius": 0.023
                - "center": [0.0, 0.045, 0.01]
                  "radius": 0.023
                - "center": [0.0, 0.075, 0.01]
                  "radius": 0.023
                - "center": [0.0, -0.08, 0.03]
                  "radius": 0.022
                - "center": [0.0, -0.045, 0.03]
                  "radius": 0.022
                - "center": [0.0, -0.015, 0.03]
                  "radius": 0.022
                - "center": [0.0, 0.015, 0.03]
                  "radius": 0.022
                - "center": [0.0, 0.045, 0.03]
                  "radius": 0.022
                - "center": [0.0, 0.08, 0.03]
                  "radius": 0.022
                - "center": [0.0, -0.08, 0.045]
                  "radius": 0.022
                - "center": [0.0, -0.045, 0.045]
                  "radius": 0.022
                - "center": [0.0, -0.015, 0.045]
                  "radius": 0.022
                - "center": [0.0, 0.015, 0.045]
                  "radius": 0.022
                - "center": [0.0, 0.045, 0.045]
                  "radius": 0.022
                - "center": [0.0, 0.08, 0.045]
                  "radius": 0.022
        collision_sphere_buffer: 0.005
        extra_collision_spheres: {}
        self_collision_ignore: {}
        self_collision_buffer: {} # Dict[str, float]

        use_global_cumul: True
        mesh_link_names: null # List[str]

        cspace:
            joint_names: ["x_joint", "y_joint", "z_joint", "rx_joint", "ry_joint", "rz_joint", "panda_hand_tcp_joint"] # List[str]
            retract_config:  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # List[float]
            null_space_weight: [1,1,1,1,1,1,1] # List[str]
            cspace_distance_weight: [1,1,1,1,1,1,1] # List[str]
            max_jerk: 500.0
            max_acceleration: 150.0

URDF

<?xml version="1.0"?>
<robot name="moving_gripper">
  <link name="base"/>
  <link name="base1"/>
  <link name="base2"/>
  <link name="base3"/>
  <link name="base4"/>
  <link name="base5"/>
  <joint name="x_joint" type="prismatic">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base"/>
    <child link="base1"/>
    <axis xyz="1 0 0"/>
    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>

  <joint name="y_joint" type="prismatic">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base1"/>
    <child link="base2"/>
    <axis xyz="0 1 0"/>
    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>

  <joint name="z_joint" type="prismatic">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base2"/>
    <child link="base3"/>
    <axis xyz="0 0 1"/>
    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>
  <joint name="rx_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base3"/>
    <child link="base4"/>
    <axis xyz="1 0 0"/>
    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>
  <joint name="ry_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base4"/>
    <child link="base5"/>
    <axis xyz="0 1 0"/>
    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>
  <joint name="rz_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base5"/>
    <child link="panda_hand"/>
    <axis xyz="0 0 1"/>
    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
  </joint>
  <link name="panda_hand">
    <visual>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0 0"/>
      <geometry>
         <mesh filename="file:///home/ryf/AugImagination/robot/meshes/visual/hand.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.0"/>
      <geometry>
          <mesh filename="file:///home/ryf/AugImagination/robot/meshes/collision/hand.obj"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.73"/>
      <origin xyz="-0.01 0 0.03"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.0025" iyz="0" izz="0.0017"/>
    </inertial>
  </link>
  <link name="panda_hand_tcp"/>
  <joint name="panda_hand_tcp_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.1034"/>
    <parent link="panda_hand"/>
    <child link="panda_hand_tcp"/>
  </joint>
</robot>