M-File Help: SE3 View code for SE3

SE3

SE(3) homogeneous transformation class

This subclasss of SE3 < SO3 < RTBPose is an object that represents an SE(3) rigid-body motion

T = se3() is an SE(3) homogeneous transformation (4x4) representing zero translation and rotation.

T = se3(x,y,z) as above represents a pure translation.

T = SE3.rx(theta) as above represents a pure rotation about the x-axis.

Constructor methods

SE3 general constructor
SE3.exp exponentiate an se(3) matrix
SE3.angvec rotation about vector
SE3.eul rotation defined by Euler angles
SE3.oa rotation defined by o- and a-vectors
SE3.rpy rotation defined by roll-pitch-yaw angles
SE3.Rx rotation about x-axis
SE3.Ry rotation about y-axis
SE3.Rz rotation about z-axis
SE3.rand random transformation
new new SE3 object

Information and test methods

dim* returns 4
isSE* returns true
issym* true if rotation matrix has symbolic elements
isidentity true for null motion
SE3.isa check if matrix is SO2

Display and print methods

plot* graphically display coordinate frame for pose
animate* graphically animate coordinate frame for pose
print* print the pose in single line format
display* print the pose in human readable matrix form
char* convert to human readable matrix as a string

Operation methods

det determinant of matrix component
eig eigenvalues of matrix component
log logarithm of rotation matrixr>=0 && r<=1ub
inv inverse
simplify* apply symbolic simplication to all elements
Ad adjoint matrix (6x6)
increment update pose based on incremental motion
interp interpolate poses
velxform compute velocity transformation
interp interpolate between poses
ctraj Cartesian motion

Conversion methods

SE3.check convert object or matrix to SE3 object
double convert to rotation matrix
R return rotation matrix
SO3 return rotation part as an SO3 object
T convert to homogeneous transformation matrix
UnitQuaternion convert to UnitQuaternion object
toangvec convert to rotation about vector form
toeul convert to Euler angles
torpy convert to roll-pitch-yaw angles
t translation column vector
tv translation column vector for vector of SE3

Compatibility methods

homtrans apply to vector
isrot* returns false
ishomog* returns true
tr2rt* convert to rotation matrix and translation vector
t2r* convert to rotation matrix
trprint* print single line representation
trplot* plot coordinate frame
tranimate* animate coordinate frame
tr2eul convert to Euler angles
tr2rpy convert to roll-pitch-yaw angles
trnorm normalize the rotation matrix
transl return translation as a row vector

* means inherited from RTBPose

Operators

+ elementwise addition, result is a matrix
- elementwise subtraction, result is a matrix
* multiplication within group, also group x vector
.* multiplication within group followed by normalization
/ multiply by inverse
./ multiply by inverse followed by normalization
== test equality
~= test inequality

Properties

n normal (x) vector
o orientation (y) vector
a approach (z) vector
t translation vector

For single SE3 objects only, for a vector of SE3 objects use the equivalent methods

t translation as a 3x1 vector (read/write)
R rotation as a 3x3 matrix (read/write)

Methods

tv return translations as a 3xN vector

Notes

See also

SO3, SE2, RTBPose


SE3.SE3

Create an SE(3) object

Constructs an SE(3) pose object that contains a 4x4 homogeneous transformation matrix.

T = SE3() is a null relative motion

T = SE3(x, y, z) is an object representing pure translation defined by x, y and z.

T = SE3(xyz) is an object representing pure translation defined by xyz (3x1). If xyz (Nx3) returns an array of SE3 objects, corresponding to the rows of xyz

T = SE3(R, xyz) is an object representing rotation defined by the orthonormal rotation matrix R (3x3) and position given by xyz (3x1)

T = SE3(T) is an object representing translation and rotation defined by the homogeneous transformation matrix T (3x3). If T (3x3xN) returns an array of SE3 objects, corresponding to the third index of T

T = SE3(T) is an object representing translation and rotation defined by the SE3 object T, effectively cloning the object. If T (Nx1) returns an array of SE3 objects, corresponding to the index of T

Options

'deg' Angle is specified in degrees

Notes


SE3.Ad

Adjoint matrix

a = S.Ad() is the adjoint matrix (6x6) corresponding to the SE(3) value S.

See also

Twist.ad


SE3.angvec

Construct an SE(3) object from angle and axis vector

R = SE3.angvec(theta, v) is an orthonormal rotation matrix (3x3) equivalent to a rotation of theta about the vector v.

Notes

See also

SO3.angvec, eul2r, rpy2r, tr2angvec


SE3.check

Convert to SE3

q = SE3.check(x) is an SE3 object where x is SE3 object or 4x4 homogeneous transformation matrix.


SE3.ctraj

Cartesian trajectory between two poses

tc = T0.ctraj(T1, n) is a Cartesian trajectory defined by a vector of SE3 objects (1xN) from pose T0 to T1, both described by SE3 objects. There are n points on the trajectory that follow a trapezoidal velocity profile along the trajectory.

tc = CTRAJ(T0, T1, s) as above but the elements of s (Nx1) specify the fractional distance along the path, and these values are in the range [0 1]. The i'th point corresponds to a distance s(i) along the path.

Notes

Reference

Robotics, Vision & Control, Sec 3.1.5, Peter Corke, Springer 2011

See also

lspb, mstraj, trinterp, ctraj, UnitQuaternion.interp


SE3.delta

SE3 object from differential motion vector

T = SE3.delta(d) is an SE3 pose object representing differential translation and rotation. The vector d=(dx, dy, dz, dRx, dRy, dRz) represents an infinitessimal motion, and is an approximation to the spatial velocity multiplied by time.

See also

SE3.todelta, SE3.increment, tr2delta


SE3.eul

Construct an SE(3) object from Euler angles

p = SE3.eul(phi, theta, psi, options) is an SE3 object equivalent to the specified Euler angles with zero translation. 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 then p is a vector (1xN) of SE3 objects.

p = SE3.eul2R(eul, options) as above but the Euler angles are taken from consecutive columns of the passed matrix eul = [phi theta psi]. If eul is a matrix (Nx3) then they are assumed to represent a trajectory then p is a vector (1xN) of SE3 objects.

Options

'deg' Compute angles in degrees (radians default)

Note

See also

SO3.eul, SE3.rpy, eul2tr, rpy2tr, tr2eul


SE3.exp

SE3 object from se(3)

SE3.exp(sigma) is the SE3 rigid-body motion given by the se(3) element sigma (4x4).

SE3.exp(exp(S) as above, but the se(3) value is expressed as a twist vector (6x1).

SE3.exp(sigma, theta) as above, but the motion is given by sigma*theta where sigma is an se(3) element (4x4) whose rotation part has a unit norm.

Notes

See also

trexp


SE3.homtrans

Apply transformation to points

P.homtrans(v) applies SE3 pose object P to the points stored columnwise in v (3xN) and returns transformed points (3xN).

Notes

See also

RTBPose.mtimes, homtrans


SE3.increment

Apply incremental motion to an SE3 pose

p1 = P.increment(d) is an SE3 pose object formed by applying the incremental motion vector d (1x6) in the frame associated with SE3 pose P.

See also

SE3.todelta, delta2tr, tr2delta


SE3.interp

Interpolate SE3 poses

P1.interp(p2, s) is an SE3 object representing an interpolation between poses represented by SE3 objects P1 and p2. s varies from 0 (P1) to 1 (p2). If s is a vector (1xN) then the result will be a vector of SO3 objects.

P1.interp(p2,n) as above but returns a vector (1xN) of SE3 objects interpolated between P1 and p2 in n steps.

Notes

See also

trinterp, UnitQuaternion


SE3.inv

Inverse of SE3 object

q = inv(p) is the inverse of the SE3 object p. p*q will be the identity matrix.

Notes


SE3.isa

Test if a homogeneous transformation

SE3.ISA(T) is true (1) if the argument T is of dimension 4x4 or 4x4xN, else false (0).

SE3.ISA(T, 'valid') as above, but also checks the validity of the rotation sub-matrix.

Notes

See also

SO3.ISA, SE2.ISA, SO2.ISA


SE3.isidentity

Apply incremental motion to an SE(3) pose

P.isidentity() is true of the SE3 object P corresponds to null motion, that is, its homogeneous transformation matrix is identity.


SE3.log

Lie algebra

se3 = P.log() is the Lie algebra expressed as an augmented skew-symmetric matrix (4x4) corresponding to the SE3 object P.

See also

SE3.logs, SE3.Twist, trlog, logm


SE3.logs

Lie algebra

se3 = P.log() is the Lie algebra expressed as vector (1x6) corresponding to the SE2 object P. The vector comprises the translational elements followed by the unique elements of the skew-symmetric rotation submatrix.

See also

SE3.log, SE3.Twist, trlog, logm


SE3.new

Construct a new object of the same type

p2 = P.new(x) creates a new object of the same type as P, by invoking the SE3 constructor on the matrix x (4x4).

p2 = P.new() as above but defines a null motion.

Notes

See also

SO3.new, SO2.new, SE2.new


SE3.oa

Construct an SE(3) object from orientation and approach vectors

p = SE3.oa(o, a) is an SE3 object for the specified orientation and approach vectors (3x1) formed from 3 vectors such that R = [N o a] and N = o x a, with zero translation.

Notes

References

See also

rpy2r, eul2r, oa2tr, SO3.oa


SE3.rand

Construct a random SE(3) object

SE3.rand() is an SE3 object with a uniform random translation and a uniform random RPY/ZYX orientation. Random numbers are in the interval 0 to 1.

See also

rand


SE3.rpy

Construct an SE(3) object from roll-pitch-yaw angles

p = SE3.rpy(roll, pitch, yaw, options) is an SE3 object equivalent to the specified roll, pitch, yaw angles angles with zero translation. 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 then p is a vector (1xN) of SE3 objects.

p = SE3.rpy(rpy, options) as above but the roll, pitch, yaw angles angles angles are taken from consecutive columns of the passed matrix rpy = [roll, pitch, yaw]. If rpy is a matrix (Nx3) then they are assumed to represent a trajectory and p is a vector (1xN) of SE3 objects.

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)

See also

SO3.rpy, SE3.eul, tr2rpy, eul2tr


SE3.Rx

Rotation about X axis

p = SE3.Rx(theta) is an SE3 object representing a rotation of theta radians about the x-axis.

p = SE3.Rx(theta, 'deg') as above but theta is in degrees.

See also

SE3.Ry, SE3.Rz, rotx


SE3.Ry

Rotation about Y axis

p = SE3.Ry(theta) is an SE3 object representing a rotation of theta radians about the y-axis.

p = SE3.Ry(theta, 'deg') as above but theta is in degrees.

See also

SE3.Ry, SE3.Rz, rotx


SE3.Rz

Rotation about Z axis

p = SE3.Rz(theta) is an SE3 object representing a rotation of theta radians about the z-axis.

p = SE3.Rz(theta, 'deg') as above but theta is in degrees.

See also

SE3.Ry, SE3.Rz, rotx


SE3.set.t

Get translation vector

T = P.t is the translational part of SE3 object as a 3-element column vector.

Notes

See also

SE3.transl, transl


SE3.SO3

Convert rotational component to SO3 object

P.SO3 is an SO3 object representing the rotational component of the SE3 pose P. If P is a vector (Nx1) then the result is a vector (Nx1).


SE3.T

Get homogeneous transformation matrix

T = P.T() is the homogeneous transformation matrix (3x3) associated with the SO2 object P, and has zero translational component. If P is a vector (1xN) then T (3x3xN) is a stack of rotation matrices, with the third dimension corresponding to the index of P.

See also

SO2.T


SE3.toangvec

Convert to angle-vector form

[theta,v] = P.toangvec(options) is rotation expressed in terms of an angle theta (1x1) about the axis v (1x3) equivalent to the rotational part of the SE3 object P.

If P is a vector (1xN) then theta (Kx1) is a vector of angles for corresponding elements of the vector and v (Kx3) are the corresponding axes, one per row.

Options

'deg' Return angle in degrees

Notes

See also

angvec2r, angvec2tr, trlog


SE3.todelta

Convert SE(3) object to differential motion vector

d = SE3.todelta(p0, p1) is the (6x1) differential motion vector (dx, dy, dz, dRx, dRy, dRz) corresponding to infinitessimal motion (in the p0 frame) from SE(3) pose p0 to p1. .

d = SE3.todelta(p) as above but the motion is with respect to the world frame.

Notes

See also

SE3.increment, tr2delta, delta2tr


SE3.toeul

Convert to Euler angles

eul = P.toeul(options) are the ZYZ Euler angles (1x3) corresponding to the rotational part of the SE3 object P. The 3 angles eul=[PHI,THETA,PSI] correspond to sequential rotations about the Z, Y and Z axes respectively.

If P is a vector (1xN) then each row of eul corresponds to an element of the vector.

Options

'deg' Compute angles in degrees (radians default)
'flip' Choose first Euler angle to be in quadrant 2 or 3.

Notes

See also

SO3.toeul, SE3.torpy, eul2tr, tr2rpy


SE3.torpy

Convert to roll-pitch-yaw angles

rpy = P.torpy(options) are the roll-pitch-yaw angles (1x3) corresponding to the rotational part of the SE3 object P. The 3 angles rpy=[R,P,Y] correspond to sequential rotations about the Z, Y and X axes respectively.

If P is a vector (1xN) then each row of rpy corresponds to an element of the vector.

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

See also

SE3.torpy, SE3.toeul, rpy2tr, tr2eul


SE3.transl

Get translation vector

T = P.transl() is the translational part of SE3 object as a 3-element row vector. If P is a vector (1xN) then

the rows of T (Mx3) are the translational component of the

corresponding pose in the sequence.

[x,y,z] = P.transl() as above but the translational part is returned as three components. If P is a vector (1xN) then x,y,z (1xN) are the translational components of the corresponding pose in the sequence.

Notes

See also

SE3.t, transl


SE3.tv

Return translation for a vector of SE3 objects

P.tv is a column vector (3x1) representing the translational part of the SE3 pose object P. If P is a vector of SE3 objects (Nx1) then the result is a matrix (3xN) with columns corresponding to the elements of P.

See also

SE3.t


SE3.Twist

Convert to Twist object

tw = P.Twist() is the equivalent Twist object. The elements of the twist are the unique elements of the Lie algebra of the SE3 object P.

See also

SE3.logs, Twist


SE3.velxform

Velocity transformation

Transform velocity between frames. A is the world frame, B is the body frame and C is another frame attached to the body. PAB is the pose of the body frame with respect to the world frame, PCB is the pose of the body frame with respect to frame C.

J = PAB.velxform() is a 6x6 Jacobian matrix that maps velocity from frame B to frame A.

J = PCB.velxform('samebody') is a 6x6 Jacobian matrix that maps velocity from frame C to frame B. This is also the adjoint of PCB.


 

© 1990-2014 Peter Corke.