Kinematics Tutorial#

Prerequisites#

The following tutorials are recommended prior to starting this tutorial:

Robot Description Tutorial

Objectives#

Through this tutorial, you will learn how to:

Check for frames defined in the kinematics,
Query the position and orientation of a frame given the c-space coordinates (i.e. forward kinematics)

What are Kinematics?#

Robot Kinematics#

Here’s a useful definition:

The branch of mechanics concerned with the motion of objects without reference to the forces which cause the motion.

For a robot, this is the mathematical relationship between the c-space positions (which correspond to the joint angles for a traditional serial-link manipulator with revolute joints) and the position and orientation (pose) of each link of the robot. Typically we care about the pose of a frame attached to the last link, or the “tool” of the robot. We refer to this frame as the tool frame.

Forward kinematics refers to the well-defined mapping from c-space coordinates to the tool frame pose. For a given set of c-space positions, there is always a single tool frame pose.

Inverse kinematics is the mapping from tool frame pose to the c-space positions. This is a much harder problem, since there are generally many (often infinite) c-space positions that may generate the same tool frame pose. Alternatively, there may not be any valid c-space positions that produce a given tool frame pose, such as when:

The pose is outside the workspace of the robot, due to being too far away
The pose cannot be reached due to joint limits
The robot does not have enough degrees of freedom (e.g. a 3D gantry robot has a fixed orientation, and therefore cannot achieve an arbitrary tool frame orientation)
The tool frame would result in self-collision (e.g. the desired tool frame is in the middle of the base of the robot).

This tutorial is focused on the utilities provided by cuMotion for forward kinematics. See Inverse Kinematics Tutorial to learn more about using cuMotion for solving the inverse kinematics.

Kinematics in cuMotion#

For computational and representational efficiency, cuMotion assumes the kinematics of a robot can be represented by a directional tree graph structure. This means:

There is a single “base” link
Each link has a single parent
A parent can have any number of children

C-space Coordinates#

Configuration space (or c-space) is the mathematical space defined by the span of the minimal set of coordinates required to describe the forward kinematics of the robot. They are a generalization of “joint positions,” since the motion of physical joints may not, in general, be completely independent. This may be a result of physical linkages, or a choice by the manufacturer that is implemented in software (such as when both sides of a gripper are controlled using a single position command, even when they are in fact driven by independent motors).

We start by loading a robot description and using it to create the kinematics for the robot. See Robot Description Tutorial for more details.

C++

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

PYTHON

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

These functions create a new instance of the robot kinematics each time it is called. It is highly recommended that the user call this function once and save the result.

C-space Coordinate Names#

Each c-space coordinate is given a descriptive name, which is useful for debugging and analysis. Just like the robot description, the name for a given coordinate can be queried from the kinematics:

C++

const int num_coords = kinematics->numCSpaceCoords();
std::vector<std::string> coord_names(num_coords);
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  coord_names[coord_index] = robot_description->cSpaceCoordName(coord_index);
}
std::cout << "Number of c-space coordinates: " << num_coords << "\n";
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  std::cout << "Coordinate " << coord_index << " name: " << coord_names[coord_index] << "\n";
}

PYTHON

num_coords = kinematics.num_cspace_coords()
print("Number of c-space coordinates:", num_coords)
for coord_index in range(num_coords):
    coord_name = kinematics.cspace_coord_name(coord_index)
    print("Coordinate {} name: {}".format(coord_index, coord_name))

Output:

Number of c-space coordinates: 7
Coordinate 0 name: panda_joint1
Coordinate 1 name: panda_joint2
Coordinate 2 name: panda_joint3
Coordinate 3 name: panda_joint4
Coordinate 4 name: panda_joint5
Coordinate 5 name: panda_joint6
Coordinate 6 name: panda_joint7

C-space Limits#

Once we have the robot kinematics, we can query the limits on the c-space positions, velocities, accelerations, and jerks:

C++

Eigen::VectorXd position_lower_limits(num_coords);
Eigen::VectorXd position_upper_limits(num_coords);
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  auto [lower, upper] = kinematics->cSpaceCoordLimits(coord_index);
  position_lower_limits[coord_index] = lower;
  position_upper_limits[coord_index] = upper;
}

std::cout << "\nC-space limits:\n";
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  std::cout << "Coordinate " << coord_index << " limits: [" << position_lower_limits[coord_index]
            << ", " << position_upper_limits[coord_index] << "]\n";
}

print("\nC-space limits:")
for coord_index in range(num_coords):
    limits = kinematics.cspace_coord_limits(coord_index)
    print("Coordinate {} limits: [{}, {}]".format(coord_index, limits.lower, limits.upper))

Output:

C-space limits:
Coordinate 0 limits: [-2.8973, 2.8973]
Coordinate 1 limits: [-1.7628, 1.7628]
Coordinate 2 limits: [-2.8973, 2.8973]
Coordinate 3 limits: [-3.0718, -0.0698]
Coordinate 4 limits: [-2.8973, 2.8973]
Coordinate 5 limits: [-0.0175, 3.7525]
Coordinate 6 limits: [-2.8973, 2.8973]

It’s often useful to know whether a given c-space position is within the limits, which can be done with:

C++

Eigen::VectorXd test_position = position_upper_limits * 1.1;
bool within_limits = kinematics->withinCSpaceLimits(test_position, true);
std::cout << "\nTest position within limits: " << (within_limits ? "true" : "false") << "\n";

PYTHON

test_position = np.zeros(num_coords)
within_limits = kinematics.within_cspace_limits(test_position, True)
print("\033[0mTest position within limits:", within_limits)

which will produce output like the following if log_warnings is true and the c-space position violates any limits:

C-space position [3.18703, 1.93908, 3.18703, -0.07678, 3.18703, 4.12775, 3.18703] exceeds limits:
  - Coordinate "panda_joint1" [index: 0, value: 3.18703] exceeds limits: (-2.8973, 2.8973).
  - Coordinate "panda_joint2" [index: 1, value: 1.93908] exceeds limits: (-1.7628, 1.7628).
  - Coordinate "panda_joint3" [index: 2, value: 3.18703] exceeds limits: (-2.8973, 2.8973).
  - Coordinate "panda_joint5" [index: 4, value: 3.18703] exceeds limits: (-2.8973, 2.8973).
  - Coordinate "panda_joint6" [index: 5, value: 4.12775] exceeds limits: (-0.0175, 3.7525).
  - Coordinate "panda_joint7" [index: 6, value: 3.18703] exceeds limits: (-2.8973, 2.8973).

Velocity, Acceleration, and Jerk Limits#

In addition to position limits, we can also query the velocity, acceleration, and jerk limits for each c-space coordinate:

C++

Eigen::VectorXd velocity_limits(num_coords);
Eigen::VectorXd acceleration_limits(num_coords);
Eigen::VectorXd jerk_limits(num_coords);
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  velocity_limits[coord_index] = kinematics->cSpaceCoordVelocityLimit(coord_index);
  acceleration_limits[coord_index] = kinematics->cSpaceCoordAccelerationLimit(coord_index);
  jerk_limits[coord_index] = kinematics->cSpaceCoordJerkLimit(coord_index);
}

std::cout << "\nVelocity limits:\n";
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  std::cout << "Coordinate " << coord_index << ": " << velocity_limits[coord_index] << "\n";
}

std::cout << "\nAcceleration limits:\n";
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  std::cout << "Coordinate " << coord_index << ": " << acceleration_limits[coord_index] << "\n";
}

std::cout << "\nJerk limits:\n";
for (int coord_index = 0; coord_index < num_coords; ++coord_index) {
  std::cout << "Coordinate " << coord_index << ": " << jerk_limits[coord_index] << "\n";
}

PYTHON

print("\nVelocity limits:")
for coord_index in range(num_coords):
    vel_limit = kinematics.cspace_coord_velocity_limit(coord_index)
    print("Coordinate {} velocity limit: {}".format(coord_index, vel_limit))

print("\nAcceleration limits:")
for coord_index in range(num_coords):
    acc_limit = kinematics.cspace_coord_acceleration_limit(coord_index)
    print("Coordinate {} acceleration limit: {}".format(coord_index, acc_limit))

print("\nJerk limits:")
for coord_index in range(num_coords):
    jerk_limit = kinematics.cspace_coord_jerk_limit(coord_index)
    print("Coordinate {} jerk limit: {}".format(coord_index, jerk_limit))

Output:

Velocity limits:
Coordinate 0 velocity limit: 2.175
Coordinate 1 velocity limit: 2.175
Coordinate 2 velocity limit: 2.175
Coordinate 3 velocity limit: 2.175
Coordinate 4 velocity limit: 2.61
Coordinate 5 velocity limit: 2.61
Coordinate 6 velocity limit: 2.61

Acceleration limits:
Coordinate 0 acceleration limit: 15.0
Coordinate 1 acceleration limit: 7.5
Coordinate 2 acceleration limit: 10.0
Coordinate 3 acceleration limit: 12.5
Coordinate 4 acceleration limit: 15.0
Coordinate 5 acceleration limit: 20.0
Coordinate 6 acceleration limit: 20.0

Jerk limits:
Coordinate 0 jerk limit: 7500.0
Coordinate 1 jerk limit: 3750.0
Coordinate 2 jerk limit: 5000.0
Coordinate 3 jerk limit: 6250.0
Coordinate 4 jerk limit: 7500.0
Coordinate 5 jerk limit: 10000.0
Coordinate 6 jerk limit: 10000.0

Coordinate Frames#

The kinematics define multiple coordinate frames, which are all rigidly attached to a particular link of the robot. The kinematics (both forward and inverse) are defined between two frames, which are typically the base frame (a frame rigidly attached to the environment) and the tool frame.

To get a list of all the frame names in the robot, use Kinematics::frameNames():

C++

std::vector<std::string> frame_names = kinematics->frameNames();
std::cout << "\nFrame names:\n";
for (const auto &frame_name : frame_names) {
  std::cout << "  " << frame_name << "\n";
}

PYTHON

frame_names = kinematics.frame_names()
print("\nFrame names:")
for frame_name in frame_names:
    print("  {}".format(frame_name))

Output:

Frame names:
  base_link
  panda_link0
  panda_link1
  panda_link2
  panda_link3
  panda_link4
  panda_forearm_end_pt
  panda_forearm_mid_pt
  panda_link5
  panda_forearm_distal
  panda_forearm_mid_pt_shifted
  panda_link6
  panda_link7
  panda_link8
  panda_hand
  camera_bottom_screw_frame
  camera_link
  camera_depth_frame
  camera_color_frame
  camera_color_optical_frame
  camera_depth_optical_frame
  camera_left_ir_frame
  camera_left_ir_optical_frame
  camera_right_ir_frame
  camera_right_ir_optical_frame
  panda_face_back_left
  panda_face_back_right
  panda_face_left
  panda_face_right
  panda_leftfinger
  panda_leftfingertip
  panda_rightfinger
  panda_rightfingertip
  right_gripper
  panda_wrist_end_pt

The base frame (the root of the kinematic tree structure), is given by Kinematics::baseFrame():

C++

std::string base_frame_name = kinematics->frameName(kinematics->baseFrame());
std::cout << "\nBase frame: " << base_frame_name << "\n";

PYTHON

base_frame_name = kinematics.base_frame_name()
print("\nBase frame: {}".format(base_frame_name))

Output:

Base frame: base_link

Note

What is a FrameHandle? In the C++ API, most kinematics functions require a FrameHandle, instead of a std::string. To avoid the overhead of string processing and lookup, the user is expected to create the FrameHandle for the frame of interest once, and then re-use it.

Do NOT attempt to use a FrameHandle created from a difference instance of the kinematics. This will result in silent errors. For this reason, it is strongly recommended that the user create a single instance of the kinematics (which can be moved to a std::shared_ptr, if needed) and create a FrameHandle only when it can be guaranteed that the handle will always be used with the kinematics from which it was created.

Forward Kinematics#

Once we know the name of the frame we want to work with, we can use Kinematics::position(), Kinematics::orientation(), and Kinematics::pose() to compute the forward kinematics of the given frame, with respect to the baseFrame().

C++

Eigen::VectorXd cspace_position = robot_description->defaultCSpaceConfiguration();
std::string tool_frame_name = robot_description->toolFrameNames().front();
cumotion::Kinematics::FrameHandle tool_frame = kinematics->frame(tool_frame_name);
cumotion::Kinematics::FrameHandle base_frame = kinematics->baseFrame();

cumotion::Pose3 tool_pose = kinematics->pose(cspace_position, tool_frame);
cumotion::Rotation3 tool_orientation = kinematics->orientation(cspace_position, tool_frame);
Eigen::Vector3d tool_position = kinematics->position(cspace_position, tool_frame);

std::cout << "\nTool frame pose (position): [" << tool_pose.translation.transpose() << "]\n";
std::cout << "Tool frame pose (orientation quaternion): [" << tool_pose.rotation.w() << ", "
          << tool_pose.rotation.x() << ", " << tool_pose.rotation.y() << ", "
          << tool_pose.rotation.z() << "]\n";

PYTHON

cspace_position = robot_description.default_cspace_configuration()
tool_frame_name = robot_description.tool_frame_names()[0]

tool_pose: cumotion.Pose3 = kinematics.pose(cspace_position, tool_frame_name)
tool_position = kinematics.position(cspace_position, tool_frame_name)
tool_orientation: cumotion.Rotation3 = kinematics.orientation(cspace_position, tool_frame_name)

print("\nTool frame pose (position): {}".format(tool_pose.translation))
print("Tool frame pose (orientation quaternion): [{}, {}, {}, {}]".format(
    tool_pose.rotation.w, tool_pose.rotation.x,
    tool_pose.rotation.y, tool_pose.rotation.z))

Output:

Tool frame pose (position): [  0.270021 -0.0249843   0.425636]
Tool frame pose (orientation quaternion): [0.213314, -0.0172907, 0.976823, -0.00377586]

Alternatively, if we want to compute the forward kinematics with respect to a different frame, we can pass in the reference frame as the last argument:

C++

cumotion::Kinematics::FrameHandle link1_frame = kinematics->frame(frame_names[2]);
tool_pose = kinematics->pose(cspace_position, tool_frame, link1_frame);
tool_orientation = kinematics->orientation(cspace_position, tool_frame, link1_frame);
tool_position = kinematics->position(cspace_position, tool_frame, link1_frame);

std::cout << "\nTool frame pose (position): [" << tool_pose.translation.transpose() << "]\n";
std::cout << "Tool frame pose (orientation quaternion): [" << tool_pose.rotation.w() << ", "
          << tool_pose.rotation.x() << ", " << tool_pose.rotation.y() << ", "
          << tool_pose.rotation.z() << "]\n";

PYTHON

link1_name = kinematics.frame_names()[2]
tool_pose = kinematics.pose(cspace_position, tool_frame_name, link1_name)
tool_position = kinematics.position(cspace_position, tool_frame_name, link1_name)
tool_orientation = kinematics.orientation(cspace_position, tool_frame_name, link1_name)

print("\nTool position: {}".format(tool_position))
print("Tool orientation (quaternion): [{}, {}, {}, {}]".format(
    tool_orientation.w, tool_orientation.x,
    tool_orientation.y, tool_orientation.z))

Output:

Tool frame pose (position): [  0.270021 -0.0249843   0.092636]
Tool frame pose (orientation quaternion): [0.213314, -0.0172907, 0.976823, -0.00377586]

Kinematic Jacobians#

The kinematic Jacobian is the derivative of a frame’s position and orientation, with respect to the c-space positions. In other words, it tells us how much the frame will move in each direction when we move a given joint. There are many use-cases for these Jacobians; however, here we will simply demonstrate how to compute them.

The position Jacobian returns a 3 x N Eigen matrix, where N is the number of c-space coordinates (cumotion::Kinematics::numCSpaceCoords()), where column i of the Jacobian tells us how much the frame will translate in the world frame when we change coordinate i:

C++

Eigen::Matrix<double, 3, Eigen::Dynamic> position_jacobian =
    kinematics->positionJacobian(cspace_position, tool_frame);
std::cout << "Position Jacobian shape: " << position_jacobian.rows() << " x "
          << position_jacobian.cols() << "\n";

PYTHON

position_jacobian = kinematics.position_jacobian(cspace_position, tool_frame_name)
print("Position Jacobian shape: {} x {}".format(position_jacobian.shape[0],
                                                position_jacobian.shape[1]))

Output:

Position Jacobian shape: 3 x 7

The orientation Jacobian also returns a 3 x N Eigen matrix Here column i of the Jacobian tells us how much the frame will rotate about the given axis in the world frame when we change coordinate i:

C++

Eigen::Matrix<double, 3, Eigen::Dynamic> orientation_jacobian =
    kinematics->orientationJacobian(cspace_position, tool_frame);
std::cout << "Orientation Jacobian shape: " << orientation_jacobian.rows() << " x "
          << orientation_jacobian.cols() << "\n";

PYTHON

orientation_jacobian = kinematics.orientation_jacobian(cspace_position, tool_frame_name)
print("Orientation Jacobian shape: {} x {}".format(orientation_jacobian.shape[0],
                                                   orientation_jacobian.shape[1]))

Output:

Orientation Jacobian shape: 3 x 7

To compute both at the same time, use cumotion::Kinematics::jacobian(), which stacks the position Jacobian on top of the orientation Jacobian:

C++

Eigen::Matrix<double, 6, Eigen::Dynamic> full_jacobian =
    kinematics->jacobian(cspace_position, tool_frame);
std::cout << "\nFull Jacobian shape: " << full_jacobian.rows() << " x " << full_jacobian.cols()
          << "\n";

PYTHON

full_jacobian = kinematics.jacobian(cspace_position, tool_frame_name)
print("\nFull Jacobian shape: {} x {}".format(full_jacobian.shape[0], full_jacobian.shape[1]))

Output:

Full Jacobian shape: 6 x 7