can't find path when use joint limits

[HugeKangroo]

Hello, I have imported a Fanuc robot from an URDF file and noticed that the joint limits have not been loaded. I manually read limits and set the limits by code like:

  for ii in range(1,7):
      min_value = minmax_dict[f"joint_{ii}"]["min"]
      max_value = minmax_dict[f"joint_{ii}"]["max"]
      # min_value = np.rad2deg(min_value)
      # max_value = np.rad2deg(max_value)

      min_values.append(min_value)
      max_values.append(max_value)
  min_values.append(0)
  max_values.append(0)

  self.robot.setJointLimits(min_values,max_values)

the joint limits from joint_1 to joint_6(in degree) are :

joint_1: upper limit = 170.00230739326656 deg, lower limit = -170.00230739326656 deg.
joint_2: upper limit = 159.99846429028239 deg, lower limit = -99.99832398418258 deg.
joint_3: upper limit = 219.9986045963822 deg, lower limit = -90.00021045914971 deg.
joint_4: upper limit = 200.00237754631647 deg, lower limit = -200.00237754631647 deg.
joint_5: upper limit = 140.00223724021663 deg, lower limit = -140.00223724021663 deg.
joint_6: upper limit = 270.0006313774491 deg, lower limit = -270.0006313774491 deg.

then I tried to run a motion planning but can not find a valid path. However if I does not use the setJointLimits(), I will get a path from the start point to end point:

pt: 0: [  36.67  30.94 -35.52   0.   -90.     0.      ]
pt: 1:  [36.72  30.85 324.42   0.08 269.95   0.1  ]
pt: 2:  [37.99  27.55 322.62 359.1  270.48 358.2   ]
pt: 3:  [61.18 326.8  289.59 339.39 281.15 320.96  ]
pt: 4:  [64.29 318.66 285.16 336.74 282.58 315.97 ]
pt: 5:  [64.33 318.54 285.11 336.65 282.65 316.08  ]
pt: 6:  [65.91 310.83 293.44 335.29 277.16 316.43    ]
pt: 7:  [40.24 320.01 305.8  338.63 276.84 323.13    ]
pt: 8:  [2.88 349.97 329.1  342.04 278.1  336.13   ]
pt: 9: [331.72  23.   348.86 348.55 272.02 357.65  ]
pt: 10: [333.88  27.41 352.27 353.85 268.78 359.58   ]
pt: 11: [-28.65  34.38 -10.31   0.   -90.     0.       ]

As you can see, some joint has exceed the limits but in a valid range. What should I do to get path in the limit ranges?

[krishauser]

The URDF limits should be respected, so I’m not sure what’s happening with your URDF model. Do you have a link to the URDF?

With the manual setting I see two possib issues. 1. Klampt has the quirk that fixed base robots have DOF 0 always fixed at 0. You are fixing DOF 6 at 0, which I assume is the end effector roll link. You should also make sure that your start and goal configuration follow this convention. 2. It looks like you are using degrees in some places? Klampt always assumes radians.

[HugeKangroo]

The URDF limits should be respected, so I’m not sure what’s happening with your URDF model. Do you have a link to the URDF?

With the manual setting I see two possib issues. 1. Klampt has the quirk that fixed base robots have DOF 0 always fixed at 0. You are fixing DOF 6 at 0, which I assume is the end effector roll link. You should also make sure that your start and goal configuration follow this convention. 2. It looks like you are using degrees in some places? Klampt always assumes radians.

Thank you for your reply.

Here is my URDF file: https://github.com/HugeKangroo/motion_planning/blob/master/fanuc_m20id35_support/urdf/fanuc_m20id35_klampt.urdf

1. I have created 8 links and 8 joints in the URDF. In the previous post, I hid the first and last links. Apologies for the confusion.
2. The degree is just for printing output. I use radians in my code.

If I do not set the joint limits using setJointLimits, there will be a valid path from the start to the end. However, if I set the joint limits from the URDF or to the range of -pi to pi, the planner says no feasible path was found.

Here is a link to my code: https://github.com/HugeKangroo/motion_planning/blob/master/MotionPlanning.py

[krishauser]

Ah, this is because the URDF file has set these joints to continuous rotation joints, and Klamp't doesn't un-set them to normal joints if you set joint limits. So something is going wrong when you have a combination of continuous rotation + joint limits -- because it's normalizing spin joints to [0,2pi], and using geodesic interpolation. The best thing to do would be to change the URDF to have joints of the "revolute" type.

[HugeKangroo]

Thank you for your response. I will try to modify the URDF