Constrained Planning

cuRobo also supports constrained motion generation where you can lock some linear and angular axes and generate motions to reach a target in the remaining axis. The current implementation does this by adding pose cost across timesteps during trajectory optimization. User can pass pose_cost_metric (curobo.rollout.cost.pose_cost.PoseCostMetric) configuration to curobo.wrap.reacher.motion_gen.MotionGenPlanConfig.pose_cost_metric, to set the constrains on the motion.

Isaac Sim Demo

Launch omni_python examples/isaac_sim/constrained_reacher.py to see the different constrained motions. Once Isaac Sim loads, click play and the robot will start moving between the two cubes.

For the first few sequences, the robot will move with no constraints on motion axes as seen below,

Then we set hold_vec_weight=[0,0,0,0,1,0] to restrict robot moving along the y-axis. Here the axis is with respect to the goal frame. The plane of motion is shown in red as below.

We then lock the orientation as well by setting hold_vec_weight=[1,1,1,0,1,0], which turns the plane green. Now the robot will have an upright orientation throughout it’s motion.

We constrain the linear axis further by setting hold_vec_weight=[0,0,0,1,1,0] which penalizes motions across x axis as well. A red line is visible in the demo now to highlight the line of motion.

Finally, we add orientation constraint as well by setting hold_vec_weight=[1,1,1,1,1,0]. The line now changes green highlighting that the orientation is also locked.

Note

The script will automatically go through these sequences and repeat until closed.

Note

By default cuRobo constrains motions with respect to the goal frame. To instead use the robot’s base frame, use project_pose_to_goal_frame=False in curobo.wrap.reacher.motion_gen.MotionGenConfig.load_from_robot_config.

Grasp Approach Vector

cuRobo also can enable constrained motions for part of a trajectory. This is useful in pick and place tasks where traditionally the robot goes to an offset pose (pre-grasp pose) and then moves to the grasp pose in a linear motion along 1 axis (e.g., z axis) while also constraining it’s orientation. We can formulate this two step process as a single trajectory optimization problem, with orientation and linear motion costs activated for the second portion of the timesteps. To use this feature set pose_cost_metric = PoseCostMetric.create_grasp_approach_metric(offset_position=0.1, tstep_fraction=0.6,linear_axis=2) where linear_axis specifies the x y or z axis, t_step_fraction specifies the timestep fraction to start activating this constraint, and offset_position is the offset in meters. Since this is added as a cost, the trajectory will not reach the exact offset, instead it will try to take a blended path to the final grasp without stopping at the offset.

You can see the effect of this cost by runnning omni_python examples/isaac_sim/simple_stacking.py --constrain_grasp_approach. This will show the behavior as below.