components.robot.common package

Submodules

components.robot.common.check_args module

test_args module contains test for input arguments. It checks if input argument passed by user is valid or not. If any invalid data is found, the called function in test_args returns false

components.robot.common.check_args.is_mat_list(list_matrices)

is_mat_list checks(arg1) checks if arg1 is a list containing numpy matrix data type elements or not. If not, False is returned.

components.robot.common.check_args.is_vector(item)
components.robot.common.check_args.np2vtk(mat)
components.robot.common.check_args.rpy2r(theta, order)
components.robot.common.check_args.se2_constructor_args_check(x, y, rot, theta, so2, se2)
components.robot.common.check_args.se2_valid(obj)
components.robot.common.check_args.so2_angle_list_check(ang_list)
components.robot.common.check_args.so2_input_matrix(args_in)
components.robot.common.check_args.so2_input_types_check(args_in)
components.robot.common.check_args.so2_interp_check(obj1, obj2, s)
components.robot.common.check_args.so2_valid(obj)
components.robot.common.check_args.so3_constructor_args_check(args_in)
components.robot.common.check_args.super_pose_add_sub_check(obj, other)
components.robot.common.check_args.super_pose_appenditem(obj, item)
components.robot.common.check_args.super_pose_divide_check(obj, other)
components.robot.common.check_args.super_pose_multiply_check(obj, other)
components.robot.common.check_args.super_pose_subclass_check(obj, other)
components.robot.common.check_args.tr2angvec(tr, unit)
components.robot.common.check_args.tr2eul(tr, unit, flip)
components.robot.common.check_args.tr2rpy(tr, unit, order)
components.robot.common.check_args.unit_check(unit)
components.robot.common.check_args.valid_pose(obj)

components.robot.common.common module

Common Module contains code shared by robotics and machine vision toolboxes

components.robot.common.common.create_homogeneous_transform_from_point(p)

Create a homogeneous transform to move to a given point

Parameters:p (numpy.ndarray) – The (x, y, z) point we want our homogeneous tranform to move to
Returns:4x4 Homogeneous Transform of a point
Return type:numpy.matrix
components.robot.common.common.create_point_from_homogeneous_transform(T)

Create a point from a homogeneous transform

Parameters:T (numpy matrix) – The 4x4 homogeneous transform
Returns:The (x, y, z) coordinates of a point from a transform
Return type:np.ndarray
components.robot.common.common.flip_base(ee_pos, direction, value, animation=False)
components.robot.common.common.get_rotation_from_homogeneous_transform(transform)

Extract the rotation section of the homogeneous transformation

Parameters:transform (numpy.ndarray) – The 4x4 homogeneous transform to extract the rotation matrix from.
Returns:3x3 Rotation Matrix
Return type:numpy.matrix
components.robot.common.common.ishomog(tr, dim, rtest='')

ISHOMOG Test if SE(3) homogeneous transformation matrix. ISHOMOG(T) is true if the argument T is of dimension 4x4 or 4x4xN, else false. ISHOMOG(T, ‘valid’) as above, but also checks the validity of the rotation sub-matrix. See Also: isrot, ishomog2, isvec

components.robot.common.common.isrot(rot, dtest=False)

ISROT Test if SO(2) or SO(3) rotation matrix ISROT(rot) is true if the argument if of dimension 2x2, 2x2xN, 3x3, or 3x3xN, else false (0). ISROT(rot, ‘valid’) as above, but also checks the validity of the rotation. See also ISHOMOG, ISROT2, ISVEC.

components.robot.common.common.isrot2(rot, dtest=False)
components.robot.common.common.isvec(v, l=3)

ISVEC Test if vector

components.robot.common.common.round_end_effector_position(ee_pos, direction, point, offset=None)

components.robot.common.pose module

class components.robot.common.pose.SE2(theta=None, unit='rad', x=None, y=None, rot=None, so2=None, se2=None, null=False)

Bases: components.robot.common.pose.SO2

SE3()
inv()

Returns an inverse SE2 object of same length

static is_valid(obj)

Checks if a np.matrix is a valid SO2 pose.

log()
classmethod rand()
t_matrix()

Returns list of translation matrices of SE2 object

transl
transl_vec

Returns list of translation vectors of SE2 object

xyt(unit='rad')

Return list of 3x1 dimension vectors containing x, y translation components and theta

class components.robot.common.pose.SE3(x=None, y=None, z=None, rot=None, so3=None, se3=None, null=False)

Bases: components.robot.common.pose.SO3

classmethod Rx(theta, unit='rad', x=None, y=None, z=None)
classmethod Ry(theta, unit='rad', x=None, y=None, z=None)
classmethod Rz(theta, unit='rad', x=None, y=None, z=None)
static form_trans_matrix(rot, transl)
classmethod np(arg_in)
classmethod rand()
classmethod se3(args_in)
transl
class components.robot.common.pose.SO2(args_in=None, unit='rad', null=False)

Bases: components.robot.common.super_pose.SuperPose

SE2()

Returns SE2 object with same rotational component as SO2 and a zero translation component

angle

Returns angle of SO2 object matrices in unit radians

static check(obj)
det

Returns a list containing determinants of all matrices in a SO2 object

eig()
static exp()
static form_trans_matrix(rot, transl)
interp(other, s)

Returns the interpolated SO2 object

inv()

Returns inverse SO2 object

static is_valid(obj)

Checks if a np.matrix is a valid SO2 pose.

log()
new()

Returns a deep copy of SO2 object

plot()
classmethod rand()
t_matrix()

Returns a list of transformation matrices

unit
class components.robot.common.pose.SO3(args_in=None, null=False)

Bases: components.robot.common.super_pose.SuperPose

classmethod Rx(theta, unit='rad')
classmethod Ry(theta, unit='rad')
classmethod Rz(theta, unit='rad')
angvec()
animate(other=None, duration=5, gif=None)
approach_vec()
static check(self)
ctraj(T1, N)
det()
eig()
classmethod eul(theta, unit='rad')
exp()
interp()
inv()
log()
new()
norm_vec()
classmethod np(args_in)
classmethod oa(o, a)
orient_vec()
plot()
classmethod rand()
rotation()
classmethod rpy(thetas, order='zyx', unit='rad')
classmethod se3(args_in)
classmethod so3(args_in)
t_matrix()

Returns transformation matrices associated with the pose object. Return data type is list or np.matrix depending on number of transformation matrices present.

to_se3()
toangvec()
toeul()
torpy()
trnorm()

components.robot.common.quaternion module

class components.robot.common.quaternion.Quaternion(s=None, v=None)

Bases: object

conj()
double()

Return the quaternion as 4-element vector. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: 4-vector @return: the quaternion elements

inv()
matrix()
norm()

Return the norm of this quaternion. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: number @return: the norm

classmethod pure(vec)
classmethod qt(arg_in)
r()

Return an equivalent rotation matrix. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: 3x3 orthonormal rotation matrix @return: equivalent rotation matrix

tr()
unit()

Return an equivalent unit quaternion Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: quaternion @return: equivalent unit quaternion

class components.robot.common.quaternion.UnitQuaternion(s=None, v=None)

Bases: components.robot.common.quaternion.Quaternion

classmethod Rx(angle, unit='rad')
classmethod Ry(angle, unit='rad')
classmethod Rz(angle, unit='rad')
classmethod angvec(theta, v, unit='rad')
animate(qr=None, duration=5, gif=None)
dot(omega)
dotb(omega)
classmethod eul(arg_in, unit='rad')
interp(qr, r=0.5, shortest=False)

Algorithm source: https://en.wikipedia.org/wiki/Slerp :param qr: UnitQuaternion :param shortest: Take the shortest path along the great circle :param r: interpolation point :return: interpolated UnitQuaternion

matrix()
new()
classmethod omega(w)
plot()
q2r()
q2tr()
classmethod qt(arg_in)
classmethod rot(arg_in)
classmethod rpy(arg_in, unit='rad')
to_angvec(unit='rad')
to_rot()
to_rpy()
to_se3()
to_so3()
to_vec()
static tr2q(t)

Converts a homogeneous rotation matrix to a Quaternion object Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke :param t: homogeneous matrix :return: quaternion object

classmethod vec(arg_in)

components.robot.common.states module

class components.robot.common.states.Block(position, status, final_destination)

Bases: object

class components.robot.common.states.BlockFace(xPos, yPos, zPos, face, blockWidth=0.49)

Bases: object

get_face_coordinate()
class components.robot.common.states.BuildingStates

Bases: enum.IntEnum

An enumeration.

DONE = 3
DONE_ORIGIN = 2
EMPTY = 4
WAITING_FOR_FERRYING = 0
WAITING_FOR_FILLING = 1
class components.robot.common.states.Division

Bases: object

get_locations_to_place_blocks()
class components.robot.common.states.MoveBlocksStore(blocks_to_remove, division)

Bases: object

class components.robot.common.states.PathPlanners

Bases: enum.Enum

An enumeration.

AStar = 1
FaceStar = 0
class components.robot.common.states.RobotBehaviors

Bases: enum.Enum

An enumeration.

BUILD = 3
FERRY = 0
MOVE = 2
SIMULATION = 5
UPDATE = 4
WAIT = 1
class components.robot.common.states.RobotCommunicator(robot_communicator, robot_id)

Bases: object

components.robot.common.super_pose module

class components.robot.common.super_pose.SuperPose

Bases: abc.ABC

append(item)
data

Always returns a list containing the matrices of the pose object. :return: A list of matrices.

dim

Returns dimensions of first matrix in pose object. Assumed that all matrices have same dimension. :return: tuple

isSE

Checks if object is of type SE2 or SE3 or none of them. :return: bool

is_equal(other)
ishomog()
ishomog2()
isrot()
isrot2()
length

Property to return number of matrices in pose object :return: int

mat

Property to return the matrices of pose object. :return: Returns np.matrix type if only one matrix is present. Else returns a list of np.matrix.

render()
t_2_r()
tr_2_rt()
tranimate()
trplot()
trplot2()
trprint()

components.robot.common.transforms module

This file contains all of the transforms functions that will be used within the toolbox

components.robot.common.transforms.angvec2r(theta, v)

ANGVEC2R(THETA, V) is an orthonormal rotation matrix (3x3) equivalent to a rotation of THETA about the vector V.

Parameters:
  • theta – rotation in radians
  • v – vector
Returns:

rotation matrix

Notes:: - If THETA == 0 then return identity matrix. - If THETA ~= 0 then V must have a finite length.

components.robot.common.transforms.angvec2tr(theta, v)

ANGVEC2TR(THETA, V) is a homogeneous transform matrix (4x4) equivalent to a rotation of THETA about the vector V.

Parameters:
  • theta – rotation in radians
  • v – vector
Returns:

homogenous transform matrix

Notes:: - The translational part is zero. - If THETA == 0 then return identity matrix. - If THETA ~= 0 then V must have a finite length.

components.robot.common.transforms.eul2r(phi, theta=None, psi=None, unit='rad')

EUL2R Convert Euler angles to rotation matrix

Parameters:
  • phi – x axis rotation
  • theta – y axis rotation
  • psi – z axis rotation
  • unit – ‘rad’ or ‘deg’ for angles
Returns:

rotation matrix

R = EUL2R(PHI, THETA, PSI, UNIT) is an SO(3) orthonornal rotation matrix (3x3) equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively. If PHI, THETA, PSI are column vectors (Nx1) then they are assumed to represent a trajectory and R is a three-dimensional matrix (3x3xN), where the last index corresponds to rows of PHI, THETA, PSI.

R = EUL2R(EUL, OPTIONS) as above but the Euler angles are taken from the vector (1x3) EUL = [PHI THETA PSI]. If EUL is a matrix (Nx3) then R is a three-dimensional matrix (3x3xN), where the last index corresponds to rows of RPY which are assumed to be [PHI,THETA,PSI].

Options:: ‘deg’ Angles given in degrees (radians default)

Note:: - The vectors PHI, THETA, PSI must be of the same length.

components.robot.common.transforms.eul2tr(phi, theta=None, psi=None, unit='rad')

EUL2TR Convert Euler angles to homogeneous transform

Parameters:
  • phi – x axis rotation
  • theta – y axis rotation
  • psi – z axis rotation
  • unit – ‘rad’ or ‘deg’ for angles
Returns:

rotation matrix

T = EUL2TR(PHI, THETA, PSI, OPTIONS) is an SE(3) homogeneous transformation matrix (4x4) with zero translation and rotation equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively. If PHI, THETA, PSI are column vectors (Nx1) then they are assumed to represent a trajectory and R is a three-dimensional matrix (4x4xN), where the last index corresponds to rows of PHI, THETA, PSI.

R = EUL2R(EUL, OPTIONS) as above but the Euler angles are taken from the vector (1x3) EUL = [PHI THETA PSI]. If EUL is a matrix (Nx3) then R is a three-dimensional matrix (4x4xN), where the last index corresponds to rows of RPY which are assumed to be [PHI,THETA,PSI].

Options:: ‘deg’ Angles given in degrees (radians default)

Note:: - The vectors PHI, THETA, PSI must be of the same length. - The translational part is zero.

components.robot.common.transforms.np2vtk(mat)
components.robot.common.transforms.oa2r(o, a=None)

OA2R Convert orientation and approach vectors to rotation matrix

Parameters:
  • o – vector parallel to Y- axes
  • a – vector parallel to the z-axes
Returns:

rotation matrix

R = OA2R(O, A) is an SO(3) rotation matrix (3x3) for the specified orientation and approach vectors (3x1) formed from 3 vectors such that R = [N O A] and N = O x A.

Notes:: - The matrix is guaranteed to be orthonormal so long as O and A are not parallel. - The vectors O and A are parallel to the Y- and Z-axes of the coordinate frame.

components.robot.common.transforms.oa2tr(o, a=None)

OA2TR Convert orientation and approach vectors to homogeneous transformation

Parameters:
  • o – vector parallel to Y- axes
  • a – vector parallel to the z-axes
Returns:

homogeneous transform

T = OA2TR(O, A) is an SE(3) homogeneous tranformation (4x4) for the specified orientation and approach vectors (3x1) formed from 3 vectors such that R = [N O A] and N = O x A.

Notes:: - The rotation submatrix is guaranteed to be orthonormal so long as O and A are not parallel. - The translational part is zero. - The vectors O and A are parallel to the Y- and Z-axes of the coordinate frame.

components.robot.common.transforms.r2t(rmat)

R2T Convert rotation matrix to a homogeneous transform

Parameters:rmat – rotation matrix
Returns:homogeneous transformation

R2T(rmat) is an SE(2) or SE(3) homogeneous transform equivalent to an SO(2) or SO(3) orthonormal rotation matrix rmat with a zero translational component. Works for T in either SE(2) or SE(3): if rmat is 2x2 then return is 3x3, or if rmat is 3x3 then return is 4x4.

Translational component is zero.

components.robot.common.transforms.rot2(theta, unit='rad')

ROT2 SO(2) Rotational Matrix

Parameters:
  • theta – rotation in radians or degrees
  • unit – “rad” or “deg” to indicate unit being used
Returns:

rotational matrix (2x2)

ROT2(THETA) is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians. ROT2(THETA, ‘deg’) as above but THETA is in degrees.

components.robot.common.transforms.rotx(theta, unit='rad')

ROTX gives rotation about X axis

Parameters:
  • theta – angle for rotation matrix
  • unit – unit of input passed. ‘rad’ or ‘deg’
Returns:

rotation matrix

rotx(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the x-axis rotx(THETA, “deg”) as above but THETA is in degrees

components.robot.common.transforms.roty(theta, unit='rad')

ROTY Rotation about Y axis

Parameters:
  • theta – angle for rotation matrix
  • unit – unit of input passed. ‘rad’ or ‘deg’
Returns:

rotation matrix

roty(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the y-axis roty(THETA, “deg”) as above but THETA is in degrees

components.robot.common.transforms.rotz(theta, unit='rad')

ROTZ Rotation about Z axis

Parameters:
  • theta – angle for rotation matrix
  • unit – unit of input passed. ‘rad’ or ‘deg’
Returns:

rotation matrix

rotz(THETA) is an SO(3) rotation matrix (3x3) representing a rotation of THETA radians about the z-axis rotz(THETA, “deg”) as above but THETA is in degrees

components.robot.common.transforms.rpy2r(thetas, order='zyx', unit='rad')

RPY2R Roll-pitch-yaw angles to rotation matrix

Parameters:
  • thetas – list of angles
  • order – ‘xyz’, ‘zyx’ or ‘yxz’
  • unit – ‘rad’ or ‘deg’
Returns:

rotation matrix

RPY2R(ROLL, PITCH, YAW, OPTIONS) is an SO(3) orthonormal rotation matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles. These correspond to rotations about the Z, Y, X axes respectively. If ROLL, PITCH, YAW are column vectors (Nx1) then they are assumed to represent a trajectory and R is a three-dimensional matrix (3x3xN), where the last index corresponds to rows of ROLL, PITCH, YAW.

R = RPY2R(RPY, OPTIONS) as above but the roll, pitch, yaw angles are taken from the vector (1x3) RPY=[ROLL,PITCH,YAW]. If RPY is a matrix(Nx3) then R is a three-dimensional matrix (3x3xN), where the last index corresponds to rows of RPY which are assumed to be [ROLL,PITCH,YAW].

Options::
‘deg’ Compute angles in degrees (radians default) ‘xyz’ Rotations about X, Y, Z axes (for a robot gripper) ‘yxz’ Rotations about Y, X, Z axes (for a camera)

Note:: - Toolbox rel 8-9 has the reverse angle sequence as default. - ZYX order is appropriate for vehicles with direction of travel in the X direction. XYZ order is appropriate if direction of travel is in the Z direction.

components.robot.common.transforms.rpy2tr(thetas, order='zyx', unit='rad')

RPY2TR Roll-pitch-yaw angles to homogeneous transform

Parameters:
  • thetas – list of angles
  • order – order can be ‘xyz’/’arm’, ‘zyx’/’vehicle’, ‘yxz’/’camera’
  • unit – unit of input angles
Returns:

homogeneous transformation matrix

T = RPY2TR(ROLL, PITCH, YAW, OPTIONS) is an SE(3) homogeneous transformation matrix (4x4) with zero translation and rotation equivalent to the specified roll, pitch, yaw angles angles. These correspond to rotations about the Z, Y, X axes respectively. If ROLL, PITCH, YAW are column vectors (Nx1) then they are assumed to represent a trajectory and R is a three-dimensional matrix (4x4xN), where the last index corresponds to rows of ROLL, PITCH, YAW.

T = RPY2TR(RPY, OPTIONS) as above but the roll, pitch, yaw angles are taken from the vector (1x3) RPY=[ROLL,PITCH,YAW]. If RPY is a matrix (Nx3) then R is a three-dimensional matrix (4x4xN), where the last index corresponds to rows of RPY which are assumed to be ROLL,PITCH,YAW].

Options::
‘deg’ Compute angles in degrees (radians default) ‘xyz’ Rotations about X, Y, Z axes (for a robot gripper) ‘yxz’ Rotations about Y, X, Z axes (for a camera)

Note:: - Toolbox rel 8-9 has the reverse angle sequence as default. - ZYX order is appropriate for vehicles with direction of travel in the X direction. XYZ order is appropriate if direction of travel is in the Z direction.

components.robot.common.transforms.rt2tr(r, t)

RT2TR Convert rotation and translation to homogeneous transform

Parameters:
  • r – rotation matrix
  • t – translation
Returns:

homogeneous transform

RT2TR(R, t) is a homogeneous transformation matrix (N+1xN+1) formed from an orthonormal rotation matrix R (NxN) and a translation vector t (Nx1). Works for R in SO(2) or SO(3): - If R is 2x2 and t is 2x1, then TR is 3x3 - If R is 3x3 and t is 3x1, then TR is 4x4

components.robot.common.transforms.skew(v)

SKEW creates Skew-symmetric metrix from vector

Parameters:v – 1 or 3 vector
Returns:skew-symmetric matrix

SKEW(V) is a skew-symmetric matrix formed from V.

If V (1x1) then S =
0 -v |
v 0 |
and if V (1x3) then S =
0 -vz vy |
vz 0 -vx |

|-vy vx 0 |

Notes:: - This is the inverse of the function VEX(). - These are the generator matrices for the Lie algebras so(2) and so(3).

components.robot.common.transforms.skewa(s)

SKEWA creates augmented skew-symmetric matrix

Parameters:s – 3 or 6 vector
Returns:augmented skew-symmetric matrix

SKEWA(V) is an augmented skew-symmetric matrix formed from V.

If V (1x3) then S =
0 -v3 v1 |
v3 0 v2 |
0 0 0 |
and if V (1x6) then S =
0 -v6 v5 v1 |
v6 0 -v4 v2 |

|-v5 v4 0 v3 | | 0 0 0 0 |

Notes:: - This is the inverse of the function VEXA(). - These are the generator matrices for the Lie algebras se(2) and se(3). - Map twist vectors in 2D and 3D space to se(2) and se(3).

components.robot.common.transforms.t2r(tmat)

R2T Convert homogeneous transform to a rotation matrix

Parameters:tmat – homogeneous transform
Returns:rotation matrix

T2R(tmat) is the orthonormal rotation matrix component of homogeneous transformation matrix tmat. Works for T in SE(2) or SE(3) if tmat is 3x3 then return is 2x2, or if tmat is 4x4 then return is 3x3.

Validity of rotational part is not checked

components.robot.common.transforms.tr2angvec(tr, unit='rad')

TR2ANGVEC Convert rotation matrix to angle-vector form :param tr: Rotation matrix :param unit: ‘rad’ or ‘deg’ :return: Angle-vector form TR2ANGVEC(R, OPTIONS) is rotation expressed in terms of an angle THETA (1x1) about the axis V (1x3) equivalent to the orthonormal rotation matrix R (3x3). TR2ANGVEC(T, OPTIONS) as above but uses the rotational part of the homogeneous transform T (4x4). If R (3x3xK) or T (4x4xK) represent a sequence then THETA (Kx1)is a vector of angles for corresponding elements of the sequence and V (Kx3) are the corresponding axes, one per row. Options:: ‘deg’ Return angle in degrees Notes:: - For an identity rotation matrix both THETA and V are set to zero. - The rotation angle is always in the interval [0 pi], negative rotation is handled by inverting the direction of the rotation axis. - If no output arguments are specified the result is displayed.

components.robot.common.transforms.tr2eul(tr, unit='rad', flip=False)

TR2EUL Convert homogeneous transform to Euler angles :param tr: Homogeneous transformation :param unit: ‘rad’ or ‘deg’ :param flip: True or False :return: Euler angles TR2EUL(T, OPTIONS) are the ZYZ Euler angles (1x3) corresponding to the rotational part of a homogeneous transform T (4x4). The 3 angles EUL=[PHI,THETA,PSI] correspond to sequential rotations about the Z, Y and Z axes respectively. TR2EUL(R, OPTIONS) as above but the input is an orthonormal rotation matrix R (3x3). If R (3x3xK) or T (4x4xK) represent a sequence then each row of EUL corresponds to a step of the sequence. Options:: ‘deg’ Compute angles in degrees (radians default) ‘flip’ Choose first Euler angle to be in quadrant 2 or 3. Notes:: - There is a singularity for the case where THETA=0 in which case PHI is arbitrarily set to zero and PSI is the sum (PHI+PSI). - Translation component is ignored.

components.robot.common.transforms.tr2rpy(tr, unit='rad', order='zyx')

TR2RPY Convert a homogeneous transform to roll-pitch-yaw angles :param tr: Homogeneous transformation :param unit: ‘rad’ or ‘deg’ :param order: ‘xyz’, ‘zyx’ or ‘yxz’ :return: Roll-pitch-yaw angle TR2RPY(T, options) are the roll-pitch-yaw angles (1x3) corresponding to the rotation part of a homogeneous transform T. The 3 angles RPY=[R,P,Y] correspond to sequential rotations about the Z, Y and X axes respectively. TR2RPY(R, options) as above but the input is an orthonormal rotation matrix R (3x3). If R (3x3xK) or T (4x4xK) represent a sequence then each row of RPY corresponds to a step of the sequence. Options:: ‘deg’ Compute angles in degrees (radians default) ‘xyz’ Return solution for sequential rotations about X, Y, Z axes ‘yxz’ Return solution for sequential rotations about Y, X, Z axes Notes:: - There is a singularity for the case where P=pi/2 in which case R is arbitrarily set to zero and Y is the sum (R+Y). - Translation component is ignored. - Toolbox rel 8-9 has the reverse default angle sequence as default

components.robot.common.transforms.tr2rt(t)

TR2RT Convert homogeneous transform to rotation and translation

Parameters:t – homogeneous transform matrix
Returns:Rotation and translation of the homogeneous transform

TR2RT(TR) splits a homogeneous transformation matrix (NxN) into an orthonormal rotation matrix R (MxM) and a translation vector T (Mx1), where N=M+1.

Works for TR in SE(2) or SE(3) - If TR is 4x4, then R is 3x3 and T is 3x1. - If TR is 3x3, then R is 2x2 and T is 2x1.

components.robot.common.transforms.transl(x=None, y=None, z=None)

TRANSL Create or unpack an SE(3) translational homogeneous transform

Parameters:
  • x – translation along x axes, homogeneous transform or a list of translations
  • y – translation along y axes
  • z – translation along z axes
Returns:

homogeneous transform with pure translation

Create a translational SE(3) matrix:

T = TRANSL(X, Y, Z) is an SE(3) homogeneous transform (4x4) representing a pure translation of X, Y and Z.

T = TRANSL(P) is an SE(3) homogeneous transform (4x4) representing a translation of P=[X,Y,Z]. If P (Mx3) it represents a sequence and T (4x4xM) is a sequence of homogeneous transforms such that T(:,:,i) corresponds to the i’th row of P.

Extract the translational part of an SE(3) matrix:

P = TRANSL(T) is the translational part of a homogeneous transform T as a 3-element column vector. If T (4x4xM) is a homogeneous transform sequence the rows of P (Mx3) are the translational component of the corresponding transform in the sequence.

[X,Y,Z] = TRANSL(T) is the translational part of a homogeneous transform T as three components. If T (4x4xM) is a homogeneous transform sequence then X,Y,Z (1xM) are the translational components of the corresponding transform in the sequence.

components.robot.common.transforms.transl2(x=None, y=None)

TRANSL2 Create or unpack an SE(2) translational homogeneous transform

Parameters:
  • x – x translation, homogeneous transform or a list of translations
  • y – y translation
Returns:

homogeneous transform matrix or the translation elements of a

homogeneous transform

Create a translational SE(2) matrix:

T = TRANSL2(X, Y) is an SE(2) homogeneous transform (3x3) representing a pure translation.

T = TRANSL2(P) is a homogeneous transform representing a translation or point P=[X,Y]. If P (Mx2) it represents a sequence and T (3x3xM) is a sequence of homogeneous transforms such that T(:,:,i) corresponds to the i’th row of P.

Extract the translational part of an SE(2) matrix:

P = TRANSL2(T) is the translational part of a homogeneous transform as a 2-element column vector. If T (3x3xM) is a homogeneous transform sequence the rows of P (Mx2) are the translational component of the corresponding transform in the sequence.

components.robot.common.transforms.trexp(S, theta=None)

TREXP matrix exponential for so(3) and se(3)

Parameters:
  • S – SO(3), SE(3), unit vector or twist vector
  • theta – Rotation in radians
Returns:

matrix exponential

For so(3):

TREXP(OMEGA) is the matrix exponential (3x3) of the so(3) element OMEGA that yields a rotation matrix (3x3).

TREXP(OMEGA, THETA) as above, but so(3) motion of THETA*OMEGA.

TREXP(S, THETA) as above, but rotation of THETA about the unit vector S.

TREXP(W) as above, but the so(3) value is expressed as a vector W (1x3) where W = S * THETA. Rotation by ||W|| about the vector W.

For se(3):

TREXP(SIGMA) is the matrix exponential (4x4) of the se(3) element SIGMA that yields a homogeneous transformation matrix (4x4).

TREXP(TW) as above, but the se(3) value is expressed as a twist vector TW (1x6).

TREXP(SIGMA, THETA) as above, but se(3) motion of SIGMA*THETA, the rotation part of SIGMA (4x4) must be unit norm.

TREXP(TW, THETA) as above, but se(3) motion of TW*THETA, the rotation part of TW (1x6) must be unit norm.

Notes:: - Efficient closed-form solution of the matrix exponential for arguments that are so(3) or se(3). - If theta is given then the first argument must be a unit vector or a skew-symmetric matrix from a unit vector. - Angle vector argument order is different to ANGVEC2R.

components.robot.common.transforms.trexp2(S, theta=None)

TREXP2 matrix exponential for so(2) and se(2)

:param S:S: SO(2), SE(2) or unit vector :param theta: :return: matrix exponential

R = TREXP2(OMEGA) is the matrix exponential (2x2) of the so(2) element OMEGA that yields a rotation matrix (2x2).

R = TREXP2(THETA) as above, but rotation by THETA (1x1).

SE(2):

T = TREXP2(SIGMA) is the matrix exponential (3x3) of the se(2) element SIGMA that yields a homogeneous transformation matrix (3x3).

T = TREXP2(TW) as above, but the se(2) value is expressed as a vector TW (1x3).

T = TREXP2(SIGMA, THETA) as above, but se(2) rotation of SIGMA*THETA, the rotation part of SIGMA (3x3) must be unit norm.

T = TREXP(TW, THETA) as above, but se(2) rotation of TW*THETA, the rotation part of TW must be unit norm.

Notes:: - Efficient closed-form solution of the matrix exponential for arguments that are

so(2) or se(2).
  • If theta is given then the first argument must be a unit vector or a skew-symmetric matrix from a unit vector.
components.robot.common.transforms.trlog(T)

TRLOG logarithm of SO(3) or SE(3) matrix

Parameters:T – SO(3) or SE(3) Matrix
Returns:[rotation, vector]

[theta,w] = trlog(R) as above but returns directly theta the rotation angle and w (3x1) the unit-vector indicating the rotation axis.

[theta,twist] = trlog(T) as above but returns directly theta the rotation angle and a twist vector (6x1) comprising [v w].

Notes:: - Efficient closed-form solution of the matrix logarithm for arguments that are SO(3) or SE(3). - Special cases of rotation by odd multiples of pi are handled. - Angle is always in the interval [0,pi].

components.robot.common.transforms.trot2(theta, unit='rad')

TROT2 SE2 rotation matrix

Parameters:
  • theta – rotation in radians or degrees
  • unit – “rad” or “deg” to indicate unit being used
Returns:

homogeneous transform matrix (3x3)

TROT2(THETA) is a homogeneous transformation (3x3) representing a rotation of THETA radians. TROT2(THETA, ‘deg’) as above but THETA is in degrees. Notes:: - Translational component is zero.

components.robot.common.transforms.trotx(theta, unit='rad', xyz=[0, 0, 0])

TROTX Rotation about X axis

Parameters:
  • theta – rotation in radians or degrees
  • unit – “rad” or “deg” to indicate unit being used
  • xyz – the xyz translation, if blank defaults to [0,0,0]
Returns:

homogeneous transform matrix

trotx(THETA) is a homogeneous transformation (4x4) representing a rotation of THETA radians about the x-axis. trotx(THETA, ‘deg’) as above but THETA is in degrees trotx(THETA, ‘rad’, [x,y,z]) as above with translation of [x,y,z]

components.robot.common.transforms.troty(theta, unit='rad', xyz=[0, 0, 0])

TROTY Rotation about Y axis

Parameters:
  • theta – rotation in radians or degrees
  • unit – “rad” or “deg” to indicate unit being used
  • xyz – the xyz translation, if blank defaults to [0,0,0]
Returns:

homogeneous transform matrix

troty(THETA) is a homogeneous transformation (4x4) representing a rotation of THETA radians about the y-axis. troty(THETA, ‘deg’) as above but THETA is in degrees troty(THETA, ‘rad’, [x,y,z]) as above with translation of [x,y,z]

components.robot.common.transforms.trotz(theta, unit='rad', xyz=[0, 0, 0])

TROTZ Rotation about Z axis

Parameters:
  • theta – rotation in radians or degrees
  • unit – “rad” or “deg” to indicate unit being used
  • xyz – the xyz translation, if blank defaults to [0,0,0]
Returns:

homogeneous transform matrix

trotz(THETA) is a homogeneous transformation (4x4) representing a rotation of THETA radians about the z-axis. trotz(THETA, ‘deg’) as above but THETA is in degrees trotz(THETA, ‘rad’, [x,y,z]) as above with translation of [x,y,z]

components.robot.common.transforms.unitize(v)

UNIT Unitize a vector

Parameters:v – given unit vector
Returns:a unit-vector parallel to V.

Reports error for the case where V is non-symbolic and norm(V) is zero

components.robot.common.transforms.vex(s)

VEX Convert skew-symmetric matrix to vector

:param s:skew-symmetric matrix :return: vector

VEX(S) is the vector which has the corresponding skew-symmetric matrix S. In the case that S (2x2) then V is 1x1

S = | 0 -v |
v 0 |
In the case that S (3x3) then V is 3x1.
0 -vz vy |
S = | vz 0 -vx |
|-vy vx 0 |

Notes:: - This is the inverse of the function SKEW(). - Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric. - The function takes the mean of the two elements that correspond to each unique element of the matrix.

components.robot.common.util module

Module contents