Tesseract Planning Framework Acceleration

The Tesseract Robot Planning Framework provides high performance C++ libraries for planning scenes, collision detection, kinematics, and planning. The general robotics toolbox can use the Python wrapper package to accelerate kinematics using the Tesseract C++ implementations of these functions.

Tesseract requires Windows >=10 or Ubuntu >=20.04 and Python >=3.7.

The tesseract option should be installed with the package.

pip install general_robotics_toolbox[tesseract]

general_robotics_toolbox.tesseract

Tesseract Robotics integration and support functions for the General Robotics Toolbox

Tesseract Robotics is a high performance robot planning framework. This module allows the high performance kinematics functions to be used in place of the slow Python functions available in the rest of this module.

Most users will only need the TesseractRobotics class, which is initialized using a Robot structure. These can either be entered in Python, or use the yaml loaders an the general_robotics_toolbox.robotraconteur module. The TesseractRobotics class has the functions fwdkin(), jacobian(), invkin(), and redundant_solutions() available.

A simple example of using the TesseractRobotics class:

# Import reqired modules
import numpy as np
import general_robotics_toolbox as rox
from general_robotics_toolbox import tesseract as rox_tesseract
from general_robotics_toolbox import robotraconteur as rr_rox

with open("abb_1200_5_90_robot_default_config.yml", "r") as f:
    robot = rr_rox.load_robot_info_yaml_to_robot(f)

tesseract_robot = rox_tesseract.TesseractRobot(robot, "robot", invkin_solver="OPWInvKin")

# Random joint angles
q = np.array([0.1, 0.11, 0.12, 0.13, 0.14, 0.15])

# Compute forward kinematics
T_tip_link = tesseract_robot.fwdkin(q)

# Compute robot Jacobian
J = tesseract_robot.jacobian(q)

# Solve inverse kinematics for T_des pose
T_des = rox.Transform(rox.rot([0,1,0], np.deg2rad(80)), np.array([0.5,0.1,0.71]))
invkin1 = tesseract_robot.invkin(T_des,q*0.7)

# Find redundant solutions for first candidate joint angle
invkin1_redun = []
for invkin1_i in invkin1:
    invkin1_redun.extend(tesseract_robot.redundant_solutions(invkin1_i))

class general_robotics_toolbox.tesseract.OPWInvKinParameters(a1: float, a2: float, b: float, c1: float, c2: float, c3: float, c4: float, offsets: array, sign_corrections: array)

OPW inverse kinematics solver parameters. See https://github.com/Jmeyer1292/opw_kinematics and robot_to_opw_inv_kin_parameters()

a1: float: Alias for field number 0

a2: float: Alias for field number 1

b: float: Alias for field number 2

c1: float: Alias for field number 3

c2: float: Alias for field number 4

c3: float: Alias for field number 5

c4: float: Alias for field number 6

offsets: array: Alias for field number 7

sign_corrections: array: Alias for field number 8

class general_robotics_toolbox.tesseract.TesseractRobot(robot=None, robot_name='robot', invkin_solver='KDLInvKinChainLMA', tesseract_env=None)

Robot class that uses Tesseract for kinematic solvers. A tesseract_environment.Environment class is populated using a general_robotics_toolbox.Robot structure using the utility functions in the general_robotics_toolbox.tesseract module. These solvers use high performance solvers, and are significantly faster than the Python based solvers in general_robotics_toolbox. The functions of this class should return identical results to the Python based solvers.

fwdkin(theta, base_link_name=None, tip_link_name=None)

Compute robot forward kinematics at specified joint angles.

Parameters:

theta (np.array) – The N vector of joint angles. Length must match number of joints. Expects radians or meters.
base_link_name (str) – Base frame to compute kinematics. Optional, defaults to “world”
tip_link_name (str) – The tip link to compute forward kinematics. Optional, defaults to last link.

Return type:

general_robotics_toolbox.Transform

Returns:

The pose of tip link in base frame

invkin(tip_link_pose, theta_seed, base_link_name=None, tip_link_name=None)

Solve inverse kinematics of robot at specified tip link pose.

Parameters:

tip_link_pose (general_robotics_toolbox.Transform) – The desired pose of the robot tip link
theta_seed – The N vector of seed joint angles. Used as initial position of robot joints for iterative solvers. Length must match number of joints. Expects radians or meters.
base_link_name (str) – Base frame to solve kinematics. Optional, defaults to “world”
tip_link_name (str) – The tip link to solve kinematics. Optional, defaults to last link.

Return type:

List[np.array]

Returns:

A list of joint angle candidate solutions, or empty if no solution possible

jacobian(theta, base_link_name=None, link_name=None)

Compute robot jacobian at specified joint angles.

Parameters:

theta (np.array) – The N vector of joint angles. Length must match number of joints. Expects radians or meters.
base_link_name (str) – Base frame to compute jacobian. Optional, defaults to “world”
tip_link_name (str) – The tip link to compute jacobian. Optional, defaults to last link.

Return type:

np.array

Returns:

The 6x6 Jacobian array. Note that this array has angular velocity on the first three rows.

redundant_solutions(theta)

Return “redundant” joint angle solutions. Some robot joints can spin more than 360 degrees, resulting in multiple redundant solutions with the joints rotated plus or minus 360 degrees. Return these potential solutions.

Parameters:: theta (np.array) – Robot joint angles
Return type:: List[np.array]
Returns:: List of redundant joint angles

class general_robotics_toolbox.tesseract.URInvKinParameters(d1: float, a2: float, a3: float, d4: float, d5: float, d6: float)

UR inverse kinematics solver parameters. See robot_to_ur_inv_kin_parameters()

a2: float: Alias for field number 1

a3: float: Alias for field number 2

d1: float: Alias for field number 0

d4: float: Alias for field number 3

d5: float: Alias for field number 4

d6: float: Alias for field number 5

general_robotics_toolbox.tesseract.get_fixed_link_and_joint(T, link_name, joint_name, parent_link_name)

Create a tesseract link and fixed joint from parameters

All units are expected to be SI

Parameters:

T (general_robotics_toolbox.Transform) – Transform for fixed link origin
link_name (str) – Link name
joint_name (str) – Joint name
parent_link_name (str) – Parent link name

Return type:

Tuple[tesseract_scene_graph.Link,tesseract_scene_graph.Joint]

general_robotics_toolbox.tesseract.get_link_and_joint(h, p, joint_type, joint_lower_limit, joint_upper_limit, joint_vel_limit, joint_acc_limit, joint_effort_limit, link_name, joint_name, parent_link_name)

Create a tesseract link and joint from parameters

All units are expected to be SI

Parameters:

h (np.array) – Joint unit vector, corresponds to H[:,i]
p (np.array) – Joint origin vector, corresponds to P[:,i+1]
joint_type (int) – Joint type. Supported values 0 for revolute, 1 for prismatic
joint_lower_limit (float) – Joint lower position limit
joint_upper_limit (float) – Joint upper position limit
joint_vel_limit (float) – Joint velocity limit
joint_acc_limit (float) – Joint acceleration limit
joint_effort_limit (float) – Joint effort limit
link_name (str) – Link name
joint_name (str) – Joint name
parent_link_name (str) – Parent link name

Return type:

Tuple[tesseract_scene_graph.Link,tesseract_scene_graph.Joint]

general_robotics_toolbox.tesseract.get_robot_world_to_base_joint(robot, robot_name)

Create fixed joint from world to robot base origin. Uses robot.T_base if specified, otherwise return identity transform.

Parameters:

robot (general_robotics_toolbox.Robot) – Input Robot structure containing robot parameters
robot_name (str) – The name of the robot. Must match name used to initialize other parameters

Return type:

tesseract_scene_graph.Joint

Returns:

The fixed joint from world to robot origin

general_robotics_toolbox.tesseract.isometry3d_to_transform(eig_iso)

Convert a tesseract_common.Isometry3d to general_robotics_toolbox.Transform

Parameters:: T (tesseract_common.Isometry3d) – The transform to convert

:rtype : general_robotics_toolbox.Transform :return : The converted transform

general_robotics_toolbox.tesseract.kinematics_plugin_fwdkin_kdl_plugin_info_dict(robot_name, base_link, tip_link)

Create dictionary of yaml parameters for KDL forward kinematics

Parameters:

robot_name (str) – The name of the robot
base_link (str) – The name of the robot base link
tip_link (str) – The name of the robot tip link

Return type:

dict

Returns:

KDL plugin info as dict to convert to yaml

general_robotics_toolbox.tesseract.kinematics_plugin_info_dict(robot, robot_name, link_names, joint_names, chain_link_names, invkin_solver=None, invkin_plugin_info=None, base_link=None, tip_link=None)

Creates dictionary of kinematics plugin info from parameters

Parameters:

robot (general_robotics_toolbox.Robot) – Robot structure containing robot parameters
robot_name (str) – The name of the robot. Must match name used to initialize other parameters
link_names (List[str]) – The names of the links in the current robot
joint_names (List[str]) – The names of the joints in the current robot
chain_link_names (List[str]) – The names of the chain links of the current robot
invkin_solver (str) – The name of the inverse kinematics solver to use. Defaults to KDLInvKinChainLMA. Supports KDLInvKinChainLMA, KDLInvKinChainNR, OPWInvKin, and URInvKin
invkin_plugin_info (str) – Override plugin_info yaml string. invkin_solver is ignored if used
base_link (str) – The name of the robot base link. Optional, defaults to link_names[0]
tip_link (str) – The name of the tip link. Optional, default to link_names[-1]

Return type:

dict

Returns:

Kinematics plugin info as dict to convert to yaml

general_robotics_toolbox.tesseract.kinematics_plugin_info_string(robot, robot_name, link_names, joint_names, chain_link_names, invkin_solver=None, invkin_plugin_info=None, base_link=None, tip_link=None)

Creates string of kinematics plugin info from parameters

Parameters:

robot (general_robotics_toolbox.Robot) – Robot structure containing robot parameters
robot_name (str) – The name of the robot. Must match name used to initialize other parameters
link_names (List[str]) – The names of the links in the current robot
joint_names (List[str]) – The names of the joints in the current robot
chain_link_names (List[str]) – The names of the chain links of the current robot
invkin_solver (str) – The name of the inverse kinematics solver to use. Defaults to KDLInvKinChainLMA. Supports KDLInvKinChainLMA, KDLInvKinChainNR, OPWInvKin, and URInvKin
invkin_plugin_info (str) – Override plugin_info yaml string. invkin_solver is ignored if used
base_link (str) – The name of the robot base link. Optional, defaults to link_names[0]
tip_link (str) – The name of the tip link. Optional, default to link_names[-1]

Return type:

str

Returns:

Kinematics plugin info as yaml string

general_robotics_toolbox.tesseract.kinematics_plugin_invkin_kdl_plugin_info_dict(robot_name, base_link, tip_link, default_solver='KDLInvKinChainLMA')

Create dictionary of yaml parameters for KDL inverse kinematics. Supported solvers are KDLInvKinChainLMA and KDLInvKinChainNR

Parameters:

robot_name (str) – The name of the robot
base_link (str) – The name of the robot base link
tip_link (str) – The name of the robot tip link
default_solver (str) – The default KDL solver. Optional, defaults to KDLInvKinChainLMA

Return type:

dict

Returns:

KDL plugin info as dict to convert to yaml

general_robotics_toolbox.tesseract.kinematics_plugin_invkin_opw_plugin_info_dict(robot_name, base_link, tip_link, opw_params)

Create dictionary of yaml parameters for OPW inverse kinematics.

Parameters:

robot_name (str) – The name of the robot
base_link (str) – The name of the robot base link
tip_link (str) – The name of the robot tip link
default_solver – Structure containing OPW parameters. See robot_to_opw_inv_kin_parameters()

Return type:

dict

Returns:

OPWInvKin plugin info as dict to convert to yaml

general_robotics_toolbox.tesseract.kinematics_plugin_invkin_ur_plugin_info_dict(robot_name, base_link, tip_link, ur_params)

Create dictionary of yaml parameters for UR inverse kinematics.

Parameters:

robot_name (str) – The name of the robot
base_link (str) – The name of the robot base link
tip_link (str) – The name of the robot tip link
default_solver – Structure containing UR parameters. See robot_to_ur_inv_kin_parameters()

Return type:

dict

Returns:

URInvKin plugin info as dict to convert to yaml

general_robotics_toolbox.tesseract.redundant_solutions(tesseract_kin_group, theta)

Return “redundant” joint angle solutions. Some robot joints can spin more than 360 degrees, resulting in multiple redundant solutions with the joints rotated plus or minus 360 degrees. Return these potential solutions.

Parameters:

tesseract_kin_group (tesseract_kinematics.KinematicGroup) – Tesseract KinematicGroup instance
theta (np.array) – Robot joint angles

Return type:

List[np.array]

Returns:

List of redundant joint angles

general_robotics_toolbox.tesseract.robot_to_opw_inv_kin_parameters(robot)

Convert “Robot” structure to OPW Kinematics parameters a1, a2, b, c1, c2, c3, c4, offsets, and sign_corrections.

See https://github.com/Jmeyer1292/opw_kinematics for definitions of these parameters.

This function uses a heuristic method to guess the correct configuration. The following must be true:

Robot has six joints, with a vertical first axis, two parallel and orthogonal joints, and a spherical wrist.

(Orthogonal, parallel, wrist OPW) format.

The heuristic algorithm requires that h1 = z, h2 = h3 = y, and h5 = y. h4 = h5 may be either along x or z, depending if the robot home position is outstretched along X or Z. Most ROS Industrial robots use the X outstretched format, with either Joint 2 or Joint 3 bent 90 degrees from the vertical configuration. The heuristic will check to see if the bend is in one of these joints, and attempt to align with the expected OPW configuration with the robot outstretched vertically.

general_robotics_toolbox.tesseract.robot_to_scene_graph(robot, return_names=False)

Convert a general_robotics_toolbox.Robot structure to tesseract_scene_graph.Graph. Does not create world link, or use T_base.

Parameters:

robot (general_robotics_toolbox.Robot) – Input Robot structure containing robot parameters
return_names (bool) – Return the names of created links and joints. Optional, default false

Return type:

tesseract_scene_graph.Graph or (tesseract_scene_graph.Graph, List[str], List[str], List[str])

Returns:

The scene graph, and optionally the scene graph, link names, joint names, and chain link names if return_names is True

general_robotics_toolbox.tesseract.robot_to_tesseract_env(robot, robot_name='robot', include_world=True, return_names=False, invkin_solver=None, invkin_plugin_info=None)

Creates a TesseractEnvironment initialized with a specified robot structure.

Parameters:

robot (general_robotics_toolbox.Robot) – Input Robot structure containing robot parameters
robot_name (str) – The name of the robot. Optional, defaults to “robot”
include_world (bool) – Include the world link in commands. Omit if adding another robot to an environment. Optional, defaults to True.
return_names (bool) – Return names of links in joints. Optional, defaults to false
invkin_solver (str) – The name of the inverse kinematics solver to use. Defaults to KDLInvKinChainLMA. Supports KDLInvKinChainLMA, KDLInvKinChainNR, OPWInvKin, and URInvKin
invkin_plugin_info (str) – Override plugin_info yaml string. invkin_solver is ignored if used

Return type:

tesseract_environment.Commands or (tesseract_environment.Commands, List[str], List[str], List[str])

Returns:

The environment commands, and optionally the commands, link names and joint names if return_names is True

general_robotics_toolbox.tesseract.robot_to_tesseract_env_commands(robot, robot_name='robot', include_world=True, return_names=False, invkin_solver=None, invkin_plugin_info=None)

Creates a set of Tesseract Environment commands to initialize the environment with a specified robot structure.

Parameters:

robot (general_robotics_toolbox.Robot) – Input Robot structure containing robot parameters
robot_name (str) – The name of the robot. Optional, defaults to “robot”
include_world (bool) – Include the world link in commands. Omit if adding another robot to an environment. Optional, defaults to True.
return_names (bool) – Return names of links in joints. Optional, defaults to false
invkin_solver (str) – The name of the inverse kinematics solver to use. Defaults to KDLInvKinChainLMA. Supports KDLInvKinChainLMA, KDLInvKinChainNR, OPWInvKin, and URInvKin
invkin_plugin_info (str) – Override plugin_info yaml string. invkin_solver is ignored if used

Return type:

tesseract_environment.Commands or (tesseract_environment.Commands, List[str], List[str], List[str])

Returns:

The environment commands, and optionally the commands, link names and joint names if return_names is True

general_robotics_toolbox.tesseract.robot_to_ur_inv_kin_parameters(robot)

Convert “Robot” structure to Universal Robots DH parameters for inverse kinematics. Determines d1, a2, a3, d4, d5, d6

See https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/ for more details.

Robot must be aligned with +x or -x. Factory definition is along -x. URDF definition is along +x, rotated 180 degrees from factory home position.

general_robotics_toolbox.tesseract.tesseract_kinematics_information(robot, robot_name, link_names, joint_names, chain_link_names, invkin_solver=None, invkin_plugin_info=None, base_link=None, tip_link=None)

Creates a tesseract_kinematics.KinematicsInformation structure from parameters

Parameters:

robot (general_robotics_toolbox.Robot) – Robot structure containing robot parameters
robot_name (str) – The name of the robot. Must match name used to initialize other parameters
link_names (List[str]) – The names of the links in the current robot
joint_names (List[str]) – The names of the joints in the current robot
chain_link_names (List[str]) – The names of the chain links of the current robot
invkin_solver (str) – The name of the inverse kinematics solver to use. Defaults to KDLInvKinChainLMA. Supports KDLInvKinChainLMA, KDLInvKinChainNR, OPWInvKin, and URInvKin
invkin_plugin_info (str) – Override plugin_info yaml string. invkin_solver is ignored if used
base_link (str) – The name of the robot base link. Optional, defaults to link_names[0]
tip_link (str) – The name of the tip link. Optional, default to link_names[-1]

Return type:

tesseract_kinematics.KinematicsInformation

Returns:

The information structure

general_robotics_toolbox.tesseract.transform_to_isometry3d(T)

Convert a general_robotics_toolbox.Transform to tesseract_common.Isometry3d

Parameters:: T (general_robotics_toolbox.Transform) – The transform to convert

:rtype : tesseract_common.Isometry3d :return : The converted transform

general_robotics_toolbox.tesseract.world_scene_graph(world_link_name='world')

Returns a scene graph containing only a world link

Parameters:: world_name_name – The name of the world link. Optional, defaults to “world”
Return type:: tesseract_scene_graph.Graph
Returns:: Scene graph with world link