Question about planning in joint space

[crlz182]

I am using a custom robot with 14 dof and two endeffectors and I want to generate motions in joint space. However, I am not quite sure what is the best way to do this. Themotion_gen.plan_single() function solves IK internally so it cannot be used directly. I tried re-writing it and get a solution but also the status "Opt failed". This means that the optimizer fails generating a feasible motion.

The second option I tried was using the function motion_gen.plan_single_js(). However, I ususally get the output " Fail: Constraints".

Here is my code for the second option:

        motion_gen_config = MotionGenConfig.load_from_robot_config(
            self.robot_cfg,
            world_config,
            self.tensor_args,
            interpolation_steps=interpolation_steps,
            interpolation_type=InterpolateType.CUBIC,
            trajopt_tsteps=32,
            interpolation_dt=0.05,
            maximum_trajectory_dt=0.1,
            graph_trajopt_iters=500,
        )

        motion_gen = MotionGenCSpace(motion_gen_config)

        start_state = JointState.from_numpy(self.joint_names,start,np.zeros((1,14)),np.zeros((1,14)))
        goal_state = JointState.from_numpy(self.joint_names,goal,np.zeros((1,14)),np.zeros((1,14)))

        plan_config = MotionGenPlanConfig(
            enable_graph=True,
            max_attempts=50,
            timeout=20,
            enable_graph_attempt=20,
            enable_finetune_trajopt=False,

        )
        plan_result = motion_gen.plan_single_js(start_state,goal_state, plan_config)

I have the following questions:

1. How can I find out which constraints are violated exactly? Could this also mean the result is colliding with the environment?
2. What parameters could be altered to get better results?
3. As far as I understand, motion_gen.plan_single_js() does only use the trajectory optimizer. Is there a way to use the graph planner in this setting and then optimize the result?

[balakumar-s]

Regarding which constraints are violated, you can pass None for world_config to disable world collision checking. And then remove links one by one from collision_link_names to see when your plan is successful. Another way to debug is to try solving inverse kinematics to see if you get any solutions. If you don't get any solutions for IK, then it could mean that two links are in collision always and they would need to be added to the self_collision_ignore dictionary.

You can plan for two end-effectors to reach targets like this example: https://curobo.org/source/getting_started/2b_isaacsim_examples.html#multi-arm-reacher

We haven't added graph planner to the joint space planning api. We can look into this as a feature if the multi end-effector reacher is not working for you.

[crlz182]

Thank you very much for your reply. It turned out that the violated constraints were due to collision with the environment. In my setting, the trajectory solver fails most of the time when not being initialized it with the graph planner.

I have also had a look at the multi-arm reacher example in the meantime and made some progress based on that (see code below). However, the planner often fails in the optimization phase after the graph planner. I tried playing with the solver iterations, number of attempts and so on but not very successfully. Do you have any recommendations on how to setup the solver for a high-dimensional problem with multiple arms and various obstacles? I also got a bit lost in the number of parameters and where to set them. Some are accessible through the config but it seems that others have to be set in separate .yaml files.

        motion_gen_config = MotionGenConfig.load_from_robot_config(
            self.robot_cfg_yaml,
            world_config,
            self.tensor_args,
            interpolation_steps=400,
            trajopt_tsteps=50,
            collision_checker_type=CollisionCheckerType.PRIMITIVE,
            use_cuda_graph=True,
            num_trajopt_seeds=32,
            num_graph_seeds=64,
            interpolation_dt=0.03,
            collision_activation_distance=0.025,
            acceleration_scale=1.0,
            maximum_trajectory_dt=0.2,
            fixed_iters_trajopt=True,
            store_debug_in_result=True,
            num_ik_seeds=64,
            graph_trajopt_iters=5000,
            minimize_jerk=True
    )

        motion_gen = MotionGen(motion_gen_config)

        start_state = JointState.from_numpy(self.joint_names,start,np.zeros((1,14)),np.zeros((1,14)))
        goal_state = JointState.from_numpy(self.joint_names,goal,np.zeros((1,14)),np.zeros((1,14)))

        #get goal pose and link poses
        link_poses = {}
        pos, orn, _,_,_,_,_ = self.kin_model.forward(goal_state.position,"left_lbr_iiwa_link_7")
        pose = Pose(pos,orn)
        link_poses["left_lbr_iiwa_link_7"] = pose

        #this is the actual endeffector as in the robot yaml file
        pos, orn, _,_,_,_,_ = self.kin_model.forward(goal_state.position,"right_lbr_iiwa_link_7")
        goal_pose = Pose(pos,orn)

        plan_config = MotionGenPlanConfig(enable_graph=True,need_graph_success=True, enable_graph_attempt=60, max_attempts=60, enable_finetune_trajopt=True, timeout=20.0)

        plan_result = motion_gen.plan_single(start_state,goal_pose,plan_config,link_poses=link_poses)

        #usually prints "Opt Fail"
        print(plan_result.status)

[balakumar-s]

Did you try calling motion_gen.warmup() after creating motion_gen and before querying plans?

In addition:

1. Is it a dual arm system with 7 dof each?
2. How close are the two arms? Maybe there is a self collision issue?

Some notes on parameters:

1. Trajopt_tsteps = 32 should work for most trajectories. Increasing this will make the planner take more time to compute.
2. num_trajopt_seeds and num_graph_seeds can be same and you can try 12 - 32.
3. store_debug_in_result = False. Setting this to true will run the motion generation pipeline in debug mode. I am working on providing examples for how to visualize the debug files. Until then set this to false.

[crlz182]

I tried calling warmup. That just caused an "IK Fail" during planning instead of the "Opt Fail".

1. Yes, it is a dualarm system with two kuka iiwa 7
2. The two arms can theoretically touch each other but I used the yaml file to ignore certain collisions. Also, it works for some problems so it must be related to the planning problem itself. I think it struggles to handle situations where the arms have to get close or even cross each other.

Thank you for your input on the parameters. I will keep trying.

[balakumar-s]

We added graph planner to the joint space planning mode. We also tuned the cost terms.

Try out the latest release and let us know if joint space planning is better for your use case.

[crlz182]

``Thank you for the update,

Thank you for adding that feature as it makes my life much easier. I have tested the new release but ran into two problems:

1. The debug info does not get initialized if the graph planner fails. This leads to an error in this line:
   https://github.com/NVlabs/curobo/blob/c09d94908d8ad52f557ed59195c1ceb2d0434d65/src/curobo/wrap/reacher/motion_gen.py#L1606C96-L1606C96
   I could fix it by providing an empty debug_info dictionary like this:
   result = MotionGenResult(cspace_error=torch.zeros((1), device=self.tensor_args.device),debug_info={})

2. The planner returns with the log "Start or End state in collision". I exported the scene as a usd file but did not see any colliding spheres. I also use the following function to check for colliding start and goal states when I receive this error:

        config = RobotWorldConfig.load_from_config(robot_model, world_model,collision_activation_distance=0.0)
        curobo_fn = RobotWorld(config)
        state = curobo_fn.get_kinematics(q)
        d_world, d_self = curobo_fn.get_world_self_collision_distance_from_joints(q)
        print(d_world)
        print(d_self)

Both print statements result in tensor([0.], device='cuda:0') which means there is no collision or self-collision. What wlse could this be caused by? My config looks like this:

        motion_gen_config = MotionGenConfig.load_from_robot_config(
            self.robot_cfg_yaml,
            world_config,
            self.tensor_args,
            interpolation_steps=interpolation_steps,
            trajopt_tsteps=50,
            collision_checker_type=CollisionCheckerType.PRIMITIVE,
            use_cuda_graph=False,
            num_trajopt_seeds=48,
            num_graph_seeds=48,
            interpolation_dt=0.04,
            collision_activation_distance=0.0,
            acceleration_scale=1.0,
            maximum_trajectory_dt=0.2,
            finetune_dt_scale=1.0,
            fixed_iters_trajopt=True,
            store_debug_in_result=False,
            store_trajopt_debug=True,
            num_ik_seeds=64,
            graph_trajopt_iters=5000,
            position_threshold=0.05,
            rotation_threshold=0.1,
            minimize_jerk=True,
    ) 

[balakumar-s]

We also improved multi arm planning in the latest release so that might also resolve the issues you had with plan_single()