Forward and inverse kinematics

Forward kinematics

The forward kinematics function calculates the pose of the robot’s end-effector from joint states. This means the state of each joint in the articulated body of a robot needs to be defined.

Joint states are described in COMPAS FAB with the compas_fab.robots.Configuration class.

Below we demonstrate calculating the forward kinematics for a UR5 robot. (Note: Since the PyBullet server has the capacity to load multiple robots, which robot the forward kinematics are being calculated for must be specified.)

import compas_fab
from compas_robots import Configuration
from compas_fab.backends import PyBulletClient

with PyBulletClient() as client:
    urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
    robot = client.load_robot(urdf_filename)

    configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0])

    frame_WCF = robot.forward_kinematics(configuration)

    print("Frame in the world coordinate system")
    print(frame_WCF)

Inverse kinematics

Inverse kinematics is the inverse function of forward kinematics. The inverse kinematics function calculates the joint states required for the end-effector to reach a certain target pose.

Here is an example of such a calculation using PyBullet:

import compas_fab
from compas.geometry import Frame
from compas_fab.backends import PyBulletClient

with PyBulletClient() as client:
    urdf_filename = compas_fab.get('robot_library/ur5_robot/urdf/robot_description.urdf')
    robot = client.load_robot(urdf_filename)

    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
    start_configuration = robot.zero_configuration()

    configuration = robot.inverse_kinematics(frame_WCF, start_configuration)

    print("Found configuration", configuration)

It is also possible to request multiple inverse kinematic solutions:

import compas_fab
from compas.geometry import Frame
from compas_fab.backends import PyBulletClient

with PyBulletClient(connection_type='direct') as client:
    urdf_filename = compas_fab.get('robot_library/ur5_robot/urdf/robot_description.urdf')
    robot = client.load_robot(urdf_filename)

    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
    start_configuration = robot.zero_configuration()

    options = dict(max_results=20, high_accuracy_threshold=1e-6, high_accuracy_max_iter=20)
    for config in robot.iter_inverse_kinematics(frame_WCF, start_configuration, options=options):
        print("Found configuration", config)