Robot manipulator blocks

class FKine(robot=None, args={}, **blockargs)

Bases: FunctionBlock

FKINE

Robot arm forward kinematics.

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(N)	\(\mathit{q}\)
Output	0	SE3	\(\mathbf{T}\)

Compute the end-effector pose as an SE(3) object as a function of the input joint configuration.

See also

fkine()

__init__(robot=None, args={}, **blockargs)

Parameters:

• *inputs (Block or Plug) – Optional incoming connections

• robot (Any) – Robot model, defaults to None

• args (Any) – Options for fkine, defaults to {}

• blockargs (Any) – common Block options

class IKine(robot=None, q0=None, useprevious=True, ik=None, args={}, seed=None, **blockargs)

Bases: FunctionBlock

IKINE

Robot arm inverse kinematics.

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	SE3	\(\mathbf{T}\)
Output	0	ndarray(N)	\(\mathit{q}\)

Compute joint configuration required to achieve end-effector pose input as an SE(3) object.

Note:

The solution may not exist and is not unique. The solution will depend on the initial joint configuration q0.

See also

ik_LM()

__init__(robot=None, q0=None, useprevious=True, ik=None, args={}, seed=None, **blockargs)

Parameters:

• robot (Any) – Robot model, defaults to None

• q0 (Any) – Initial joint angles, defaults to None

• useprevious (bool) – Use previous IK solution as q0, defaults to True

• ik (Any) – Specify an IK function, defaults to “LM”

• args (Any) – Options passed to IK function

• seed (Any) – random seed for solution

• blockargs (Any) – common Block options

class Jacobian(robot, frame='0', representation=None, inverse=False, pinv=False, damping=None, transpose=False, **blockargs)

Bases: FunctionBlock

JACOBIAN

Robot arm Jacobian matrix.

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(N)	\(\mathit{q}\)
Output	0	ndarray(N,N)	\(\mathbf{J}\)

Compute the Jacobian matrix as a function of the input joint configuration. The Jacobian can be computed in the world or end-effector frame, for spatial or analytical velocity, and its inverse, damped inverse or transpose can be returned.

See also

jacob0() jacobe() jacob0_analytical()

__init__(robot, frame='0', representation=None, inverse=False, pinv=False, damping=None, transpose=False, **blockargs)

Parameters:

• robot (Any) – Robot model

• frame (str) – Frame to compute Jacobian for, one of: “0” [default], “e”

• representation (Any) – representation for analytical Jacobian

• inverse (bool) – output inverse of Jacobian, defaults to False

• pinv (bool) – output pseudo-inverse of Jacobian, defaults to False

• damping (Any) – damping term for inverse, defaults to None

• transpose (bool) – output transpose of Jacobian, defaults to False

• blockargs (Any) – common Block options

If an inverse is requested and damping is not None it is added to the diagonal of the Jacobian prior to the inversion. If a scalar is provided it is added to each element of the diagonal, otherwise an N-vector is assumed.

Note

• Only one of inverse or pinv can be True

• inverse or pinv can be used in conjunction with transpose

• inverse requires that the Jacobian is square

• If inverse is True and the Jacobian is singular a runtime error will occur.

class ArmPlot(robot=None, q0=None, backend=None, **blockargs)

Bases: GraphicsBlock

ARMPLOT

Plot robot arm.

Inputs:

1 [ndarray(N)]

Outputs:

0

States:

0

Inputs:

1

Outputs:

0

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(N)	\(\mathit{q}\), joint configuration

Create a robot animation using the robot’s plot method.

See also

plot()

PLOT3D = True

__init__(robot=None, q0=None, backend=None, **blockargs)

Parameters:

• robot (Any) – Robot model

• q0 (Any) – initial joint angles, defaults to None

• backend (Any) – RTB backend name, defaults to ‘pyplot’

• blockargs (Any) – common GraphicsBlock options

class JTraj(q0, qf, qd0=None, qdf=None, T=None, **blockargs)

Bases: SourceBlock

JTRAJ

Joint-space trajectory

Inputs:

0

Outputs:

3

States:

0

Port type	Port number	Types	Description
Output	0	ndarray	\(q(s)\)
Output	1	ndarray	\(\dot{q}(s)\)
Output	2	ndarray	\(\ddot{q}(s)\)

Outputs a joint space trajectory where the joint coordinates vary from q0 to qf over the course of the simulation. A quintic (5th order) polynomial is used with default zero boundary conditions for velocity and acceleration.

See also

ctraj() xplot jtraj()

__init__(q0, qf, qd0=None, qdf=None, T=None, **blockargs)

Parameters:

• q0 (Any) – initial joint coordinate

• qf (Any) – final joint coordinate

• T (Any) – time vector or number of steps, defaults to None

• qd0 (Any) – initial velocity, defaults to None

• qdf (Any) – final velocity, defaults to None

• blockargs (Any) – common Block options

coeffs: Any

class CTraj(T1, T2, T, trapezoidal=True, **blockargs)

Bases: SourceBlock

CTRAJ

Task space trajectory

Inputs:

0

Outputs:

1

States:

0

Port type	Port number	Types	Description
Output	0	SE3	\(\mathbf{T}(t)\)

The block outputs a pose that varies smoothly from T1 to T2 over the course of T seconds.

If T is not given it defaults to the simulation time.

If trapezoidal is True then a trapezoidal motion profile is used along the path to provide initial acceleration and final deceleration. Otherwise, motion is at constant velocity.

See also

interp() ctraj() xplot jtraj()

__init__(T1, T2, T, trapezoidal=True, **blockargs)

Parameters:

• T1 (Any) – initial pose

• T2 (Any) – final pose

• T (Any) – motion time

• trapezoidal (bool) – Use LSPB motion profile along the path

• blockargs (Any) – common Block options

class CirclePath(radius=1, centre=(0, 0, 0), pose=None, frequency=1, unit='rps', phase=None, **blockargs)

Bases: SourceBlock

CIRCLEPATH

Circular motion.

Inputs:

0 or 1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Output	0	ndarray(3) or SE3	\(\mathit{p}(t)\) or \(\mathbf{T}(t)\)

The block outputs the coordinates of a point moving in a circle of radius r centred at centre and parallel to the xy-plane.

By default the output is a 3-vector \((x, y, z)\) but if pose is an SE3 instance the output is a copy of that pose with its translation set to the coordinate of the moving point. This is the motion of a frame with fixed orientation following a circular path.

__init__(radius=1, centre=(0, 0, 0), pose=None, frequency=1, unit='rps', phase=None, **blockargs)

Parameters:

• radius (float) – radius of circle, defaults to 1

• centre (Any) – center of circle, defaults to [0,0,0]

• pose (Any) – SE3 pose of output, defaults to None

• frequency (float) – rotational frequency, defaults to 1

• unit (str) – unit for frequency, one of: ‘rps’ [default], ‘rad’

• phase (float | None) – phase

• blockargs (Any) – common Block options

class Trapezoidal(q0, qf, V=None, T=None, **blockargs)

Bases: SourceBlock

Trapezoidal

Trapezoidal scalar trajectory

Inputs:

0

Outputs:

3

States:

0

Port type	Port number	Types	Description
Output	0	float	\(q(t)\)
Output	1	float	\(\dot{q}(t)\)
Output	2	float	\(\ddot{q}(t)\)

Scalar trapezoidal trajectory that varies from q0 to qf over the simulation period.

Changed in version 1.5.0: The outputs are scalars, as the table says. They were arrays with one element, because trapezoidal_func returned arrays for a scalar time.

See also

ctraj, xplot, jtraj()

__init__(q0, qf, V=None, T=None, **blockargs)

Compute a joint-space trajectory

Parameters:

• q0 (Any) – initial joint coordinate

• qf (Any) – final joint coordinate

• T (Any) – maximum time, defaults to None

• blockargs (Any) – common Block options

If T is given the value qf is reached at this time. This can be less or greater than the simulation time.

class Traj(y0=0, yf=1, T=None, time=False, traj='trapezoidal', **blockargs)

Bases: FunctionBlock

TRAJ

Vector trajectory

Inputs:

0 or 1

Outputs:

3

States:

0

Port type	Port number	Types	Description
Input	0	float	\(s \in [0, 1]\) distance along trajectory.
Output	0	ndarray	\(y(s)\)
Output	1	ndarray	\(\dot{y}(s)\)
Output	2	ndarray	\(\ddot{y}(s)\)

Generates a vector trajectory using a trapezoidal or quintic polynomial profile that varies from y0 to yf

The distance along the trajectory is either:

• a linear function from 0 to T or maximum simulation time if time is True, or

• the value [0, 1] given on inport port if time is False

Changed in version 1.5.0: An unknown traj raises ValueError when the block is created. It previously failed with an UnboundLocalError when the simulation started. The outputs are unchanged.

See also

spatialmath.base.mtraj()

__init__(y0=0, yf=1, T=None, time=False, traj='trapezoidal', **blockargs)

Parameters:

• y0 (ndarray | Sequence[float] | float) – initial value, defaults to 0

• yf (ndarray | Sequence[float] | float) – final value, defaults to 1

• T (Any) – maximum time, defaults to None

• time (bool) – if True trajectory is based on simulation time, else based on input 0. Defaults to False

• traj (str) – trajectory type, one of: ‘trapezoidal’ [default], ‘quintic’

• blockargs (Any) – common Block options

class IDyn(robot, gravity=None, **blockargs)

Bases: FunctionBlock

IDYN

Robot arm inverse dynamics model.

Inputs:

3

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(N)	\(\mathit{q}\), joint configuration
Input	1	ndarray(N)	\(\dot{\mathit{q}}\), joint velocity
Input	2	ndarray(N)	\(\ddot{\mathit{q}}\), joint acceleration
Output	0	ndarray(N)	\(\mathit{Q}\), generalized joint force

Compute the generalized joint torques required to achieve the input joint configuration, velocity and acceleration. This uses the recursive Newton-Euler (RNE) algorithm.

See also

rne()

__init__(robot, gravity=None, **blockargs)

Parameters:

• robot (Any) – Robot model

• gravity (Any) – gravitational acceleration in the world frame, downwards gravitational force is equivalent to robot base acceleration upwards (positive)

• blockargs (Any) – common Block options

class Gravload(robot, gravity=None, **blockargs)

Bases: FunctionBlock

GRAVLOAD

Robot arm gravity load.

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(N)	\(\mathit{q}\), joint configuration
Output	0	ndarray(N)	\(\mathit{g}\), generalized joint force

Compute generalized joint forces due to gravity for the input joint configuration.

See also

gravload()

__init__(robot, gravity=None, **blockargs)

Parameters:

• robot (Any) – Robot model

• gravity (Any) – gravitational acceleration in the world frame, downwards gravitational force is equivalent to robot base acceleration upwards (positive)

• blockargs (Any) – common Block options

class Gravload_X(robot, representation='rpy/xyz', gravity=None, **blockargs)

Bases: FunctionBlock

GRAVLOAD_X

Task-space robot arm gravity wrench.

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(6)	\(\mathit{x}\), end-effector pose
Output	0	ndarray(6)	\(\mathit{g}_x\), generalized joint force

Compute end-effector wrench due to gravity for the input end-effector pose.

See also

gravload_x() gravload()

__init__(robot, representation='rpy/xyz', gravity=None, **blockargs)

Parameters:

• robot (Any) – Robot model

• representation (str) – task-space representation, defaults to “rpy/xyz”

• gravity (Any) – gravitational acceleration in the world frame, downwards gravitational force is equivalent to robot base acceleration upwards (positive)

• blockargs (Any) – common Block options

class Inertia(robot, **blockargs)

Bases: FunctionBlock

INERTIA

Robot arm inertia matrix.

Inputs:

1 [ndarray(N)]

Outputs:

3 [ndarray(N,N)]

States:

0

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray	\(\mathit{q}\), joint configuration
Output	0	ndarray(N,N)	\(\mathbf{M}\), mass matrix

Joint-space inertia matrix (mass matrix) as a function of joint configuration.

See also

inertia()

__init__(robot, **blockargs)

Parameters:

• robot (Any) – Robot model

• blockargs (Any) – common Block options

class Inertia_X(robot, representation='rpy/xyz', pinv=False, **blockargs)

Bases: FunctionBlock

INERTIA_X

Task-space robot arm inertia matrix.

Inputs:

1

Outputs:

1

States:

0

Port type	Port number	Types	Description
Input	0	ndarray(6)	\(\mathit{x}\), end-effector pose
Output	0	ndarray(6,6)	\(\mathbf{M_x}\), task-space mass matrix

Task-space inertia matrix as a function of end-effector pose.

See also

inertia_x()

__init__(robot, representation='rpy/xyz', pinv=False, **blockargs)

Parameters:

• robot (Any) – Robot model

• representation (str) – task-space representation, defaults to “rpy/xyz”

• blockargs (Any) – common Block options

class FDyn(robot, q0=None, **blockargs)

Bases: ContinuousBlock

FDYN

Robot arm forward dynamics.

Inputs:

1

Outputs:

3

States:

2N

Port type	Port number	Types	Description
Input	0	ndarray(N)	\(\mathit{Q}\), generalized joint force
Output	0	ndarray(N)	\(\mathit{q}\), joint configuration
Output	1	ndarray(N)	\(\dot{\mathit{q}}\), joint velocity
Output	2	ndarray(N)	\(\ddot{\mathit{q}}\), joint acceleration

Compute the manipulator arm forward dynamics in joint space, the joint acceleration for the input configuration and applied joint forces. The acceleration is integrated to obtain joint velocity and joint configuration.

See also

fdyn()

__init__(robot, q0=None, **blockargs)

Parameters:

• robot (Any) – Robot model

• q0 (Any) – Initial joint configuration

• blockargs (Any) – common Block options

class FDyn_X(robot, q0=None, gravcomp=False, velcomp=False, representation='rpy/xyz', **blockargs)

Bases: ContinuousBlock

FDYN_X

Task-space robot arm forward dynamics.

Inputs:

1

Outputs:

3

States:

12

Port type	Port number	Types	Description
Input	0	ndarray(6)	\(\mathit{\tau}\), end-effector wrench
Output	0	ndarray(6)	\(\mathit{x}\), end-effector pose
Output	1	ndarray(6)	\(\dot{\mathit{x}}\), end-effector velocity
Output	2	ndarray(6)	\(\dot{\mathit{x}}`\), end-effector acceleration

Compute the manipulator arm forward dynamics in task space, the end-effector acceleration for the input end-effector pose and applied end-effector wrench. The acceleration is integrated to obtain task-space velocity and task-space pose.

See also

fdyn_x

__init__(robot, q0=None, gravcomp=False, velcomp=False, representation='rpy/xyz', **blockargs)

Parameters:

• robot (Any) – Robot model

• q0 (Any) – Initial joint configuration

• gravcomp (bool) – perform gravity compensation

• velcomp (bool) – perform velocity term compensation

• representation (str) – task-space representation, defaults to “rpy/xyz”

• blockargs (Any) – common Block options