Curobo with Isaac sim: IK failure

[sanskar1]

I’m trying to use cuRobo with Isaac Sim. I followed their instructions for installation and the examples run well. I tried configuring my own robot (a robot with two arms from the waist above). The URDF is fine as it works with other solvers. I followed the steps under configuring your own robot, initially only trying to achieve one arm control (later I want to try dual arm control).

As my robot has a lot of collision spheres, I was getting collision errors upon starting, so for now I have made every link ignore collision with another.

However, I just get: “Plan did not converge to a solution: MotionGenStatus.IK_FAIL”.

I’ve attached the .yml config file as a .txt file. I can add some snippets from my urdf if needed, but I can’t upload the entire file due confidentiality reasons.

I’m very stumped as to why I’m stuck. Any help to debug this issue would be very appreciated.
trial.txt (82.8 KB)

[zhengwang]

Thanks and welcome to the community!
Could you please provide the snippets of your robot URDF if that doesn’t conflict with confidentiality? I will try to replicate your issue on my desktop.

[zhengwang]

I think one thing you can try is to run our reachability example with your robot and see which regions are reachable. If there are reachable regions, try to move the target within the reachable region.

[sanskar1]

I could provide some snippets but I’m not sure it would be helpful. I would have to provide the meshes as well for the urdf to compile and that would be against company policies.

I looked more into it and I think the issue might be that our robot is a 5 DOF robot. when i move the cube in small distances up and down the z-axis, it gets solutions.

So, one thing I want to try is adding a 6th DOF and see how much that changes things.

When i try to run the reachability program, I move the cube and nothing happens. In the terminal, there’s no expected print statements like “IK completed: …”. It just doesn’t do anything. I’m currently trying to debug that. If you have come across this, could you tell me what it means?

Another question I have is, is there a way to set a tolerance for the end effector position and orientation in python with isaac sim?

Thanks for your help!

[sanskar1]

So quick update: I made one change to every script. I commented out the line:

and np.linalg.norm(past_pose - cube_position) == 0.0

Now, even if the robot cannot reach the last cube pose, I can move it around to try a new motion plan rather than getting stuck.

When I run ik_reachability.py, I get all red dots. When I run motion_gen_reacher.py, I get “Plan did not converge to a solution: MotionGenStatus.IK_FAIL”.

When I run multi_arm_reacher.py, the primary end effector can be controlled very well, specially after adding PoseCostMetric. However, the secondary arm, whenever I try moving that, I get the IK failure error or an error saying trajectory optimization failed.

It doesn’t matter which arm/end-effector I choose as primary, they work well, but neither as secondary.

Dual arm control is essential for my project. Can you help me debug this?

I was thinking of some quick-fix solutions that could work if the example does not:

1. run two simulations, each controlling one arm (other arm is locked from curobo) and then stream joint angles from one to the opther simulation.
2. run two motion_gen objects in the same simulation - make two config files (one for each arm) and two different motion_gen objects, making it so that both function as primary. I don’t wanna try this as I don’t think curobo supports multiple motion planners, plus there might be errors due to two set of urdfs/ collision spheres being generated.

If the example doesn’t work, what would be the best approach?

Thanks!

[zhengwang]

The ik_reachability.py script will generate a bunch of goal poses based on the target pose and offsets. Then the goal poses will be passed into the IK solver to find a solution. The goal poses with solutions will show up as green dots (reachable). The goal poses without a solution will show up as red dots (unreachable). Based on the result, the robot arm will start moving to the reachable goal poses.

Based on your description, it seems that no goal poses are reachable. So after IK solver (and prints “IK completed”), the robot arm would stay still.

Another question I have is, is there a way to set a tolerance for the end effector position and orientation in python with isaac sim?

For our IKSolverConfig, we load robot configuration via function load_from_robot_config. By default, the position_threshold(link) is 0.005 and rotation_threshold(link) is 0.05. You can change the values in the corresponding example you run. In the ik_reachbility.py example, you can change the values here. For the motion_gen_reacher.py, no values are set for rotation_threshold and position_threshold, so it is using the default values. But you can add them yourselves (link). Here is what I tried:

    motion_gen_config = MotionGenConfig.load_from_robot_config(
        ...
        rotation_threshold=1,
        position_threshold=1,
        tensor_args=tensor_args,
        ...
    )

Maybe you can set both thresholds to larger values and see if the dual robot arm can converge to a solution?

As for the multi_arm_reacher.py example, the tutorial mentioned that

Multi-Arm motion planning is experimental and does not work as well as single arm planning. There are open research questions on how to solve multi-arm planning and cuRobo’s current implementation is only meant to be a starting point for research in this area.

Also for your case, are you trying to have both arms reaching to the target at the same time?

[sanskar1]

@zhengwang

So after IK solver (and prints “IK completed”), the robot arm would stay still.

This would make sense if it printed IK completed or a failure message. But I don’t get anything. Not even red dots. It works with the franka model though.

@zhengwang

Also for your case, are you trying to have both arms reaching to the target at the same time?

They don’t necessarily need to reach at the same time, but it would be preferred if they both could move/start as soon as possible (rather than one arm at a time).

Also, is it possible to control some of the robots’ joints by streaming joint angles? I think I’ll have to resort to running two different simulations. If I could stream the joint angles, then I could run one half of the hand in one simulation, and the other half by streaming joint angles from another simulation (configured to control just the other arm).

[zhengwang]

Could you please check the names of both robot arms? They need to have distinct names in order to work properly.

[sanskar1]

@zhengwang

  motion_gen_config = MotionGenConfig.load_from_robot_config(
        ...
        rotation_threshold=1,
        position_threshold=1,
        tensor_args=tensor_args,
        ...
    )

Turns out this was all I needed to get the multi_arm_control running. I had a PoseCostMetric which turns out is only associated to the primary end effector. This i why I was able to control the primary end effector but not the second one. Increasing the threshold increased the reachability and dexterity.

Is there any word on when multi-end-effector support will be full fledged?

Thanks for your help!

[zhengwang]

Great it works out!

Just curious, did you also set the link_poses for the secondary arm when calling the API plan_single? With link_poses, the MotionGen class could calculate a motion plan for specified links. In your case, if you set it for the end effector of secondary arm, it should try to compute a result as well. Theoretically you should be able to use it to control multi-end-effector.

Why do you use PoseCostMetric? Is there some constraint you wish to add for robot arm motion?

I am also trying to use the dual ur10e robot to replicate your case. Here is what I did:

1. Update src/curobo/content/configs/robot/dual_ur10e.yml file to ignore all of the self collision and change collision sphere buffer to 0.0 and self collision buffer to 0.0
2. In `src/curobo/content/configs/robot/spheres/dual_ur10e.yml1 file, change all of the radius to 0.0
3. In src/curobo/content/assets/robot/ur_description/dual_ur10e.urdf, change the position of base_link_1 to move the robot arm closer to the other arm so they can reach the same target
4. In multi_arm_reacher.py, increase the position & rotation thresholds for motion generation and change the target pose for link tool1 to the pose of the red cuboid.

I attached all of the files that I made changes and you can give it a try with command omni_python examples/isaac_sim/multi_arm_reacher.py --robot dual_ur10e.yml

dual_ur10e.urdf (19.8 KB)
dual_ur10e.yml (5.6 KB)
dual_ur10e.yml (2.7 KB)
multi_arm_reacher.py (11.4 KB)

Here is a video of moving the target and both arms are trying to reach to it. Please note that it is possible that no plan is generated due to the joint position limits.

[sanskar1]

Yeah I ended up using the multi_arm_reacher.py, put the two links in link_names and one in ee_link. It wasn’t working earlier because the thresholds were too low.

From going through the source code, it looked like PoseCostMetric can also be used for tolerance as well ( I did not know about position_threshold and rotation_threshold then). I’m not sure if that is what it’s intended for, but it does seem to do the job, although only for the primary end effector. Here’s how I set it up:

from curobo.rollout.cost.pose_cost import PoseCostMetric
pose_cost_metric = PoseCostMetric(
    hold_partial_pose=True,
    release_partial_pose=False,
    hold_vec_weight=torch.tensor([ 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]),
    reach_partial_pose=True,
    reach_full_pose=False,
    reach_vec_weight=torch.tensor([ 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]),
    offset_position=None,
    offset_rotation=None,
    offset_tstep_fraction=0.75,
    remove_offset_waypoint=False,
    include_link_pose=True,
)

Setting position_threshold and rotation_threshold added tolerance to both end effectors.

Everything seems fine except for one issue that pops up sometimes. The sim starts fine and within the joint limits, but at some point, while following the cube, the sim says that the robot is at an invalid start state.

I wonder if the robot maybe get too close to the joint limits (or even surpass them) in attempts to reach some partially reachable end effector pose?

[zhengwang]

Sorry for my late reply.
PostCostMetric is used for setting up constraint planning. Here are more explanations and examples for using PostCostMetric: Constrained Planning - cuRobo

I didn’t set up PostCostMetric and I also see the warning MotionGenStatus.INVALID_START_STATE_JOINT_LIMITS.
I have reached out to our internal team for more information on this error. I will let you know once I hear from them.

Since this is a different issue, I created another post for it.