import numpy as np
from math import sin, cos, atan2, tan, sqrt, pi
import matplotlib.pyplot as plt
import time
from spatialmath import base, SE3
from bdsim.components import TransferBlock, FunctionBlock, SourceBlock
from bdsim.graphics import GraphicsBlock
from roboticstoolbox import tpoly_func, lspb_func
"""
Robot blocks:
- have inputs and outputs
- are a subclass of ``FunctionBlock`` |rarr| ``Block`` for kinematics and have no states
- are a subclass of ``TransferBlock`` |rarr| ``Block`` for dynamics and have states
"""
# The constructor of each class ``MyClass`` with a ``@block`` decorator becomes a method ``MYCLASS()`` of the BlockDiagram instance.
# ------------------------------------------------------------------------ #
[docs]class FKine(FunctionBlock):
"""
:blockname:`FKINE`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| ndarray | SE3 | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('q',)
outlabels = ('T',)
[docs] def __init__(self, robot=None, args={}, **blockargs):
"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param robot: Robot model, defaults to None
:type robot: Robot subclass, optional
:param args: Options for fkine, defaults to {}
:type args: dict, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a FORWARD_KINEMATICS block
:rtype: Foward_Kinematics instance
Robot arm forward kinematic model.
**Block ports**
:input q: Joint configuration vector as an ndarray.
:output T: End-effector pose as an SE(3) object
"""
super().__init__(inputs=inputs, **blockargs)
self.type = "forward-kinematics"
self.robot = robot
self.args = args
self.inport_names(("q",))
self.outport_names(("T",))
def output(self, t=None):
return [self.robot.fkine(self.inputs[0], **self.args)]
[docs]class IKine(FunctionBlock):
"""
:blockname:`IKINE`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| SE3 | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('T',)
outlabels = ('q',)
[docs] def __init__(
self, robot=None, q0=None, useprevious=True, ik=None, **blockargs
):
"""
:param robot: Robot model, defaults to None
:type robot: Robot subclass, optional
:param q0: Initial joint angles, defaults to None
:type q0: array_like(n), optional
:param useprevious: Use previous IK solution as q0, defaults to True
:type useprevious: bool, optional
:param ik: Specify an IK function, defaults to 'ikine_LM'
:type ik: callable f(T)
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an INVERSE_KINEMATICS block
:rtype: Inverse_Kinematics instance
Robot arm inverse kinematic model.
The block has one input port:
1. End-effector pose as an SE(3) object
and one output port:
1. Joint configuration vector as an ndarray.
"""
super().__init__(inputs=inputs, **blockargs)
self.type = "inverse-kinematics"
self.robot = robot
self.q0 = q0
self.qprev = q0
self.useprevious = useprevious
self.ik = None
self.inport_names(("T",))
self.outport_names(("q",))
def start(self):
super().start()
if self.useprevious:
self.qprev = self.q0
def output(self, t=None):
if self.useprevious:
q0 = self.qprev
else:
q0 = self.q0
if self.ik is None:
sol = self.robot.ikine_LM(self.inputs[0], q0=q0)
else:
sol = self.ik(self.inputs[0])
if not sol.success:
raise RuntimeError("inverse kinematic failure for pose", self.inputs[0])
if self.useprevious:
self.qprev = sol.q
return [sol.q]
# ------------------------------------------------------------------------ #
[docs]class Jacobian(FunctionBlock):
"""
:blockname:`JACOBIAN`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| ndarray | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('q',)
outlabels = ('J',)
[docs] def __init__(
self,
robot,
frame="0",
inverse=False,
pinv=False,
transpose=False,
**blockargs
):
"""
:param robot: Robot model
:type robot: Robot subclass
:param frame: Frame to compute Jacobian for, one of: '0' [default], 'e'
:type frame: str, optional
:param inverse: output inverse of Jacobian, defaults to False
:type inverse: bool, optional
:param pinv: output pseudo-inverse of Jacobian, defaults to False
:type pinv: bool, optional
:param transpose: output transpose of Jacobian, defaults to False
:type transpose: bool, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a JACOBIAN block
:rtype: Jacobian instance
Robot arm Jacobian.
The block has one input port:
1. Joint configuration vector as an ndarray.
and one output port:
1. Jacobian matrix as an ndarray(6,n)
.. notes::
- 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.
"""
super().__init__(inputs=inputs, **blockargs)
self.robot = robot
if frame == "0":
self.jfunc = robot.jacob0
elif frame == "e":
self.jfunc = robot.jacobe
else:
raise ValueError("unknown frame")
if inverse and robot.n != 6:
raise ValueError("cannot invert a non square Jacobian")
if inverse and pinv:
raise ValueError("can only set one of inverse and pinv")
self.inverse = inverse
self.pinv = pinv
self.transpose = transpose
self.inport_names(("q",))
self.outport_names(("J",))
def output(self, t=None):
J = self.jfunc(self.inputs[0])
if self.inverse:
J = np.linalg.inv(J)
if self.pinv:
J = np.linalg.pinv(J)
if self.transpose:
J = J.T
return [J]
[docs]class Tr2Delta(FunctionBlock):
"""
:blockname:`TR2DELTA`
.. table::
:align: left
+------------+------------+---------+
| inputs | outputs | states |
+------------+------------+---------+
| 2 | 1 | 0 |
+------------+------------+---------+
| SE3, SE3 | ndarray(6) | |
+------------+------------+---------+
"""
nin = 2
nout = 1
inlabels = ('T1', 'T2')
outlabels = ('Δ',)
[docs] def __init__(self, **blockargs):
"""
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a TR2DELTA block
:rtype: Tr2Delta instance
Difference between T1 and T2 as a 6-vector
The block has two input port:
1. T1 as an SE3.
2. T2 as an SE3.
and one output port:
1. delta as an ndarray(6,n)
:seealso: :func:`spatialmath.base.tr2delta`
"""
super().__init__(inputs=inputs, **blockargs)
self.inport_names(("T1", "T2"))
self.outport_names(("$\delta$",))
def output(self, t=None):
return [base.tr2delta(self.inputs[0].A, self.inputs[1].A)]
# ------------------------------------------------------------------------ #
[docs]class Delta2Tr(FunctionBlock):
"""
:blockname:`DELTA2TR`
.. table::
:align: left
+------------+----------+---------+
| inputs | outputs | states |
+------------+----------+---------+
| 1 | 1 | 0 |
+------------+----------+---------+
| ndarray(6) | SE3 | |
+------------+----------+---------+
"""
nin = 1
nout = 1
outlabels = ('T',)
inlabels = ('Δ',)
[docs] def __init__(self, **blockargs):
"""
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a DELTA2TR block
:rtype: Delta2Tr instance
Delta to SE(3)
The block has one input port:
1. delta as an ndarray(6,n)
and one output port:
1. T as an SE3
:seealso: :func:`spatialmath.base.delta2tr`
"""
super().__init__(inputs=inputs, **blockargs)
self.inport_names(("$\delta$",))
self.outport_names(("T",))
def output(self, t=None):
return [SE3.Delta(self.inputs[0])]
# ------------------------------------------------------------------------ #
[docs]class Point2Tr(FunctionBlock):
"""
:blockname:`POINT2TR`
.. table::
:align: left
+------------+----------+---------+
| inputs | outputs | states |
+------------+----------+---------+
| 1 | 1 | 0 |
+------------+----------+---------+
| ndarray(3) | SE3 | |
+------------+----------+---------+
"""
nin = 1
nout = 1
[docs] def __init__(self, T, **blockargs):
"""
:param T: the transform
:type T: SE3
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a POINT2TR block
:rtype: Point2Tr instance
The block has one input port:
1. a 3D point as an ndarray(3)
and one output port:
1. T as an SE3 with its position part replaced by the input
:seealso: :func:`spatialmath.base.delta2tr`
"""
super().__init__(inputs=inputs, **blockargs)
self.inport_names(("t",))
self.outport_names(("T",))
self.pose = T
def output(self, t=None):
T = SE3.SO3(self.pose.R, t=self.inputs[0])
return [T]
# ------------------------------------------------------------------------ #
[docs]class TR2T(FunctionBlock):
"""
:blockname:`TR2T`
.. table::
:align: left
+------------+----------+---------+
| inputs | outputs | states |
+------------+----------+---------+
| 1 | 3 | 0 |
+------------+----------+---------+
| SE3 | float | |
+------------+----------+---------+
"""
nin = 1
nout = 3
inlabels = ('T',)
outlabels = ('x', 'y', 'z')
[docs] def __init__(self, **blockargs):
"""
:param T: the transform
:type T: SE3
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a POINT2TR block
:rtype: Point2Tr instance
The block has one input port:
1. a 3D point as an ndarray(3)
and one output port:
1. T as an SE3 with its position part replaced by the input
:seealso: :func:`spatialmath.base.delta2tr`
"""
super().__init__(inputs=inputs, **blockargs)
self.inport_names(("T",))
self.outport_names(("x", "y", "z"))
def output(self, t=None):
t = self.inputs[0].t
return list(t)
# ------------------------------------------------------------------------ #
[docs]class FDyn(TransferBlock):
"""
:blockname:`FDYN`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 3 | 0 |
+------------+---------+---------+
| ndarray | ndarray,| |
| | ndarray,| |
| | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 3
outlabels = ('q', 'qd', 'qdd')
inlabels = ('τ')
[docs] def __init__(self, robot, q0=None, **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param q0: Initial joint configuration
:type q0: array_like(n)
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a FORWARD_DYNAMICS block
:rtype: Foward_Dynamics instance
Robot arm forward dynamics model.
The block has one input port:
1. Joint force/torque as an ndarray.
and three output ports:
1. joint configuration
2. joint velocity
3. joint acceleration
"""
super().__init__(**blockargs)
self.type = "forward-dynamics"
self.robot = robot
self.nstates = robot.n * 2
# state vector is [q qd]
self.inport_names(("$\tau$",))
self.outport_names(("q", "qd", "qdd"))
if q0 is None:
q0 = np.zeros((robot.n,))
else:
q0 = base.getvector(q0, robot.n)
self._x0 = np.r_[q0, np.zeros((robot.n,))]
self._qdd = None
def output(self, t=None):
n = self.robot.n
q = self._x[:n]
qd = self._x[n:]
qdd = self._qdd # from last deriv
return [q, qd, qdd]
def deriv(self):
# return [qd qdd]
Q = self.inputs[0]
n = self.robot.n
assert len(Q) == n, "torque vector wrong size"
q = self._x[:n]
qd = self._x[n:]
qdd = self.robot.accel(q, qd, Q)
self._qdd = qdd
return np.r_[qd, qdd]
[docs]class IDyn(FunctionBlock):
"""
:blockname:`IDYN`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 3 | 1 | 0 |
+------------+---------+---------+
| ndarray, | ndarray | |
| ndarray, | | |
| ndarray | | |
+------------+---------+---------+
"""
nin = 3
nout = 1
inlabels = ('q', 'qd', 'qdd')
outlabels = ('τ')
[docs] def __init__(self, robot, gravity=None, **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param gravity: gravitational acceleration
:type gravity: float
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an INVERSE_DYNAMICS block
:rtype: Inverse_Dynamics instance
Robot arm forward dynamics model.
The block has three input port:
1. Joint configuration vector as an ndarray.
2. Joint velocity vector as an ndarray.
3. Joint acceleration vector as an ndarray.
and one output port:
1. joint torque/force
.. TODO:: end-effector wrench input, base wrench output, payload input
"""
super().__init__(inputs=inputs, **blockargs)
self.type = "inverse-dynamics"
self.robot = robot
self.gravity = gravity
# state vector is [q qd]
self.inport_names(("q", "qd", "qdd"))
self.outport_names(("$\tau$",))
def output(self, t=None):
tau = self.robot.rne(self.inputs[0], self.inputs[1], self.inputs[2], gravity=gravity)
return [tau]
[docs]class Gravload(FunctionBlock):
"""
:blockname:`GRAVLOAD`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| ndarray | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('q',)
outlabels = ('τ')
[docs] def __init__(self, robot, gravity=None, **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param gravity: gravitational acceleration
:type gravity: float
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a GRAVLOAD block
:rtype: Gravload instance
Robot arm gravity torque.
The block has one input port:
1. Joint configuration vector as an ndarray.
and one output port:
1. joint torque/force due to gravity
"""
super().__init__(**blockargs)
self.type = "gravload"
self.robot = robots
self.gravity = gravity
self.inport_names(("q",))
self.outport_names(("$\tau$",))
def output(self, t=None):
tau = self.robot.gravload(self.inputs[0], gravity=self.gravity)
return [tau]
[docs]class Gravload_X(FunctionBlock):
"""
:blockname:`GRAVLOAD_X`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| ndarray | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('q',)
outlabels = ('w')
[docs] def __init__(self, robot, gravity=None, **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param gravity: gravitational acceleration
:type gravity: float
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a GRAVLOAD block
:rtype: Gravload instance
Robot arm gravity torque.
The block has one input port:
1. Joint configuration vector as an ndarray.
and one output port:
1. joint torque/force due to gravity
"""
super().__init__(**blockargs)
self.type = "gravload-x"
self.robot = robots
self.gravity = gravity
self.inport_names(("q",))
self.outport_names(("$\tau$",))
def output(self, t=None):
q = self.inputs[0]
tau = self.robot.gravload(q, gravity=self.gravity)
J = self.robot.jacob0(q)
if J.shape[0] == J.shape[1]:
w = np.linalg.inv(J).T * tau
else:
w = np.linalg.pinv(J).T * tau
return [w]
[docs]class Inertia(FunctionBlock):
"""
:blockname:`INERTIA`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| ndarray | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('q',)
outlabels = ('M')
[docs] def __init__(self, robot, gravity=None, **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an INERTIA block
:rtype: Inertia instance
Robot arm inertia matrix.
The block has one input port:
1. Joint configuration vector as an ndarray.
and one output port:
1. Joint-space inertia matrix :math:`\mat{M}(q)`
"""
super().__init__(**blockargs)
self.type = "inertia"
self.robot = robots
self.inport_names(("q",))
self.outport_names(("M",))
def output(self, t=None):
M = self.robot.inertia(self.inputs[0])
return [M]
[docs]class Inertia_X(FunctionBlock):
"""
:blockname:`INERTIA_X`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 1 | 0 |
+------------+---------+---------+
| ndarray | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 1
inlabels = ('q',)
outlabels = ('M')
[docs] def __init__(self, robot, representation=None, pinv=False, **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: an INERTIA_X block
:rtype: Inertia_X instance
Robot arm task-space inertia matrix.
The block has one input port:
1. Joint configuration vector as an ndarray.
and one output port:
1. Task-space inertia matrix :math:`\mat{M}_x(q)`
"""
super().__init__(**blockargs)
self.type = "inertia-x"
self.robot = robot
self.representation = representation
self.pinv = pinv
self.inport_names(("q",))
self.outport_names(("M",))
def output(self, t=None):
q = self.inputs[0]
Mx = self.robot.inertia_x(q, pinv=self.pinv, representation=self.representation)
return [Mx]
# ------------------------------------------------------------------------ #
[docs]class FDyn_X(TransferBlock):
"""
:blockname:`FDYN_X`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 1 | 3 | 0 |
+------------+---------+---------+
| ndarray | ndarray,| |
| | ndarray,| |
| | ndarray | |
+------------+---------+---------+
"""
nin = 1
nout = 5
outlabels = ('q', 'qd', 'x', 'xd', 'xdd')
inlabels = ('w')
[docs] def __init__(self, robot, q0=None, gravcomp=False, velcomp=False, representation='rpy/xyz', **blockargs):
"""
:param robot: Robot model
:type robot: Robot subclass
:param end: Link to compute pose of, defaults to end-effector
:type end: Link or str
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: a FDYN_X block
:rtype: FDyn_X instance
Robot arm forward dynamics model.
The block has one input port:
1. Applied end-effector wrench as an ndarray.
and three output ports:
1. task space pose
2. task space velocity
3. task space acceleration
"""
super().__init__(**blockargs)
self.type = "forward-dynamics-x"
self.robot = robot
self.nstates = robot.n * 2
self.gravcomp = gravcomp
self.velcomp = velcomp
self.representation = representation
# state vector is [q qd]
self.inport_names(("w",))
self.outport_names(("q", "qd", "x", "xd", "xdd"))
if q0 is None:
q0 = np.zeros((robot.n,))
else:
q0 = base.getvector(q0, robot.n)
# append qd0, assumed to be zero
self._x0 = np.r_[q0, np.zeros((robot.n,))]
self._qdd = None
def output(self, t=None):
n = self.robot.n
q = self._x[:n]
qd = self._x[n:]
qdd = self._qdd # from last deriv
T = self.robot.fkine(q)
x = base.tr2x(T.A)
Ja = self.robot.jacob0(q, analytical=self.representation)
xd = Ja @ qd
# print(q)
# print(qd)
# print(xd)
# print(Ja)
# print()
if qdd is None:
xdd = None
else:
Ja_dot = self.robot.jacob_dot(q, qd, J0=Ja)
xdd = Ja @ qdd + Ja_dot @ qd
return [q, qd, x, xd, xdd]
def deriv(self):
# return [qd qdd]
# get current joint space state
n = self.robot.n
q = self._x[:n]
qd = self._x[n:]
# compute joint forces
w = self.inputs[0]
assert len(w) == 6, "wrench vector wrong size"
Q = self.robot.jacob0(q, analytical=self.representation).T @ w
if self.gravcomp or self.velcomp:
if self.velcomp:
qd_rne = qd
else:
qd_rne = np.zeros((n,))
Q_rne = self.robot.rne(q, qd_rne, np.zeros((n,)))
qdd = self.robot.accel(q, qd, Q + Q_rne)
self._qdd = qdd
return np.r_[qd, qdd]
# ------------------------------------------------------------------------ #
[docs]class ArmPlot(GraphicsBlock):
"""
:blockname:`ARMPLOT`
.. table::
:align: left
+--------+---------+---------+
| inputs | outputs | states |
+--------+---------+---------+
| 1 | 0 | 0 |
+--------+---------+---------+
| ndarray| | |
+--------+---------+---------+
"""
nin = 1
nout = 0
inlabels = ('q',)
[docs] def __init__(self, robot=None, *inputs, q0=None, backend=None, **blockargs):
"""
:param ``*inputs``: Optional incoming connections
:type ``*inputs``: Block or Plug
:param robot: Robot model
:type robot: Robot subclass
:param backend: RTB backend name, defaults to 'pyplot'
:type backend: str, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: An ARMPLOT block
:rtype: ArmPlot instance
Create a robot animation.
Notes:
- Uses RTB ``plot`` method
Example of vehicle display (animated). The label at the top is the
block name.
"""
super().__init__(inputs=inputs, **blockargs)
self.inport_names(("q",))
if q0 is None:
q0 = np.zeros((robot.n,))
self.robot = robot
self.backend = backend
self.q0 = q0
self.env = None
def start(self, state, **blockargs):
# create the plot
super().reset()
if state.options.graphics:
self.fig = self.create_figure(state)
self.env = self.robot.plot(
self.q0, backend=self.backend, fig=self.fig, block=False
)
super().start()
def step(self, state):
# inputs are set
self.robot.q = self.inputs[0]
if state.options.animation:
self.env.step()
super().step(state)
def done(self, block=False, **blockargs):
if self.bd.options.graphics:
plt.show(block=block)
super().done()
# ------------------------------------------------------------------------ #
[docs]class Traj(FunctionBlock):
"""
:blockname:`TRAJ`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 0 or 1 | 1 | 0 |
+------------+---------+---------+
| float | float | |
+------------+---------+---------+
"""
nin = -1
nout = 3
outlabels = ('q',)
[docs] def __init__(self, y0=0, yf=1, T=None, time=False, traj="lspb", **blockargs):
"""
:param y0: initial value, defaults to 0
:type y0: array_like(m), optional
:param yf: final value, defaults to 1
:type yf: array_like(m), optional
:param T: time vector or number of steps, defaults to None
:type T: array_like or int, optional
:param time: x is simulation time, defaults to False
:type time: bool, optional
:param traj: trajectory type, one of: 'lspb' [default], 'tpoly'
:type traj: str, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: TRAJ block
:rtype: Traj instance
Create a trajectory block.
A block that generates a trajectory using a trapezoidal or quintic
polynomial profile.
"""
self.time = time
if time:
nin = 1
blockclass = "function"
else:
nin = 0
blockclass = "source"
super().__init__(nin=nin, blockclass=blockclass, inputs=inputs, **blockargs)
y0 = base.getvector(y0)
yf = base.getvector(yf)
assert len(y0) == len(yf), "y0 and yf must have same length"
self.y0 = y0
self.yf = yf
self.time = time
self.T = T
self.traj = traj
self.outport_names(("y", "yd", "ydd"))
def start(self, **blockargs):
if self.time:
assert self.x[0] <= 0, "interpolation not defined for t=0"
assert self.x[-1] >= self.bd.T, "interpolation not defined for t=T"
if self.traj == "lspb":
trajfunc = lspb_func
elif self.traj == "tpoly":
trajfunc = tpoly_func
self.trajfuncs = []
for i in range(len(self.y0)):
self.trajfuncs.append(trajfunc(self.y0[i], self.yf[i], self.T))
def output(self, t=None):
if self.time:
t = self.inputs[0]
out = []
for i in range(len(self.y0)):
out.append(self.trajfuncs[i](t))
# we have a list of tuples out[i][j]
# i is the timestep, j is y/yd/ydd
y = [o[0] for o in out]
yd = [o[1] for o in out]
ydd = [o[2] for o in out]
return [np.hstack(y), np.hstack(yd), np.hstack(ydd)]
# ------------------------------------------------------------------------ #
[docs]class JTraj(SourceBlock):
"""
:blockname:`JTRAJ`
.. table::
:align: left
+------------+------------+---------+
| inputs | outputs | states |
+------------+------------+---------+
| 0 | 3 | 0 |
+------------+------------+---------+
| | ndarray(n) | |
+------------+------------+---------+
"""
nin = 0
nout = 3
outlabels = ('q', 'qd', 'qdd')
[docs] def __init__(self, q0, qf, qd0=None, qdf=None, T=None, **blockargs):
"""
Compute a joint-space trajectory
:param q0: initial joint coordinate
:type q0: array_like(n)
:param qf: final joint coordinate
:type qf: array_like(n)
:param T: time vector or number of steps, defaults to None
:type T: array_like or int, optional
:param qd0: initial velocity, defaults to None
:type qd0: array_like(n), optional
:param qdf: final velocity, defaults to None
:type qdf: array_like(n), optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: TRAJ block
:rtype: Traj instance
- ``tg = jtraj(q0, qf, N)`` is a joint space trajectory where the joint
coordinates vary from ``q0`` (M) to ``qf`` (M). A quintic (5th order)
polynomial is used with default zero boundary conditions for velocity and
acceleration. Time is assumed to vary from 0 to 1 in ``N`` steps.
- ``tg = jtraj(q0, qf, t)`` as above but ``t`` is a uniformly-spaced time
vector
The return value is an object that contains position, velocity and
acceleration data.
Notes:
- The time vector, if given, scales the velocity and acceleration outputs
assuming that the time vector starts at zero and increases
linearly.
:seealso: :func:`ctraj`, :func:`qplot`, :func:`~SerialLink.jtraj`
"""
super().__init__(**blockargs)
self.type = "source"
self.outport_names(
(
"q",
"qd",
"qdd",
)
)
q0 = base.getvector(q0)
qf = base.getvector(qf)
if not len(q0) == len(qf):
raise ValueError("q0 and q1 must be same size")
if qd0 is None:
qd0 = np.zeros(q0.shape)
else:
qd0 = getvector(qd0)
if not len(qd0) == len(q0):
raise ValueError("qd0 has wrong size")
if qdf is None:
qdf = np.zeros(q0.shape)
else:
qd1 = getvector(qdf)
if not len(qd1) == len(q0):
raise ValueError("qd1 has wrong size")
self.q0 = q0
self.qf = qf
self.qd0 = qd0
self.qdf = qf
# call start now, so that output works when called by compile
# set T to 1 just for now
if T is None:
self.T = 1
self.start()
self.T = T
def start(self, state=None):
if self.T is None:
# use simulation tmax
self.T = state.T
tscal = self.T
self.tscal = tscal
q0 = self.q0
qf = self.qf
qd0 = self.qd0
qdf = self.qdf
# compute the polynomial coefficients
A = 6 * (qf - q0) - 3 * (qdf + qd0) * tscal
B = -15 * (qf - q0) + (8 * qd0 + 7 * qdf) * tscal
C = 10 * (qf - q0) - (6 * qd0 + 4 * qdf) * tscal
E = qd0 * tscal
F = q0
self.coeffs = np.array([A, B, C, np.zeros(A.shape), E, F])
self.dcoeffs = np.array(
[np.zeros(A.shape), 5 * A, 4 * B, 3 * C, np.zeros(A.shape), E]
)
self.ddcoeffs = np.array(
[
np.zeros(A.shape),
np.zeros(A.shape),
20 * A,
12 * B,
6 * C,
np.zeros(A.shape),
]
)
def output(self, t=None):
tscal = self.tscal
ts = t / tscal
tt = np.array([ts ** 5, ts ** 4, ts ** 3, ts ** 2, ts, 1]).T
qt = tt @ self.coeffs
# compute velocity
qdt = tt @ self.dcoeffs / tscal
# compute acceleration
qddt = tt @ self.ddcoeffs / tscal ** 2
return [qt, qdt, qddt]
# ------------------------------------------------------------------------ #
[docs]class LSPB(SourceBlock):
"""
:blockname:`LSPB`
.. table::
:align: left
+------------+------------+---------+
| inputs | outputs | states |
+------------+------------+---------+
| 0 | 3 | 0 |
+------------+------------+---------+
| | float | |
+------------+------------+---------+
"""
nin = 0
nout = 3
outlabels = ('q', 'qd', 'qdd')
[docs] def __init__(self, q0, qf, V=None, T=None, **blockargs):
"""
Compute a joint-space trajectory
:param q0: initial joint coordinate
:type q0: array_like(n)
:param qf: final joint coordinate
:type qf: array_like(n)
:param T: time vector or number of steps, defaults to None
:type T: array_like or int, optional
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: LSPB block
:rtype: LSPB instance
- ``tg = jtraj(q0, qf, N)`` is a joint space trajectory where the joint
coordinates vary from ``q0`` (M) to ``qf`` (M). A quintic (5th order)
polynomial is used with default zero boundary conditions for velocity and
acceleration. Time is assumed to vary from 0 to 1 in ``N`` steps.
- ``tg = jtraj(q0, qf, t)`` as above but ``t`` is a uniformly-spaced time
vector
The return value is an object that contains position, velocity and
acceleration data.
Notes:
- The time vector, if given, scales the velocity and acceleration outputs
assuming that the time vector starts at zero and increases
linearly.
:seealso: :func:`ctraj`, :func:`qplot`, :func:`~SerialLink.jtraj`
"""
super().__init__(nout=3, **blockargs)
self.type = "source"
self.T = T
self.q0 = q0
self.qf = qf
def start(self):
if self.T is None:
self.T = self.bd.state.T
self.lspbfunc = lspb_func(self.q0, self.qf, self.T)
def output(self, t=None):
return self.lspbfunc(t)
# ------------------------------------------------------------------------ #
[docs]class CTraj(SourceBlock):
"""
:blockname:`CTRAJ`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 0 | 1 | 0 |
+------------+---------+---------+
| | float | |
+------------+---------+---------+
"""
nin = 0
nout = 1
outlabels = ('T',)
[docs] def __init__(
self,
T1,
T2,
T,
lspb=True,
**blockargs
):
"""
[summary]
:param T1: initial pose
:type T1: SE3
:param T2: final pose
:type T2: SE3
:param T: motion time
:type T: float
:param lspb: Use LSPB motion profile along the path
:type lspb: bool
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: CTRAJ block
:rtype: CTraj instance
Create a Cartesian motion block.
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 ``lspb`` is True then an LSPB motion profile is used along the path
to provide initial acceleration and final deceleration. Otherwise,
motion is at constant velocity.
:seealso: :method:`SE3.interp`
"""
# TODO
# flag to rotate the frame rather than just translate it
super().__init__(**blockargs)
self.T1 = T1
self.T2 = T2
self.T = T
def start(self, state):
if self.T is None:
self.T = self.bd.state.T
if self.lspb:
self.lspbfunc = lspb_func(self.q0, self.qf, self.T)
def output(self, t=None):
if lspb:
s = self.lspbfunc(t)
else:
s = np.min(t / self.T, 1.0)
return self.T1.interp(self.T2, s)
# ------------------------------------------------------------------------ #
[docs]class CirclePath(SourceBlock):
"""
:blockname:`CIRCLEPATH`
.. table::
:align: left
+------------+---------+---------+
| inputs | outputs | states |
+------------+---------+---------+
| 0 or 1 | 1 | 0 |
+------------+---------+---------+
| float | float | |
+------------+---------+---------+
"""
nin = 0
nout = 1
[docs] def __init__(
self,
radius=1,
centre=(0, 0, 0),
pose=None,
frequency=1,
unit="rps",
phase=0,
**blockargs
):
"""
:param radius: radius of circle, defaults to 1
:type radius: float
:param centre: center of circle, defaults to [0,0,0]
:type centre: array_like(3)
:param pose: SE3 pose of output, defaults to None
:type pose: SE3
:param frequency: rotational frequency, defaults to 1
:type frequency: float
:param unit: unit for frequency, one of: 'rps' [default], 'rad'
:type unit: str
:param phase: phase
:type phase: float
:param blockargs: |BlockOptions|
:type blockargs: dict
:return: TRAJ block
:rtype: Traj instance
Create a circular motion block.
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 :math:`(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.
"""
# TODO
# flag to rotate the frame rather than just translate it
super().__init__(**blockargs)
if unit == "rps":
omega = frequency * 2 * pi
phase = frequency * 2 * pi
elif unit == "rad":
omega = frequency
# Redundant assignment, commented for LGTM
# phase = phase
else:
raise ValueError("bad units: rps or rad")
self.radius = radius
assert len(centre) == 3, "centre must be a 3 vector"
self.centre = centre
self.pose = pose
self.omega = omega
self.phase = phase
self.outport_names(("y",))
def output(self, t=None):
theta = t * self.omega + self.phase
x = self.radius * cos(theta) + self.centre[0]
y = self.radius * sin(theta) + self.centre[1]
p = (x, y, self.centre[2])
if self.pose is not None:
pp = SE3.Rt(self.pose.R, p)
p = pp
return [p]
if __name__ == "__main__":
import pathlib
import os.path
exec(
open(
os.path.join(pathlib.Path(__file__).parent.absolute(), "test_robots.py")
).read()
)