Inverse Kinematics 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 an IK config

  3. Sett a parameter in the IK config

  4. Create an IK solver

  5. Solve an inverse kinematics problem with a target pose

  6. Query the solution

  7. Solve an inverse kinematics problem with a goalset

What is Inverse Kinematics?#

Inverse kinematics (IK) is the process of calculating the necessary c-space positions to move a given frame to a desired position and orientation. Unlike forward kinematics, which map a single c-space position to a single frame pose, inverse kinematics has several possible solutions:

  • A discrete set of c-space positions (e.g. for a 6 DOF serial-link manipulator)

  • An infinite number of c-space positions (e.g. for a 7 DOF serial-link manipulator)

  • No valid solutions, due to the target pose being:

    • Outside the reachable workspace of the robot (too far away)

    • Infeasible given the degrees of freedom of the robot (e.g. a 3D gantry robot has a fixed orientation)

    • Infeasible given the limits on the c-space positions

    • Infeasible due to world collisions (e.g. the target is inside an obstacle)

    • Infeasible due to self collisions (e.g. the target is inside the base link of the robot)

At its core, inverse kinematics is a feasibility problem, where the goal is to find a solution that satisfies all the constraints:

  • The pose of the desired frame meets the given criteria, which may include:

    • Position is within a set distance from a target position in the world frame

    • Orientation is within a set angle from a target orientation in the world frame

    • An axis of the desired frame is within a set angle from an axis in the world frame

  • The c-space positions are within the position limits

  • No self-collision

  • No world-collision

There are many approaches to solving these types of problems, which leads to many different IK algorithms. The accelerated algorithm implemented by the cuMotion library prioritizes robustness and runtime performance when there is not a good initial guess for the c-space position.

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 initial position of the tool frame. See the tutorials listed in the Prerequisites for more details on these steps.

std::unique_ptr<cumotion::RobotDescription> robot_description =
    cumotion::LoadRobotFromFile(xrdf_file, urdf_file);
std::unique_ptr<cumotion::Kinematics> kinematics = robot_description->kinematics();
assert(robot_description != nullptr);
assert(kinematics != nullptr);

// Create a world.
std::shared_ptr<cumotion::World> world = cumotion::CreateWorld();

// Add a world view.
cumotion::WorldViewHandle world_view = world->addWorldView();

// Get the tool frame name.
std::string tool_frame_name = robot_description->toolFrameNames().front();
cumotion::Kinematics::FrameHandle tool_frame = kinematics->frame(tool_frame_name);

// Compute the tool frame pose at the default c-space configuration.
Eigen::VectorXd default_cspace = robot_description->defaultCSpaceConfiguration();
cumotion::Pose3 default_tool_pose = kinematics->pose(default_cspace, tool_frame);

// Create a cuboid obstacle near the initial position of the tool frame.
std::unique_ptr<cumotion::Obstacle> cuboid =
    cumotion::CreateObstacle(cumotion::Obstacle::Type::CUBOID);
cuboid->setAttribute(cumotion::Obstacle::Attribute::SIDE_LENGTHS, Eigen::Vector3d(0.2, 0.4, 0.2));

// Position the obstacle slightly offset from the default tool position.
Eigen::Vector3d obstacle_position = default_tool_pose.translation;
obstacle_position.y() += 0.3;  // Offset in the y-direction
cumotion::Pose3 obstacle_pose = cumotion::Pose3::FromTranslation(obstacle_position);

// Add the obstacle to the world.
cumotion::World::ObstacleHandle obstacle_handle = world->addObstacle(*cuboid, obstacle_pose);

robot_description: cumotion.RobotDescription = cumotion.load_robot_from_file(xrdf_path, urdf_path)
kinematics: cumotion.Kinematics = robot_description.kinematics()

# Create a world.
world = cumotion.create_world()

# Add a world view.
world_view = world.add_world_view()

# Get the tool frame name.
tool_frame_name = robot_description.tool_frame_names()[0]

# Compute the tool frame pose at the default c-space configuration.
default_cspace = robot_description.default_cspace_configuration()
default_tool_pose = kinematics.pose(default_cspace, tool_frame_name)

# Create a cuboid obstacle near the initial position of the tool frame.
cuboid = cumotion.create_obstacle(cumotion.Obstacle.Type.CUBOID)
cuboid.set_attribute(cumotion.Obstacle.Attribute.SIDE_LENGTHS, np.array([0.2, 0.4, 0.2]))

# Position the obstacle slightly offset from the default tool position.
obstacle_position = default_tool_pose.translation.copy()
obstacle_position[1] += 0.3  # Offset in the y-direction
obstacle_pose = cumotion.Pose3.from_translation(obstacle_position)

# Add the obstacle to the world.
obstacle_handle = world.add_obstacle(cuboid, obstacle_pose)

Creating an IK Config#

After creating the robot and world, we can create a cumotion::CollisionFreeIkSolverConfig object, which is used to specify the parameters for the IK solver. Here we need to specify the robot description, the tool frame, and provide a world view that will be used to avoid collisions with the environment (which is currently empty since we haven’t updated it after adding the obstacle).

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

config = cumotion.create_default_collision_free_ik_solver_config(
    robot_description, tool_frame_name, world_view
)

Setting an IK Parameter#

The IK solver has many parameters that can be set with cumotion::CollisionFreeIkSolverConfig::setParam(), whose documentation describes the list of parameters. Here we set the number of seeds to be higher than the default, which increases computational cost, but increases the likelihood of finding a variety of different solutions:

bool did_set = config->setParam("num_seeds", 50);
assert(did_set);

did_set = config.set_param("num_seeds", 50)
assert did_set

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 an IK solver parameter. This is likely to change in future versions.

Creating the IK Solver#

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

std::unique_ptr<cumotion::CollisionFreeIkSolver> ik_solver =
    cumotion::CreateCollisionFreeIkSolver(*config);

ik_solver = cumotion.create_collision_free_ik_solver(config)

Instantiating a new IK solver requires allocating both host and device memory, and should be considered an expensive operation.

Once the solver is instantiated, the parameters cannot be changed. If any parameter needs to be modified, a new solver will need to be instantiated.

Solving for a Target Pose#

Solving an IK problem involves two steps:

  1. Defining the task-space constraint

  2. Calling cumotion::CollisionFreeIkSolver::solve() or

    cumotion::CollisionFreeIkSolver::solveGoalset()

Here we start by defining a complete target pose constraint for the tool frame, which must be satisfied exactly:

// Create a pose target by offsetting the default tool pose.
Eigen::Vector3d target_translation = default_tool_pose.translation;
target_translation.x() += 0.1;  // Move 10cm in the x-direction
cumotion::Rotation3 target_rotation = default_tool_pose.rotation;

// Create the translation and orientation constraints.
cumotion::CollisionFreeIkSolver::TranslationConstraint translation_constraint =
    cumotion::CollisionFreeIkSolver::TranslationConstraint::Target(target_translation);
cumotion::CollisionFreeIkSolver::OrientationConstraint orientation_constraint =
    cumotion::CollisionFreeIkSolver::OrientationConstraint::Target(target_rotation);

// Create the task space target.
cumotion::CollisionFreeIkSolver::TaskSpaceTarget task_space_target(translation_constraint,
                                                                    orientation_constraint);

# Create a pose target by offsetting the default tool pose.
target_translation = default_tool_pose.translation.copy()
target_translation[0] += 0.1  # Move 10cm in the x-direction
target_rotation = default_tool_pose.rotation

# Create the translation and orientation constraints.
translation_constraint = cumotion.CollisionFreeIkSolver.TranslationConstraint.target(
    target_translation
)
orientation_constraint = cumotion.CollisionFreeIkSolver.OrientationConstraint.target(
    target_rotation
)

# Create the task space target.
task_space_target = cumotion.CollisionFreeIkSolver.TaskSpaceTarget(
    translation_constraint, orientation_constraint
)

There are several types of task-space constraints supported by the solver. See cumotion::CollisionFreeIkSolver::TranslationConstraint and cumotion::CollisionFreeIkSolver::OrientationConstraint for all the options.

Once we have created a cumotion::CollisionFreeIkSolver::TaskSpaceTarget object, we can pass it to cumotion::CollisionFreeIkSolver::solve():

std::unique_ptr<cumotion::CollisionFreeIkSolver::Results> results =
    ik_solver->solve(task_space_target);

results = ik_solver.solve(task_space_target)

The output of solve is (a pointer to) a cumotion::CollisionFreeIkSolver::Results object, which we use to query the solution to the IK problem.

Querying the Results#

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

cumotion::CollisionFreeIkSolver::Results::Status status = results->status();
if (status == cumotion::CollisionFreeIkSolver::Results::Status::SUCCESS) {
std::cout << cumotion::text_style::BRIGHT_GREEN << "IK solve succeeded!\n";
} else {
std::cout << cumotion::text_style::BRIGHT_RED << "IK solve failed.\n";
}
std::cout << cumotion::text_style::RESET;

status = results.status()
if status == cumotion.CollisionFreeIkSolver.Results.Status.SUCCESS:
    print("IK solve succeeded!")
else:
    print("IK solve failed.")

Output:

IK solve succeeded!

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.

All feasible c-space positions found by the solver can be retrieved using cumotion::CollisionFreeIkSolver::Results::cSpacePositions():

std::vector<Eigen::VectorXd> cspace_positions = results->cSpacePositions();
std::cout << "Number of solutions found: " << cspace_positions.size() << "\n";

cspace_positions = results.cspace_positions()
print(f"Number of solutions found: {len(cspace_positions)}")

Output:

Number of solutions found: 43

The solutions are sorted from lowest-to-highest cost, so the “best” solutions are at the beginning of the list. To avoid returning solutions with different c-space positions that result in identical physical configurations of the robot (which is only possible with revolute joints that have a range greater than \(2 \pi\)), the solutions are “wrapped” to the c-space position that is closest to the default configuration specified by the robot description, and only unique solutions are returned.

Updating the World#

Note that our solver currently thinks there are no obstacles in the world, even though we added one. Here we show how to update the world, call solve again, and see how some of the previous solutions are no longer returned since they would be in collision with the obstacle.

// Update the world view to include the obstacle we added.
world_view.update();

// Solve again with the updated world view.
std::unique_ptr<cumotion::CollisionFreeIkSolver::Results> results_with_obstacle =
    ik_solver->solve(task_space_target);

// Check the status after updating the world.
cumotion::CollisionFreeIkSolver::Results::Status status_with_obstacle =
    results_with_obstacle->status();
if (status_with_obstacle == cumotion::CollisionFreeIkSolver::Results::Status::SUCCESS) {
    std::cout << cumotion::text_style::BRIGHT_GREEN << "\nIK solve with obstacle succeeded!\n";
} else {
    std::cout << cumotion::text_style::BRIGHT_RED << "\nIK solve with obstacle failed.\n";
}
std::cout << cumotion::text_style::RESET;

// Get the c-space positions with obstacle.
std::vector<Eigen::VectorXd> cspace_positions_with_obstacle =
    results_with_obstacle->cSpacePositions();
std::cout << "Number of solutions found with obstacle: " << cspace_positions_with_obstacle.size()
          << "\n";

# Update the world view to include the obstacle we added.
world_view.update()

# Solve again with the updated world view.
results_with_obstacle = ik_solver.solve(task_space_target)

# Check the status after updating the world.
status_with_obstacle = results_with_obstacle.status()
if status_with_obstacle == cumotion.CollisionFreeIkSolver.Results.Status.SUCCESS:
    print("\nIK solve with obstacle succeeded!")
else:
    print("\nIK solve with obstacle failed.")

# Get the c-space positions with obstacle.
cspace_positions_with_obstacle = results_with_obstacle.cspace_positions()
print(f"Number of solutions found with obstacle: {len(cspace_positions_with_obstacle)}")

Output:

IK solve with obstacle succeeded!
Number of solutions found with obstacle: 23

Solving for a Target 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 IK for a goalset

  3. How to identify which goal / target in the goalset a solution “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)

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:

// Create a goalset with translation-only constraints on either side of the obstacle.
std::vector<Eigen::Vector3d> translation_targets;

// Target on the left side of the obstacle.
Eigen::Vector3d left_target = default_tool_pose.translation;
left_target.x() += 0.3;
left_target.y() -= 0.1;
translation_targets.push_back(left_target);

// Target on the right side of the obstacle.
Eigen::Vector3d right_target = default_tool_pose.translation;
right_target.x() += 0.3;
right_target.y() += 0.1;
translation_targets.push_back(right_target);

// Create translation constraint goalset (position-only, no orientation constraint).
cumotion::CollisionFreeIkSolver::TranslationConstraintGoalset translation_goalset =
    cumotion::CollisionFreeIkSolver::TranslationConstraintGoalset::Target(translation_targets);

// Create an empty orientation constraint goalset.
cumotion::CollisionFreeIkSolver::OrientationConstraintGoalset orientation_goalset =
    cumotion::CollisionFreeIkSolver::OrientationConstraintGoalset::None();

// Create the task space target goalset.
cumotion::CollisionFreeIkSolver::TaskSpaceTargetGoalset task_space_target_goalset(
    translation_goalset, orientation_goalset);

# Create a goalset with translation-only constraints on either side of the obstacle.
translation_targets = []

# Target on the left side of the obstacle.
left_target = default_tool_pose.translation.copy()
left_target[0] += 0.3
left_target[1] -= 0.1
translation_targets.append(left_target)

# Target on the right side of the obstacle.
right_target = default_tool_pose.translation.copy()
right_target[0] += 0.3
right_target[1] += 0.1
translation_targets.append(right_target)

# Create translation constraint goalset (position-only, no orientation constraint).
translation_goalset = cumotion.CollisionFreeIkSolver.TranslationConstraintGoalset.target(
    translation_targets
)

# Create an empty orientation constraint goalset.
orientation_goalset = cumotion.CollisionFreeIkSolver.OrientationConstraintGoalset.none()

# Create the task space target goalset.
task_space_target_goalset = cumotion.CollisionFreeIkSolver.TaskSpaceTargetGoalset(
    translation_goalset, orientation_goalset
)

Solving a Goalset#

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

std::unique_ptr<cumotion::CollisionFreeIkSolver::Results> goalset_results =
    ik_solver->solveGoalset(task_space_target_goalset);

goalset_results = ik_solver.solve_goalset(task_space_target_goalset)

Output:

Goalset IK solve succeeded!
Number of valid solutions: 49

We can query the status and solutions the same way we did before. However, we often want to know which goal / target each IK solution corresponds to. We can use cumotion::CollisionFreeIkSolver::Results::targetIndices() to get a list of indices. This list has the same length as the output of cumotion::CollisionFreeIkSolver::Results::cSpacePositions(), and provides the goal index of each solution, which in our example will be either 0 or 1 since we have two goals:

// Load the robot.
cumotion::CollisionFreeIkSolver::Results::Status goalset_status = goalset_results->status();
if (goalset_status == cumotion::CollisionFreeIkSolver::Results::Status::SUCCESS) {
    std::cout << cumotion::text_style::BRIGHT_GREEN << "\nGoalset IK solve succeeded!\n";
} else {
    std::cout << cumotion::text_style::BRIGHT_RED << "\nGoalset IK solve failed.\n";
}
std::cout << cumotion::text_style::RESET;

// Get the c-space positions and target indices from the goalset results.
std::vector<Eigen::VectorXd> goalset_cspace_positions = goalset_results->cSpacePositions();
std::cout << "Number of valid solutions: " << goalset_cspace_positions.size() << "\n";

// Print the goal assignment for each solution and verify using forward kinematics.
std::vector<int> target_indices = goalset_results->targetIndices();

// Print table header
std::cout << "\n"
            << std::left << std::setw(18) << "Solution Index" << std::setw(18) << "Target Index"
            << "Position Error (um)\n";
std::cout << std::string(54, '-') << "\n";

for (size_t i = 0; i < goalset_cspace_positions.size(); ++i) {
    int target_idx = target_indices[i];

    // Compute the forward kinematics for this solution.
    cumotion::Pose3 solution_pose = kinematics->pose(goalset_cspace_positions[i], tool_frame);

    // Compute the distance to the indicated target.
    double distance_to_target =
        (solution_pose.translation - translation_targets[target_idx]).norm();

    // Convert to micrometers and print row
    double error_um = distance_to_target * 1e6;
    std::cout << std::left << std::setw(18) << i << std::setw(18) << target_idx << std::fixed
            << std::setprecision(2) << error_um << "\n";
}

# Load the robot.
goalset_status = goalset_results.status()
if goalset_status == cumotion.CollisionFreeIkSolver.Results.Status.SUCCESS:
    print("\nGoalset IK solve succeeded!")
else:
    print("\nGoalset IK solve failed.")

# Get the c-space positions and target indices from the goalset results.
goalset_cspace_positions = goalset_results.cspace_positions()
print(f"Number of valid solutions: {len(goalset_cspace_positions)}")

# Print the goal assignment for each solution and verify using forward kinematics.
target_indices = goalset_results.target_indices()

print(f"\n{'Solution Index':<18}{'Target Index':<18}Position Error (um)")
print("-" * 54)
for i in range(len(goalset_cspace_positions)):
    target_idx = target_indices[i]

    # Compute the forward kinematics for this solution.
    solution_pose = kinematics.pose(goalset_cspace_positions[i], tool_frame_name)

    # Compute the distance to the indicated target.
    distance_to_target = np.linalg.norm(solution_pose.translation - translation_targets[target_idx])

    # Convert to micrometers and print row
    error_um = distance_to_target * 1e6
    print(f"{i:<18}{target_idx:<18}{error_um:.2f}")

Output:

Solution Index    Target Index      Position Error (um)
------------------------------------------------------
0                 1                 0.47
1                 1                 0.39
2                 0                 0.20
3                 0                 0.34
4                 1                 0.32
5                 1                 0.71
6                 0                 0.24
7                 1                 0.44
8                 0                 0.24
9                 1                 0.41
10                0                 0.24
11                0                 0.25
12                0                 0.11
13                1                 0.41
14                1                 0.16
15                1                 0.51
16                1                 0.25
17                0                 0.27
18                0                 0.32
19                1                 0.27
20                1                 0.18
21                0                 0.19
22                0                 0.12
23                0                 0.25
24                0                 0.17
25                0                 0.26
26                0                 0.35
27                1                 0.29
28                1                 0.28
29                0                 0.31
30                1                 0.36
31                0                 0.47
32                1                 0.34
33                1                 0.26
34                0                 0.20
35                0                 0.15
36                0                 0.32
37                1                 0.35
38                0                 0.27
39                0                 0.16
40                1                 0.34
41                0                 0.29
42                0                 0.12
43                1                 0.42
44                0                 0.19
45                1                 0.19
46                0                 0.24
47                0                 0.15
48                0                 0.30