Analytical kinematics

For some applications it is useful to retrieve more than one solution for an inverse kinematics request, this can be achieved through analytic solvers. For a general 6-DoF robot (industrial arm with 6 revolute joints), up to eight possible solutions can be found. (More, if we include joint positions ± 360 degrees).

The resulting eight solutions have an order. That means that if you call IK on two subsequent frames and compare the 8 configurations of the first frame with the 8 configurations of the second frame at their respective indices, then these configurations are “close” to one another. For this reason, for certain use cases, e.g. for cartesian path planning, it makes sense to keep the order of solutions. This can be achieved by setting the optional parameter keep_order to True. The configurations that are in collision or outside joint boundaries are then not removed from the list of solutions, they are set to None.

We currently support the following robotic arms:

• Offset-Wrist manipulators:

  • UR3, UR3e

  • UR5, UR5e

  • UR10, UR10e

• Spherical-Wrist manipulators:

  • Staubli_TX260L

  • ABB_IRB4600_40_255

Links

• DH parameters for UR robots

• UR kinematics

• Lobster : A tool for solving the inverse-kinematics of 6-axis robot arms

Inverse 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 using AnalyticalInverseKinematics:

from compas.geometry import Frame
from compas_fab.robots import RobotLibrary
from compas_fab.backends import AnalyticalInverseKinematics

ik = AnalyticalInverseKinematics()
robot = RobotLibrary.ur5()

frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))

# The solver must be one of the supported PLANNER_BACKENDS
for joint_positions, joint_names in ik.inverse_kinematics(robot, frame_WCF, options={'solver': 'ur5'}):
    print(joint_positions)

The above solutions, could however be in collision. In order to check for collisions, we have to use a client. See below the example for using the PyBulletClient

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

frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))


with PyBulletClient(connection_type='direct') as client:
    robot = client.load_ur5(load_geometry=True)

    ik = AnalyticalInverseKinematics(client)
    # set a new IK function
    client.inverse_kinematics = ik.inverse_kinematics

    options = {"solver": "ur5", "check_collision": True, "keep_order": True}

    for solution in robot.iter_inverse_kinematics(frame_WCF, options=options):
        print(solution)

Or, alternatively, use the AnalyticalPyBulletClient:

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


with AnalyticalPyBulletClient(connection_type='direct') as client:
    robot = client.load_ur5(load_geometry=True)

    frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))

    options = {"solver": "ur5", "check_collision": True, "keep_order": True}
    # 8 solutions, `None` are those in collision
    for config in robot.iter_inverse_kinematics(frame_WCF, options=options):
        print(config)

We can also use the AnalyticalPyBulletClient to calculate a cartesian path:

import matplotlib.pyplot as plt
from compas.geometry import Frame

from compas_fab.backends import AnalyticalPyBulletClient
from compas_fab.robots import FrameWaypoints

frames_WCF = [
    Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
    Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)),
    Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)),
    Frame((0.388, 0.079, 0.309), (0.881, 0.000, 0.473), (0.149, 0.949, -0.278)),
    Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296)),
]

with AnalyticalPyBulletClient(connection_type="direct") as client:
    robot = client.load_ur5(load_geometry=True)

    options = {"solver": "ur5", "check_collision": True}
    start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1]
    waypoints = FrameWaypoints(frames_WCF)
    trajectory = robot.plan_cartesian_motion(waypoints, start_configuration=start_configuration, options=options)
    assert trajectory.fraction == 1.0

    j = [c.joint_values for c in trajectory.points]
    plt.plot(j)
    plt.show()