Clarification on the collision cost

[proyan]

Dear developers,

I recently started to tryout your wonderful library, and while I am able to run the examples without any issue, a slight modification to the examples did not result in the expected behaviour. I wonder if you could explain the reason for that.

I am trying to replicate the motion generation example available in the docs. The example itself works perfectly.

I then adapt the collision_table.yml to add another cuboid, pole, as follows:

cuboid:                                                                                                                                                                                                            
  table:                                                                                                                                                                                                           
    dims: [5.0, 5.0, 0.2] # x, y, z                                                                                                                                                                                
    pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw                                                                                                                                                 
  pole:                                                                                                                                                                                                            
    dims: [0.01, 0.01, 5] # x, y, z                                                                                                                                                                                
    pose: [0.1, 0.05, 0.0, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw                                                                                                                                                 

My hope is to see the collision avoidance behaviour between the robot and the pole when it tries to reach the target. However, that does not happen, and it goes through the pole:

franka_example_with_pole.mp4

I can see in the lag in motion generation that the collision with the pole is being considered, but eventually it is not acted upon. Could you please help me understand how to create a proper example that exhibits this sort of avoidance behaviour? Thank you.
Best,
Rohan

[proyan]

Trying with different target configurations, I see the following behaviours:

franka_example_with_pole1.mp4

franka_example_with_pole2.mp4

which makes the first behaviour an outlier (perhaps low cost weights?)

[balakumar-s]

In the first example where it failed, can you give me few details:

1. Did you check if result.success was True?
2. Can you give me the start configuration and the goal pose so that I can reproduce this locally?
3. Are you rendering the trajectory steps or running with a controller to track the trajectory in the provided visualization?

[proyan]

Hi @balakumar-s ,

Here is the information on the above test-case, and some further exploration that I did:

---

Did you check if result.success was True?

I just checked, and indeed the result.success is False. The status as well mentions : Opt Fail.

Can you give me the start configuration and the goal pose so that I can reproduce this locally?

The test case is exactly as in the example above. The start and goal pose are obtained from the retract config as follows:

retract_cfg = motion_gen.get_retract_config()
state = motion_gen.rollout_fn.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
result = motion_gen.plan_single(start_state, goal_pose = retract_pose, plan_config = MotionGenPlanConfig(max_attempts=1))

The description of the pole is as mentioned above:
The description of the pole in the scene is the same as in my first message:

cuboid:                                                                                                                                                                                                            
  table:                                                                                                                                                                                                           
    dims: [5.0, 5.0, 0.2] # x, y, z                                                                                                                                                                                
    pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw                                                                                                                                                 
  pole:                                                                                                                                                                                                            
    dims: [0.01, 0.01, 5] # x, y, z                                                                                                                                                                                
    pose: [0.1, 0.05, 0.0, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw                                                                                                                                                 

Are you rendering the trajectory steps or running with a controller to track the trajectory in the provided visualization?

The visualization that I show is a simple rendering using the trajectory obtained from the MotionGen solver. There is no outside engine interacting with the curobo solver in any way.

---

After your remarks, I went back to try and increase the max_attempts in the MotionGenConfig to try and find a solution. It looks to me that the graph planner is where the problem fails. However, somewhat surprisingly, when I set the max_attempts to 4 or above, I get the following result status:

In [1]: result.status
Out[1]: 'IK Fail'
In [2]: result.attempts
Out[2]: 4
In [3]: result.success
Out[3]: tensor([False], device='cuda:0')

Since IK has been done already successfully, I am surprised that the solver fails at the IK level after having already found a solution in previous attempts. From what I understood in the technical report, the planner stage is only after the IK stage. Am I missing something here?

---

As another trial, I added a mesh geometry using a .ply flie to the world, and let the robot find a collision free path around the mesh. I set the collision_checker_type=CollisionCheckerType.Mesh to enable collision with meshes.

The solver is unable to find a path from a non-colliding initial to a non-colliding final state, even though the path exists. I have tried to increase the num_graph_seeds to 30 in MotionGenPlanConfig, as well as in MotionGenConfig (I assume one of these is redundant, but I set both to be sure). I also set enable_graph=True in MotionGenPlanConfig.

The result seems to be stuck in the same minima that I suppose the first test case gets stuck in. Could you please point me to the parameters I could tune to increase the chances of success? Other than the number of seeds for trajopt/IK for example?

[balakumar-s]

We improved convergence properties in the latest release so you should see less outliers.

Here is a video that shows the performance from the latest release:

pole_test.mp4

Two things I noticed from the position of the pole:

1. It puts the robot in collision at the goal pose as it's colliding with one of the spheres that we use to approximate panda_hand.
2. The pole is very close to the base, limiting the movement of the first joint (see attached image). So the robot will have limited reachability.
   

Regarding your other questions:

Since IK has been done already successfully, I am surprised that the solver fails at the IK level after having already found a solution in previous attempts. From what I understood in the technical report, the planner stage is only after the IK stage. Am I missing something here?

cuRobo planning attempts start from scratch, so at every attempt it will find new IK solutions. So if at some attempt, there is not IK solution, it will return IK Fail.