MotionGen.plan_single() - specifying joint range constraints?

[parth-emancro]

Please provide the below information in addition to your issue:

cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): docker python
python version: 3.10
Isaac Sim version (if using): NA

I'm trying to use MotionGen.plan_single() to plan from our platform’s 7 DOF robot arm's start joint angles to a Cartesian goal ee pose.

To avoid cable-twisting, we needed constraints on joint angles to be close to preferred JointState’s already collected. Using a restricted joint-angle range in the URDF greatly reduced the fraction of successful cartesian plans.

Issue: We get about 40mm median cartesian error using the below method to incorporate the constraints, since MotionGen.plan_single() doesn’t directly take joint angle range-constraints.

1. Get a number of IK solutions for goal ee pose using IKSolver.solve_single() (50-200 return seeds)
2. Filter these IK solutions over our set of preferred joint states /angles
3. Ranking them by how close they are to any of our preferred JS
4. Plan using MotionGen.plan_single_js()

The cspace error from IKSolver is on the order of 0.01-0.05 radians as per get_unique_solutions() from the IKResult object.

When comparing the goal Pose we query above, with the last JointState we get from the plan_single_js() trajectory passed through Curobo’s forward kinematic model, the cartesian error is 1e-6

However this same error is median 41mm when compared to the cartesian goal pose we request over a ROS message which we convert to a Curobo Pose in a callback.

I believe this is not a frame transform error given

1. We get <1mm error if we directly use MotionGen.plan_single() with high_precision=True . Since we can’t pass in preferred joint states constraints we can’t use this directly.

Statically changing our robot arm URDF joints to have restricted limits results in the planning success rate dropping from 0.99-1.0 (with our IK solutions method), to 0.75 [ plan_single() ] even with 15 retries on failure.

2. There’s a significant standard deviation on the error as shown below.

Planner Benchmarks: {
  "total_count": 1000,
  "success_count": 1000,
  "success_rate_fraction": 1.0,
  "ik_success_count": 1000,
  "ik_success_rate_fraction": 1.0
},
 Planning Time Stats (seconds): {
  "total_sum": 841.9068222045898,
  "mean": 0.8419068222045898,
  "std": 0.21002498291249855,
  "median": 0.801460862159729,
  "min": 0.6779828071594238,
  "max": 3.2417209148406982,
  "fraction_within_1_stddev": 0.96,
  "fraction_within_20pct_mean": 0.948
}
)
 Goalposition XYZ error Stats (mm): {
  "mean": 42.98088259831211,
  "std": 21.157441075081874,
  "median": 41.18384363548193,
  "min": 0.26255438303209727,
  "max": 114.72418631150224,
  "fraction_within_1_stddev": 0.663,
  "fraction_within_20pct_mean": 0.319
}
)

Questions -

A. Is there another way to achieve these joint-state constraints while having a higher success rate with plan_single()?
B. Is there a way to get `MotionGen.plan_single() to try out n-number of IK seeds internally?
C. Ideas on the resultant end-to-end cartesian error given IKSolver seems to think internally that the solutions it generates have very low error.?