Trajectory Optimization Tutorial#

Prerequisites#

The following tutorials are recommended prior to starting this tutorial:

Objectives#

Through this tutorial, you will learn how to:

  1. Create a problem setup with a single robot and a world with obstacles

  2. Create a trajectory optimization config

  3. Set a parameter in the trajectory optimization config

  4. Create a trajectory optimization solver

  5. Solve for a trajectory to a target position

  6. Working with Trajectory objects

  7. Update the world

  8. Use the RobotWorldInspector to check for collisions

  9. Solve for a trajectory with path constraints

  10. Solve for a trajectory with a goalset

What is Trajectory Optimization?#

Trajectory optimization is a broad discipline whose history spans several decades and many industries. In the context of the cuMotion library, trajectory optimization involves finding feasible trajectories that go from an initial c-space position to a desired target (more details on what “target” means below). The goal is to find the fastest collision-free path. Due to limitations of actuators and the limited control bandwidth of the motion controllers used to execute the trajectories, the cuMotion algorithm balances minimum time with “smoothness,” which the current implementation achieves by minimizing c-space acceleration and jerk.

cuMotion defines a feasible path to be one that satisfies all the constraints:

  • Constraints on the terminal c-space position (either through task-space or c-space constraints)

  • Constraints on the pose of the tool frame throughout the entire trajectory

  • C-space position limits

  • C-space velocity limits

  • C-space acceleration limits

  • C-space jerk limits

  • Self-collision

  • World-collision

The trajectory optimization algorithm implemented by the cuMotion library makes the following assumptions:

  • The initial and final c-space velocities, accelerations, and jerks are all zero.

  • Purely kinematic trajectories are sufficient for most applications. (The current algorithm does not allow for any type of torque/force/energy minimization.)

  • Only the best feasible trajectory needs to be provided to the user.

  • All obstacles are stationary for the duration of the trajectory.

In general, exactly solving trajectory optimization problems requires solving an infinite-dimensional nonlinear optimization problem. (The reader can correctly assume that this means they are very hard to solve.) There are many approaches to simplifying these problems to make them computationally feasible, but all will—at best—return merely approximations to the globally optimal solution. More typically, the true global optimal is completely unknown or intractable to find, and the algorithm will produce a “good enough” feasible solution.

The current algorithm implemented by the cuMotion library starts with a particle-based optimization (PBO) method that helps identify coarse high-quality solutions. These coarse solutions are then optimized using a limited-memory quasi-Newton method (L-BFGS) that refines these coarse solutions to feasible trajectories that are time-optimized and smooth.

Setting up the Robot and Environment#

Here we load a UR10 robot, create a world, an empty world view, and add a cuboid obstacle near the initial position of the tool frame. See the tutorials listed in the Prerequisites for more details on these steps.

// Load the UR10 robot description.
std::unique_ptr<cumotion::RobotDescription> robot_description =
    cumotion::LoadRobotFromFile(xrdf_file, urdf_file);
assert(robot_description != nullptr);

// Get the tool frame name and default c-space position.
std::string tool_frame_name = robot_description->toolFrameNames().front();
Eigen::VectorXd initial_cspace_position = robot_description->defaultCSpaceConfiguration();

// Create a world and add a world view.
std::shared_ptr<cumotion::World> world = cumotion::CreateWorld();
cumotion::WorldViewHandle world_view = world->addWorldView();

// Create a cuboid obstacle near the end effector.
std::unique_ptr<cumotion::Kinematics> kinematics = robot_description->kinematics();
Eigen::Vector3d initial_tool_position =
    kinematics->position(initial_cspace_position, kinematics->frame(tool_frame_name));

std::unique_ptr<cumotion::Obstacle> cuboid =
    cumotion::CreateObstacle(cumotion::Obstacle::Type::CUBOID);
Eigen::Vector3d obstacle_size = Eigen::Vector3d(0.1, 0.1, 0.1) * 2;
cuboid->setAttribute(cumotion::Obstacle::Attribute::SIDE_LENGTHS, obstacle_size);

cumotion::Pose3 obstacle_pose =
    cumotion::Pose3::FromTranslation(initial_tool_position + Eigen::Vector3d(0.25, 0.0, 0.0));
cumotion::World::ObstacleHandle obstacle_handle = world->addObstacle(*cuboid, obstacle_pose);

# Load the UR10 robot description.
robot_description: cumotion.RobotDescription = cumotion.load_robot_from_file(
    xrdf_path, urdf_path)

# Get the tool frame name and default c-space position.
tool_frame_name = robot_description.tool_frame_names()[0]
initial_cspace_position = robot_description.default_cspace_configuration()

# Create a world and add a world view.
world: cumotion.World = cumotion.create_world()
world_view: cumotion.WorldView = world.add_world_view()

# Create a cuboid obstacle near the end effector.
kinematics: cumotion.Kinematics = robot_description.kinematics()
initial_tool_position = kinematics.position(initial_cspace_position, tool_frame_name)

cuboid: cumotion.Obstacle = cumotion.create_obstacle(cumotion.Obstacle.Type.CUBOID)
obstacle_size = np.array([0.1, 0.1, 0.1]) * 2
cuboid.set_attribute(cumotion.Obstacle.Attribute.SIDE_LENGTHS, obstacle_size)

obstacle_pose = cumotion.Pose3.from_translation(
    initial_tool_position + np.array([0.25, 0.0, 0.0]))
obstacle_handle: cumotion.World.ObstacleHandle = world.add_obstacle(cuboid, obstacle_pose)

Note that we’re going to use the default c-space position specified by the robot description as the initial c-space position of the robot.

Creating a Trajectory Optimization Config#

After creating the robot and world, we can create a cumotion::TrajectoryOptimizerConfig object, which is used to specify the parameters for the trajectory optimization solver. The config object needs the robot description, the tool frame name, and a world view. We can create a new config using either of the following methods:

The first method is useful for when users have a set of tuned solver parameters they want to set each time a trajectory optimization config object is created.

The second method is recommended for new users, and is what we use to create the config object below:

std::unique_ptr<cumotion::TrajectoryOptimizerConfig> config =
    cumotion::CreateDefaultTrajectoryOptimizerConfig(
        *robot_description, tool_frame_name, world_view);

config: cumotion.TrajectoryOptimizerConfig = (
    cumotion.create_default_trajectory_optimizer_config(
        robot_description, tool_frame_name, world_view))

Setting a Parameter#

The trajectory optimization solver has many parameters that affect the performance of the solver and the quality of the solution returned. While most of these have been tuned so that they should be applicable for a broad variety of robots and environments, users may find improvements in computational efficiency or solution quality by tuning these parameters for their application.

The documentation for cumotion::TrajectoryOptimizerConfig::setParam() contains the list of all the parameters, their default values, and an explanation of how they effect the solver. Here we increase the number of L-BFGS iterations used at the end of the algorithm, which increases computational cost but often produces higher quality smooth trajectories:

bool success = config->setParam("trajopt/lbfgs/retiming_iterations", 500);
if (!success) {
  std::cerr << cumotion::text_style::BRIGHT_RED
            << "Failed to set parameter 'trajopt/lbfgs/retiming_iterations'\n"
            << cumotion::text_style::RESET;
}

success = config.set_param("trajopt/lbfgs/retiming_iterations", 500)
if not success:
    print("\033[91mFailed to set parameter 'trajopt/lbfgs/retiming_iterations'\033[0m")

As demonstrated above, it is recommended to capture the boolean output of setParam() and check that the parameter was successfully set. Non-fatal error messages will be logged if the cumotion::LogLevel::ERROR log level is enabled.

Currently, cuMotion does not provide an interface for querying the current value of a trajectory optimization solver parameter. This is likely to change in future versions.

Creating the Solver#

Once we have set the solver parameters using the configuration object, we can instantiate the solver using cumotion::CreateTrajectoryOptimizer():

std::unique_ptr<cumotion::TrajectoryOptimizer> optimizer =
    cumotion::CreateTrajectoryOptimizer(*config);

optimizer: cumotion.TrajectoryOptimizer = cumotion.create_trajectory_optimizer(config)

Solve to a Target C-Space Position#

Solving a trajectory optimization problem involves three steps:

  1. Defining the task-space target, task-space goalset, or c-space target

  2. Defining the task-space path constraints

  3. Calling cumotion::TrajectoryOptimizer::planToTaskSpaceTarget(),

    cumotion::TrajectoryOptimizer::planToTaskSpaceGoalset(), or cumotion::TrajectoryOptimizer::planToCSpaceTarget().

Here we start with one of the simplest options: planning to c-space target, with no constraints on the path taken to reach the target:

Eigen::VectorXd target_cspace_position = initial_cspace_position;
target_cspace_position[0] -= 0.8;
target_cspace_position[1] -= 0.6;
target_cspace_position[2] += 0.6;
cumotion::TrajectoryOptimizer::CSpaceTarget cspace_target(target_cspace_position);

target_cspace_position = initial_cspace_position.copy()
target_cspace_position[0] -= 0.8
target_cspace_position[1] -= 0.6
target_cspace_position[2] += 0.6
cspace_target = cumotion.TrajectoryOptimizer.CSpaceTarget(target_cspace_position)

There are many different types of terminal and path constraints we can choose from. We will cover a few more during this tutorial, and provide an overview of all the available constraints at the end.

After defining a cumotion::TrajectoryOptimizer::CSpaceTarget, we call cumotion::TrajectoryOptimizer::planToCSpaceTarget(), which returns (a pointer to) a cumotion::TrajectoryOptimizer::Results object:

std::unique_ptr<cumotion::TrajectoryOptimizer::Results> results =
    optimizer->planToCSpaceTarget(initial_cspace_position, cspace_target);

results: cumotion.TrajectoryOptimizer.Results = optimizer.plan_to_cspace_target(
    initial_cspace_position, cspace_target)

Querying the Optimized Trajectory#

Once we have (a pointer to) a cumotion::TrajectoryOptimizer::Results we first need to check the cumotion::TrajectoryOptimizer::Results::Status:

auto print_status = [](const cumotion::TrajectoryOptimizer::Results &results) {
  cumotion::TrajectoryOptimizer::Results::Status status = results.status();
  if (status == cumotion::TrajectoryOptimizer::Results::Status::SUCCESS) {
    std::cout << cumotion::text_style::BRIGHT_GREEN << "  Trajectory optimization succeeded!"
              << cumotion::text_style::RESET << "\n";
  } else {
    std::cout << cumotion::text_style::BRIGHT_RED << "  Trajectory optimization failed!"
              << cumotion::text_style::RESET << "\n";
  }
  std::cout << "  Trajectory time: " << results.trajectory()->domain().span() << " seconds\n";
};
std::cout << "Initial solve (with no obstacles in the world view):\n";
print_status(*results);

def print_status(results: cumotion.TrajectoryOptimizer.Results):
    """Print the status and trajectory time."""
    status = results.status()
    if status == cumotion.TrajectoryOptimizer.Results.Status.SUCCESS:
        print("\033[92m  Trajectory optimization succeeded!\033[0m")
    else:
        print("\033[91m  Trajectory optimization failed!\033[0m")

    trajectory = results.trajectory()
    domain = trajectory.domain()
    print(f"  Trajectory time: {domain.upper - domain.lower} seconds")


print("Initial solve (with no obstacles in the world view):")
print_status(results)

Output:

Initial solve (with no obstacles in the world view):
  Trajectory optimization succeeded!
  Trajectory time: 0.645759 seconds

Tip

As shown in the C++ example above, cumotion/text_style.h includes some useful constants for manipulating the style of text sent to the terminal.

Here we create a helper function that checks the status and prints a helpful message based on the result, which we’ll reuse later.

As long as the status is SUCCESS, we can get the optimized trajectory using cumotion::TrajectoryOptimizer::Results::trajectory():

std::unique_ptr<cumotion::Trajectory> initial_trajectory = results->trajectory();
cumotion::Trajectory::Domain domain = initial_trajectory->domain();

initial_trajectory: cumotion.Trajectory = results.trajectory()
domain = initial_trajectory.domain()

This function returns a new copy of the trajectory each time it is called, so it’s better to save the trajectory to a local variable to avoid unnecessary copies. Note we actually create a temporary copy of the trajectory in print_status.

As shown above, we can use cumotion::Trajectory::domain() to get a cumotion::Trajectory::Domain object, which provides information about the time domain over which the trajectory is defined.

Querying the trajectory at a given time:

With a cumotion::Trajectory object, we can evaluate the position, velocity, acceleration, and jerk anywhere in the time domain of the trajectory:

double mid_time = (domain.lower + domain.upper) / 2.0;
int num_coords = robot_description->numCSpaceCoords();
Eigen::VectorXd mid_position(num_coords);
Eigen::VectorXd mid_velocity(num_coords);
Eigen::VectorXd mid_acceleration(num_coords);
Eigen::VectorXd mid_jerk(num_coords);
initial_trajectory->eval(mid_time, &mid_position, &mid_velocity, &mid_acceleration, &mid_jerk);
std::cout << "\n  Mid-trajectory position: [" << mid_position.transpose() << "]\n";

mid_time = (domain.lower + domain.upper) / 2.0
num_coords = robot_description.num_cspace_coords()
mid_position, mid_velocity, mid_acceleration, mid_jerk = initial_trajectory.eval_all(mid_time)
print(f"\n  Mid-trajectory position: {mid_position}")

Output:

Mid-trajectory position: [  -1.96899   -1.88544   -1.26998   -1.55027    1.56002 0.00981941]

We can also ask for the minimum and maximum c-space positions achieved (asynchronously) by each coordinate, as well as the maximum c-space velocities, accelerations, and jerks:

Eigen::VectorXd min_pos = initial_trajectory->minPosition();
Eigen::VectorXd max_pos = initial_trajectory->maxPosition();
Eigen::VectorXd max_vel = initial_trajectory->maxVelocityMagnitude();
Eigen::VectorXd max_accel = initial_trajectory->maxAccelerationMagnitude();
Eigen::VectorXd max_jerk = initial_trajectory->maxJerkMagnitude();
std::cout << "  Max velocity magnitude: [" << max_vel.transpose() << "]\n";
std::cout << "  Max acceleration magnitude: [" << max_accel.transpose() << "]\n";
std::cout << "  Max jerk magnitude: [" << max_jerk.transpose() << "]\n";
std::cout << std::endl;

min_pos = initial_trajectory.min_position()
max_pos = initial_trajectory.max_position()
max_vel = initial_trajectory.max_velocity_magnitude()
max_accel = initial_trajectory.max_acceleration_magnitude()
max_jerk = initial_trajectory.max_jerk_magnitude()
print(f"  Max velocity magnitude: {max_vel}")
print(f"  Max acceleration magnitude: {max_accel}")
print(f"  Max jerk magnitude: {max_jerk}")
print()

Output:

Max velocity magnitude: [  2.15784   1.57412   1.55129  0.108781 0.0561832 0.0549447]
Max acceleration magnitude: [ 10.6658  10.0294  9.09374  1.35142  0.67106 0.631026]
Max jerk magnitude: [287.342 266.967 244.902 33.7659 17.2246 16.1836]

Using the Robot World Inspector#

The trajectory we got above looks great, but there’s one big problem: it’s passing straight through the obstacle! To see this, we’re going to create a cumotion::RobotWorldInspector, and use it to check for collision along the trajectory:

auto check_for_collisions = [](const cumotion::RobotWorldInspector &inspector,
                               const cumotion::Trajectory &trajectory) {
  // Check for collisions along the trajectory.
  auto [t0, tf] = trajectory.domain();
  double dt = 0.01;  // Sample every 10 ms
  std::optional<double> first_collision_time;
  std::optional<double> last_collision_time;
  for (double t = t0; t <= tf; t += dt) {
    Eigen::VectorXd cspace_pos = trajectory.eval(t);
    if (inspector.inCollisionWithObstacle(cspace_pos)) {
      if (!first_collision_time.has_value()) {
        first_collision_time = t;
      }
      last_collision_time = t;
    }
  }

  if (first_collision_time.has_value()) {
    std::cout << cumotion::text_style::BRIGHT_RED << "  Trajectory is in collision!"
              << cumotion::text_style::RESET << "\n";
    std::cout << "    First collision at t = " << first_collision_time.value() << " seconds\n";
    std::cout << "    Last collision at t = " << last_collision_time.value() << " seconds\n";
  } else {
    std::cout << cumotion::text_style::BRIGHT_GREEN << "  Trajectory is collision-free!"
              << cumotion::text_style::RESET << "\n";
  }
};

// Create a robot world inspector to check for collisions.
auto new_world_view = world->addWorldView();
std::unique_ptr<cumotion::RobotWorldInspector> inspector =
    cumotion::CreateRobotWorldInspector(*robot_description, new_world_view);

// Check for collisions.
check_for_collisions(*inspector, *initial_trajectory);

def check_for_collisions(inspector: cumotion.RobotWorldInspector, trajectory: cumotion.Trajectory):
    """Check for collisions along the trajectory."""
    domain = trajectory.domain()
    t0, tf = domain.lower, domain.upper
    dt = 0.01  # Sample every 10 ms
    first_collision_time: Optional[float] = None
    last_collision_time: Optional[float] = None

    t = t0
    while t <= tf:
        cspace_pos = trajectory.eval(t)
        if inspector.in_collision_with_obstacle(cspace_pos):
            if first_collision_time is None:
                first_collision_time = t
            last_collision_time = t
        t += dt

    if first_collision_time is not None:
        print("\033[91m  Trajectory is in collision!\033[0m")
        print(f"   First collision at t = {first_collision_time} seconds")
        print(f"   Last collision at t = {last_collision_time} seconds")
    else:
        print("\033[92m  Trajectory is collision-free!\033[0m")


# Create a robot world inspector to check for collisions.
new_world_view = world.add_world_view()
inspector: cumotion.RobotWorldInspector = cumotion.create_robot_world_inspector(
    robot_description, new_world_view)

# Check for collisions.
check_for_collisions(inspector, initial_trajectory)

Output:

Trajectory is in collision!
  First collision at t = 0.19 seconds
  Last collision at t = 0.48 seconds

Updating the World View#

Since we added the obstacle to the world after creating the world view, the world view passed to the solver was completely unaware there was an obstacle in the environment. Let’s update the world view and resolve:

world_view.update();
results = optimizer->planToCSpaceTarget(initial_cspace_position, cspace_target);
std::cout << "\nAfter updating world view:\n";
print_status(*results);

world_view.update()
results = optimizer.plan_to_cspace_target(initial_cspace_position, cspace_target)
print("\nAfter updating world view:")
print_status(results)

Output:

After updating world view:
  Trajectory optimization succeeded!
  Trajectory time: 0.745283 seconds

Now lets sanity-check that our new trajectory isn’t in collision with the environment:

auto trajectory_with_collision_enabled = results->trajectory();
check_for_collisions(*inspector, *trajectory_with_collision_enabled);

trajectory_with_collision_enabled = results.trajectory()
check_for_collisions(inspector, trajectory_with_collision_enabled)

Output:

Trajectory is collision-free!

Planning to a Task-Space Target#

Now that we have a trajectory to our new c-space position, let’s find a new trajectory to a point 20 cm below:

// Plan to a task-space target 20 cm below the target c-space position.
Eigen::Vector3d target_tool_position =
    kinematics->position(target_cspace_position, kinematics->frame(tool_frame_name));
Eigen::Vector3d new_target_position = target_tool_position - Eigen::Vector3d(0.0, 0.0, 0.2);

cumotion::TrajectoryOptimizer::TranslationConstraint translation_constraint =
    cumotion::TrajectoryOptimizer::TranslationConstraint::Target(new_target_position);
cumotion::TrajectoryOptimizer::TaskSpaceTarget task_space_target(translation_constraint);

std::cout << "\nTask-space goal:\n";
results = optimizer->planToTaskSpaceTarget(target_cspace_position, task_space_target);
print_status(*results);

# Plan to a task-space target 20 cm below the target c-space position.
target_tool_position = kinematics.position(target_cspace_position, tool_frame_name)
new_target_position = target_tool_position - np.array([0.0, 0.0, 0.2])

translation_constraint = cumotion.TrajectoryOptimizer.TranslationConstraint.target(
    new_target_position)
task_space_target = cumotion.TrajectoryOptimizer.TaskSpaceTarget(translation_constraint)

print("\nTask-space goal:")
results = optimizer.plan_to_task_space_target(target_cspace_position, task_space_target)
print_status(results)

Output:

Task-space goal:
  Trajectory optimization succeeded!
  Trajectory time: 0.65272 seconds

For these types of short moves, we typically want the tool to translation along a linear path, without any change in orientation. Let’s calculate the orientation change to see if that’s what we’re getting:

auto orientation_change = [&kinematics,
                           &tool_frame_name](const cumotion::Trajectory &trajectory) {
  auto [t0, tf] = trajectory.domain();
  Eigen::VectorXd initial_cspace_position = trajectory.eval(t0);
  Eigen::VectorXd final_cspace_position = trajectory.eval(tf);

  auto tool_frame = kinematics->frame(tool_frame_name);
  cumotion::Rotation3 initial_orientation =
      kinematics->orientation(initial_cspace_position, tool_frame);
  cumotion::Rotation3 final_orientation =
      kinematics->orientation(final_cspace_position, tool_frame);
  return cumotion::Rotation3::Distance(initial_orientation, final_orientation);
};

// Calculate the orientation change.
auto task_space_trajectory = results->trajectory();
std::cout << "  Change in tool orientation: "
          << orientation_change(*task_space_trajectory) * 180.0 / M_PI << " deg\n";

def orientation_change(trajectory: cumotion.Trajectory) -> float:
    """Calculate the change in tool orientation along the trajectory."""
    domain = trajectory.domain()
    t0, tf = domain.lower, domain.upper
    initial_cspace_position = trajectory.eval(t0)
    final_cspace_position = trajectory.eval(tf)

    initial_orientation = kinematics.orientation(initial_cspace_position, tool_frame_name)
    final_orientation = kinematics.orientation(final_cspace_position, tool_frame_name)
    return cumotion.Rotation3.distance(initial_orientation, final_orientation)


task_space_trajectory = results.trajectory()
print("  Change in tool orientation: {} deg".format(
    orientation_change(task_space_trajectory) * 180.0 / np.pi))

Output:

Change in tool orientation: 71.8535 deg

Uh-oh! We’re seeing some significant orientation change. Since we’re allowing the orientation to be anything, the solver is finding a trajectory that moves to a different final orientation. In the next section, we’ll see how to explicitly ask the solver to maintain the same orientation throughout the entire trajectory.

Adding Path Constraints#

In the above example, we didn’t constraint the final orientation, so even though we wanted a simple linear move (in task space), the tool rotated. It also didn’t follow a straight line down to the new position. To keep the same orientation throughout the entire trajectory, we can use the Constant orientation constraint. To have the tool move in a straight line between the initial and final positions, we can use a LinearPathConstraint.

After adding these two path constraints and solving, we get a trajectory that’s probably more what we expect from a simple translation in task-space:

// Add path constraints for constant orientation and linear translation.
cumotion::TrajectoryOptimizer::OrientationConstraint orientation_constraint =
    cumotion::TrajectoryOptimizer::OrientationConstraint::Constant();
translation_constraint =
    cumotion::TrajectoryOptimizer::TranslationConstraint::LinearPathConstraint(
        new_target_position);
task_space_target = cumotion::TrajectoryOptimizer::TaskSpaceTarget(translation_constraint,
                                                                   orientation_constraint);

std::cout << "\nTask-space goal with path constraints:\n";
results = optimizer->planToTaskSpaceTarget(target_cspace_position, task_space_target);
print_status(*results);
auto task_space_trajectory_with_path_constraints = results->trajectory();
std::cout << "  Change in tool orientation: "
          << orientation_change(*task_space_trajectory_with_path_constraints) * 180.0 / M_PI
          << " deg\n";

# Add path constraints for constant orientation and linear translation.
orientation_constraint = cumotion.TrajectoryOptimizer.OrientationConstraint.constant()
translation_constraint = (
    cumotion.TrajectoryOptimizer.TranslationConstraint.linear_path_constraint(
        new_target_position))
task_space_target = cumotion.TrajectoryOptimizer.TaskSpaceTarget(
    translation_constraint, orientation_constraint)

print("\nTask-space goal with path constraints:")
results = optimizer.plan_to_task_space_target(target_cspace_position, task_space_target)
print_status(results)
task_space_trajectory_with_path_constraints = results.trajectory()
print(f"  Change in tool orientation: "
      f"{orientation_change(task_space_trajectory_with_path_constraints) * 180.0 / np.pi} deg")

Output:

Task-space goal with path constraints:
  Trajectory optimization succeeded!
  Trajectory time: 0.396552 seconds
  Change in tool orientation: 0.0485696 deg

So not only did we get almost zero orientation change, it also resulted in a shorter trajectory!

Planning to a Goalset#

Sometimes we don’t have a single desired task-space goal, but rather several goals that are all equally valid. For example, this arises when there are:

  • Multiple valid grasps for the same object

  • Multiple objects to be grasped, without preference for which is grasped first

Here we demonstrate the following:

  1. How to define a goalset

  2. How to solve for a trajectory to a goalset

  3. How to identify which goal / target in the goalset the trajectory “picked”

It is important to note that cuMotion makes the following assumptions about goalsets:

  • All goals / targets have identical constraint types. For example, they all must be full pose targets, or they all must specify only position, and leave orientation free.

  • All goals / targets are equally valid. There can be no preference for which is selected (this can, of course, by done externally by the user, either by making individual calls for each target, or by filtering the returned solutions)

  • Goalsets can only be in task-space. C-space goalsets are unsupported.

Defining a Goalset#

Defining a goalset task is straightforward. Here we demonstrate specifying two goals, one on either side of the obstacle, this time allowing the orientation at the target to be unconstrained:

Eigen::Vector3d goal1 = target_tool_position + Eigen::Vector3d(-0.5, 0.0, 0.0);
Eigen::Vector3d goal2 = target_tool_position + Eigen::Vector3d(0.0, -1.0, 0.0);
std::vector<Eigen::Vector3d> translation_targets = {goal1, goal2};

cumotion::TrajectoryOptimizer::TranslationConstraintGoalset translation_goalset =
    cumotion::TrajectoryOptimizer::TranslationConstraintGoalset::Target(translation_targets);
cumotion::TrajectoryOptimizer::TaskSpaceTargetGoalset goalset(translation_goalset);

goal1 = target_tool_position + np.array([-0.5, 0.0, 0.0])
goal2 = target_tool_position + np.array([0.0, -1.0, 0.0])
translation_targets = [goal1, goal2]

translation_goalset = cumotion.TrajectoryOptimizer.TranslationConstraintGoalset.target(
    translation_targets)
goalset = cumotion.TrajectoryOptimizer.TaskSpaceTargetGoalset(translation_goalset)

Here we print the status, and use cumotion::TrajectoryOptimizer::Results::targetIndex() to identify which of the two goals the solver picked. Here we see the solver planned a trajectory to the 2nd goal (index 1 in 0-based indexing).

Solving a Goalset#

Once we have a cumotion::TrajectoryOptimizer::TaskSpaceTargetGoalset object, we can pass it to cumotion::TrajectoryOptimizer::planToTaskSpaceGoalset() and save the results to a new variable:

std::cout << "\nTask-space goalset:\n";
auto goalset_results = optimizer->planToTaskSpaceGoalset(target_cspace_position, goalset);
print_status(*goalset_results);
std::cout << "  Goal index: " << goalset_results->targetIndex() << "\n";

print("\nTask-space goalset:")
goalset_results = optimizer.plan_to_task_space_goalset(target_cspace_position, goalset)
print_status(goalset_results)
print(f"  Goal index: {goalset_results.target_index()}")

Output:

Task-space goalset:
  Trajectory optimization succeeded!
  Trajectory time: 1.01994 seconds

We can query the status and solutions the same way we did before. However, we often want to know which goal / target the trajectory ended up using. cumotion::TrajectoryOptimizer::Results::targetIndex() returns the goal / target index used by the final trajectory:

std::cout << "  Goal index: " << goalset_results->targetIndex() << "\n";

print(f"  Goal index: {goalset_results.target_index()}")

Output:

Goal index: 1

As expected, it picked the goal that’s on the same side of the obstacle, since that’ll be faster than going around the obstacle. Let’s move the obstacle and see if it picks the closer target:

// Move the obstacle and update the world view.
cumotion::Pose3 new_obstacle_pose =
    cumotion::Pose3::FromTranslation(target_tool_position + Eigen::Vector3d(0.0, 0.3, 0.0));
world->setPose(obstacle_handle, new_obstacle_pose);
world_view.update();

// Resolve.
std::cout << "\nTask-space goalset, after moving obstacle:\n";
auto goalset_results_after_move =
    optimizer->planToTaskSpaceGoalset(target_cspace_position, goalset);
print_status(*goalset_results_after_move);
std::cout << "  Goal index: " << goalset_results_after_move->targetIndex() << "\n";

# Move the obstacle and update the world view.
new_obstacle_pose = cumotion.Pose3.from_translation(
    target_tool_position + np.array([0.0, 0.3, 0.0]))
world.set_pose(obstacle_handle, new_obstacle_pose)
world_view.update()

# Re-solve.
goalset_results_after_move = optimizer.plan_to_task_space_goalset(
    target_cspace_position, goalset)
print("\nTask-space goalset, after moving obstacle:")
print_status(goalset_results_after_move)
print(f"  Goal index: {goalset_results_after_move.target_index()}")

Output:

Task-space goalset, after moving obstacle:
  Trajectory optimization succeeded!
  Trajectory time: 0.729065 seconds
  Goal index: 0

We see that, after moving the obstacle, it’s now planning a trajectory to the other target, which is closer to the initial tool position.

Summary of Supported Tool Constraints#

We’ve seen a few different types of tool constraints we can use to control both the final and intermediate tool poses. Here we summarize all the constraints currently supported by the solver.

Tool-Space Targets#

Here are the supported constraints, which are identical for both TaskSpaceTarget s and TaskSpaceTargetGoalset s.

Translation Constraints:

Orientation Constraints:

  • None

  • Constant

  • TerminalTarget

  • TerminalAndPathTarget

  • TerminalAxis

  • TerminalAndPathAxis

  • TerminalTargetAndPathAxis

C-Space Targets#

C-space targets support the following path constraints:

Translation path constraints:

Orientation path constraints: