general_robotics_toolbox

Robotics functions for geometry, forward kinematics, inverse kinematics, and dynamics.

general_robotics_toolbox

general_robotics_toolbox.R2q(R)

Converts a 3 x 3 rotation matrix into a quaternion. Quaternion is returned in the form q = [q0;qv].

Parameters:

R (numpy.array) – 3 x 3 rotation matrix

Return type:

numpy.array

Returns:

the quaternion as a 4 x 1 vector q = [q0;qv]

general_robotics_toolbox.R2rot(R)

Recover k and theta from a 3 x 3 rotation matrix

sin(theta) = | R-R^T |/2 cos(theta) = (tr(R)-1)/2 k = invhat(R-R^T)/(2*sin(theta)) theta = atan2(sin(theta),cos(theta)

Parameters:

R (numpy.array) – 3 x 3 rotation matrix

Return type:

(numpy.array, number)

Returns:

( 3 x 1 k unit vector, rotation about k in radians)

class general_robotics_toolbox.Robot(H, P, joint_type, joint_lower_limit=None, joint_upper_limit=None, joint_vel_limit=None, joint_acc_limit=None, M=None, R_tool=None, p_tool=None, joint_names=None, root_link_name=None, tip_link_name=None, T_flange=None, T_base=None)

Holds the kinematic information for a single chain robot

Attribute H:

A 3 x N matrix containing the direction the joints as unit vectors, one joint per column

Attribute P:

A 3 x (N + 1) matrix containing the distance vector from i to i+1, one vector per column

Attribute joint_type:

A list of N numbers containing the joint type. 0 for rotary, 1 for prismatic, 2 and 3 for mobile

Attribute joint_lower_limit:

A list of N numbers containing the joint lower limits. Optional

Attribute joint_upper_limit:

A list of N numbers containing the joint upper limits. Optional

Attribute joint_vel_limit:

A list of N numbers containing the joint velocity limits. Optional

Attribute joint_acc_limit:

A list of N numbers containing the joint acceleration limits. Optional

Attribute M:

A list of N, 6 x 6 spatial inertia matrices for the links. Optional

Attribute R_tool:

A 3 x 3 rotation matrix for the tool frame. Optional

Attribute p_tool:

A 3 x 1 vector for the tool frame. Optional

Attribute joint_names:

A list of N strings containing the names of the joints if loaded from URDF. Optional

Attribute root_link_name:

A string containing the name of the kinematic chain root link if loaded from URDF. Optional

Attribute tip_link_name:

A string containing the name of the kinematic chain tip link if loaded from URDF. Optional

Attribute T_flange:

Optional transform between end of kinematic chain and the tool frame. This is for compatibility with ROS tool formats.

Attribute T_base:

Optional transform of base of robot in world frame.

class general_robotics_toolbox.Transform(R, p, parent_frame_id=None, child_frame_id=None)

Holds a transform consisting of a rotation matrix and a vector

Attribute R:

The 3 x 3 rotation matrix

Attribute p:

The 3 x 1 position vector

Note: Transform objects are also used to represent the pose of links

general_robotics_toolbox.apply_robot_aux_transforms(robot, T)

Apply R_tool,p_tool, T_flange, and T_base to T

Parameters:

  • robot (Robot) – Robot instance with aux transforms

  • T (Transform) – Transform to apply aux transforms

Return type:

Transform

Returns:

Returns transform with applied aux transforms

general_robotics_toolbox.equivalent_configurations(robot, theta_v, last_joints=None)

Return equivalent robot configurations with joints +-360 degree joint offset within joint limits

Parameters:

  • robot (general_robotics_toolbox.Robot) – The robot object representing the geometry of the robot

  • theta_v (list) – The desired pose of the robot

  • last_joints (list, tuple, or numpy.array) – The joints of the robot at the last timestep. The returned first returned joint configuration will be the closets to last_joints. Optional

Return type:

list of numpy.array

Returns:

A list of zero or more additional configurations that are equivalent with +-360 degree joint offsets within joint limits.

general_robotics_toolbox.fwdkin(robot, theta, _ignore_limits=False)

Computes the pose of the robot tool flange based on a Robot object and the joint angles.

Parameters:

  • robot (Robot) – The robot object containing kinematic information

  • theta (numpy.array) – N x 1 array of joint angles. Must have same number of joints as Robot object

Return type:

Transform

Returns:

The Pose of the robot tool flange

general_robotics_toolbox.hat(k)

Returns a 3 x 3 cross product matrix for a 3 x 1 vector

[ 0 -k3 k2]

khat = [ k3 0 -k1]

[-k2 k1 0]

Parameters:

k (numpy.array) – 3 x 1 vector

Return type:

numpy.array

Returns:

the 3 x 3 cross product matrix

general_robotics_toolbox.identity_transform()

Returns an identity transform

general_robotics_toolbox.iterative_invkin(robot, desired_pose, q_current, max_steps=200, Kp=array([[0.3, 0., 0.], [0., 0.3, 0.], [0., 0., 0.3]]), KR=array([[0.3, 0., 0.], [0., 0.3, 0.], [0., 0., 0.3]]), tol=0.0001)

Inverse kinematics using gradient descent. Convex solution based on q_current input joint positions. Return may be out of range of robot since only produced a local solution!

Parameters:

  • robot (general_robotics_toolbox.Robot) – The robot object representing the geometry of the robot

  • desired_pose (general_robotics_toolbox.Transform) – The desired pose of the robot

  • q_current (list, tuple, or numpy.array) – Seed joint position for gradient descent

  • max_steps (int) – The maximum iterations before failing. Default 200

  • Kp (np.array) – Gain array for position error. Default 0.3

  • KP – Gain array for rotation error. Default 0.3

  • tol (float) – Convergence tolerance

Return type:

tuple converged, list of numpy.array

Returns:

A tuple with a boolean of convergence, and a list of joint angles. This list will typically only contain one entry.

general_robotics_toolbox.q2R(q)

Converts a quaternion into a 3 x 3 rotation matrix according to the Euler-Rodrigues formula.

Parameters:

q (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

Return type:

numpy.array

Returns:

the 3x3 rotation matrix

general_robotics_toolbox.q2rot(q)

Converts a quaternion into k and theta

Parameters:

q (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

Return type:

(numpy.array, number)

Returns:

the 3x1 rotation vector and theta

general_robotics_toolbox.quatcomplement(q)

Generates the quaternion complement

in: q = [q0;qv]; out: qc = [q0;-qv];

Parameters:

q (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

Return type:

numpy.array

Returns:

the quaternion complement as a 4 x 1 vector q = [q0;-qv]

general_robotics_toolbox.quatjacobian(q)

Returns the 4 x 3 Jacobian matrix relating an angular velocity to the quarternion rate of change

Parameters:

q (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

Return type:

numpy.array

Returns:

the 4 x 3 Jacobian matrix

general_robotics_toolbox.quatproduct(q)

generates matrix representation of a Hamilton quaternion product operator

in: q = [q0;qv]; out: Q = [q0 -qv’; qv q0*eye(3)+ cross(qv)]

Parameters:

q (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

Return type:

numpy.array

Returns:

the 4 x 4 product matrix

general_robotics_toolbox.robot6_sphericalwrist_invkin(robot, desired_pose, last_joints=None)

Inverse kinematics for six axis articulated industrial robots with spherical wrists. Also referred to as “OPW” robots. Examples include Puma 260, ABB IRB6640, Staubli TX40, etc. Note that this is not for Universal Robot wrist configurations.

Parameters:

  • robot (general_robotics_toolbox.Robot) – The robot object representing the geometry of the robot

  • desired_pose (general_robotics_toolbox.Transform) – The desired pose of the robot

  • last_joints (list, tuple, or numpy.array) – The joints of the robot at the last timestep. The returned first returned joint configuration will be the closets to last_joints. Optional

Return type:

list of numpy.array

Returns:

A list of zero or more joint angles that match the desired pose. An empty list means that the desired pose cannot be reached. If last_joints is specified, the first entry is the closest configuration to last_joints.

general_robotics_toolbox.robotjacobian(robot, theta, _ignore_limits=False)

Computes the Jacobian matrix for the robot tool flange based on a Robot object and the joint angles.

Parameters:

  • robot (Robot) – The robot object containing kinematic information

  • theta (numpy.array) – N x 1 array of joint angles in radians or meters as appropriate. Must have same number of joints as Robot object.

Return type:

numpy.array

Returns:

The 6 x N Jacobian matrix

general_robotics_toolbox.rot(k, theta)

Generates a 3 x 3 rotation matrix from a unit 3 x 1 unit vector axis and an angle in radians using the Euler-Rodrigues formula

R = I + sin(theta)*hat(k) + (1 - cos(theta))*hat(k)^2

Parameters:

  • k (numpy.array) – 3 x 1 unit vector axis

  • theta (number) – rotation about k in radians

Return type:

numpy.array

Returns:

the 3 x 3 rotation matrix

general_robotics_toolbox.rot2q(k, theta)

Converts a 3 x 1 rotation vector and theta into a quaternion. Quaternion is returned in the form q = [q0;qv].

Parameters:

  • k (numpy.array) – 3 x 1 unit vector axis

  • theta (number) – rotation about k in radians

Return type:

numpy.array

Returns:

the quaternion as a 4 x 1 vector q = [q0;qv]

general_robotics_toolbox.screw_matrix(r)

Returns the matrix G representing the 6 x 6 transformation between screws in a rigid body, i.e.

(twist)_2 = transpose(G)*(twist)_1

(twist) = [angular velocity, linear velocity]

(wrench)_1 = G*(wrench)_2

(wrench) = [torque, force]

Parameters:

r (numpy.array) – the 3 x 1 displacement vector from point 1 to point 2

Return type:

numpy.array

Returns:

the 6 x 6 screw matrix

general_robotics_toolbox.slerp(q0, q1, t)

Spherical linear interpolation between two quaternions

Parameters:

  • q0 (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

  • q1 (numpy.array) – 4 x 1 vector representation of a quaternion q = [q0;qv]

  • t (number) – interpolation parameter in the range [0,1]

Return type:

numpy.array

Returns:

the 4 x 1 interpolated quaternion

general_robotics_toolbox.subproblem0(p, q, k)

Solves canonical geometric subproblem 0, theta subtended between p and q according to

q = rot(k, theta)*p

** assumes k’*p = 0 and k’*q = 0

Requires that p and q are perpendicular to k. Use subproblem 1 if this is not guaranteed.

Parameters:

  • p (numpy.array) – 3 x 1 vector before rotation

  • q (numpy.array) – 3 x 1 vector after rotation

  • k (numpy.array) – 3 x 1 rotation axis unit vector

Return type:

number

Returns:

theta angle as scalar in radians

general_robotics_toolbox.subproblem1(p, q, k)

Solves canonical geometric subproblem 1, theta subtended between p and q according to

q = rot(k, theta)*p

Parameters:

  • p (numpy.array) – 3 x 1 vector before rotation

  • q (numpy.array) – 3 x 1 vector after rotation

  • k (numpy.array) – 3 x 1 rotation axis unit vector

Return type:

number

Returns:

theta angle as scalar in radians

general_robotics_toolbox.subproblem2(p, q, k1, k2)

Solves canonical geometric subproblem 2, solve for two coincident, nonparallel axes rotation a link according to

q = rot(k1, theta1) * rot(k2, theta2) * p

solves by looking for the intersection between cones of

rot(k1,-theta1)q = rot(k2, theta2) * p

may have 0, 1, or 2 solutions

Parameters:

  • p (numpy.array) – 3 x 1 vector before rotations

  • q (numpy.array) – 3 x 1 vector after rotations

  • k1 (numpy.array) – 3 x 1 rotation axis 1 unit vector

  • k2 (numpy.array) – 3 x 1 rotation axis 2 unit vector

Return type:

list of number pairs

Returns:

theta angles as list of number pairs in radians

general_robotics_toolbox.subproblem3(p, q, k, d)

Solves canonical geometric subproblem 3,solve for theta in an elbow joint according to

|| q + rot(k, theta)*p || = d

may have 0, 1, or 2 solutions

Parameters:

  • p (numpy.array) – 3 x 1 position vector of point p

  • q (numpy.array) – 3 x 1 position vector of point q

  • k (numpy.array) – 3 x 1 rotation axis for point p

  • d (number) – desired distance between p and q after rotation

Return type:

list of numbers

Returns:

list of valid theta angles in radians

general_robotics_toolbox.subproblem4(p, q, k, d)

Solves canonical geometric subproblem 4, theta for static displacement from rotation axis according to

d = p.T*rot(k, theta)*q

may have 0, 1, or 2 solutions

Parameters:

  • p (numpy.array) – 3 x 1 position vector of point p

  • q (numpy.array) – 3 x 1 position vector of point q

  • k (numpy.array) – 3x1 rotation axis for point p

  • d (number) – desired displacement

Return type:

list of numbers

Returns:

list of valid theta angles in radians

general_robotics_toolbox.unapply_robot_aux_transforms(robot, T)

Unapply R_tool,p_tool, T_flange, and T_base to T

Parameters:

  • robot (Robot) – Robot instance with aux transforms

  • T (Transform) – Transform to unapply aux transforms

Return type:

Transform

Returns:

Returns transform with unapplied aux transforms

general_robotics_toolbox.ur_invkin(robot, desired_pose, last_joints=None)

Inverse kinematics for Universal Robot articulated industrial robots. Examples include UR3, UR5, UR10

Parameters:

  • robot (general_robotics_toolbox.Robot) – The robot object representing the geometry of the robot

  • desired_pose (general_robotics_toolbox.Transform) – The desired pose of the robot

  • last_joints (list, tuple, or numpy.array) – The joints of the robot at the last timestep. The returned first returned joint configuration will be the closets to last_joints. Optional

Return type:

list of numpy.array

Returns:

A list of zero or more joint angles that match the desired pose. An empty list means that the desired pose cannot be reached. If last_joints is specified, the first entry is the closest configuration to last_joints.