def exp(self, theta=None, units='rad'):
"""
Twist.exp Convert twist to homogeneous transformation
TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3).
TW.exp(THETA) as above but with a rotation of THETA about the twist.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = argcheck.getunit(theta, units)
if isinstance(theta, (int, np.int64, float, np.float64)):
return SE2(tr.trexp2(self.S * theta))
else:
return SE2([tr.trexp2(self.S * t) for t in theta])
def draw(self):
if not self.display:
return
if not self.drawn:
self.init()
return
## Update the robot links
# compute all link frames
T = self.robot.fkine_all(self.robot.q)
# draw all the line segments for the noodle plot
for i, segment in enumerate(self.segments):
linkframes = []
for link in segment:
if link is None:
linkframes.append(self.robot.base)
else:
linkframes.append(T[link.number])
points = np.array([linkframe.t for linkframe in linkframes])
self.links[i].set_xdata(points[:, 0])
self.links[i].set_ydata(points[:, 1])
## Draw the end-effectors
# Remove old ee coordinate frame
if self.eeframes:
for quiver in self.eeframes:
quiver.remove()
self.eeframes = []
if self.eeframe:
len = self.options['eelength']
Tjx = SE2(len, 0)
Tjy = SE2(0, len)
# add new ee coordinate frame
for link in self.robot.ee_links:
Te = T[link.number]
# ee axes arrows
Tex = Te * Tjx
Tey = Te * Tjy
xaxis = self._plot_quiver(Te.t, Tex.t, self.options['eex'])
yaxis = self._plot_quiver(Te.t, Tey.t, self.options['eey'])
self.eeframes.extend([xaxis, yaxis])
def draw(self):
if not self.display:
return
if not self.drawn:
self.init()
return
## Update the robot links
# compute all link frames
T = self.robot.fkine_path(self.robot.q)
# draw all the line segments for the noodle plot
for i, segment in enumerate(self.segments):
linkframes = []
for link in segment:
if link is None:
linkframes.append(self.robot.base)
else:
linkframes.append(T[link.number + 1])
points = np.array([linkframe.t for linkframe in linkframes])
self.links[i].set_xdata(points[:, 0])
self.links[i].set_ydata(points[:, 1])
## Draw the end-effectors
# Remove old ee coordinate frame
if self.eeframes:
for quiver in self.eeframes:
quiver.remove()
self.eeframes = []
Tjx = SE2(0.06, 0)
Tjy = SE2(0, 0.06)
red = '#F84752' # '#EE9494'
green = '#BADA55' # '#93E7B0'
# add new ee coordinate frame
for link in self.robot.ee_links:
Te = T[link.number + 1]
# ee axes arrows
Tex = Te * Tjx
Tey = Te * Tjy
xaxis = self._plot_quiver(Te.t, Tex.t, red, 2)
yaxis = self._plot_quiver(Te.t, Tey.t, green, 2)
self.eeframes.extend([xaxis, yaxis])
def create_marker(scene, x, y, shape, colour=None):
"""
Draw the shape at the given position
:param scene: The scene in which to draw the object
:type scene: class:`vpython.canvas`
:param x: The x location to draw the object at
:type x: `float`
:param y: The y location to draw the object at
:type y: `float`
:param shape: The shape of the object to draw
:type shape: `str`
:param colour: The colour of the object
:type colour: `list`
:returns: A 2D object that has been drawn
:rtype: class:`graphics.graphics_object2d.Object2D`
"""
# Set default colour
# Stops a warning about mutable parameter
if colour is None:
colour = [0, 0, 0]
# Create an SE2 for the object
obj_se2 = SE2(x=x, y=y, theta=0)
# Create the object and return it
return Marker2D(obj_se2, scene, shape, colour)
def SE2(tw):
"""
%Twist.SE Convert twist to SE2 or SE3 object
%
TW.SE is an SE2 or SE3 object representing the homogeneous transformation equivalent to the twist.
%
See also Twist.T, SE2, SE3.
"""
return SE2(tw.exp())
def A(self, q=0.0, **kwargs):
"""
Link transform matrix
:param q: Joint coordinate (radians or metres). Not required for links
with no variable
:type q: float
:param fast: return NumPy array instead of ``SE3``
:type param: bool
:return T: link frame transformation matrix
:rtype T: SE3 or ndarray(4,4)
``LINK.A(q)`` is an SE(3) matrix that describes the rigid-body
transformation from the previous to the current link frame to
the next, which depends on the joint coordinate ``q``.
If ``fast`` is True return a NumPy array, either SE(2) or SE(3).
A value of None means that it is the identity matrix.
If ``fast`` is False return an ``SE2`` or ``SE3`` instance.
"""
if self.isjoint:
# a variable joint
if q is None:
raise ValueError("q is required for variable joints")
# premultiply variable part by constant part if present
Ts = self.Ts
if Ts is None:
T = self.v.T(q)
else:
T = Ts @ self.v.T(q)
else:
# a fixed joint
T = self.Ts
if T is None:
return SE2()
else:
return SE2(T, check=False)
def test_fkine_all(self):
a1 = 1
a2 = 1
e = ETS2.r() * ETS2.tx(a1) * ETS2.r() * ETS2.tx(a2)
robot = ERobot2(e)
T = robot.fkine_all([0, 0])
self.assertIsInstance(T, SE2)
self.assertEqual(len(T), 4)
nt.assert_array_almost_equal(T[0].A, SE2().A)
nt.assert_array_almost_equal(T[1].A, SE2().A)
nt.assert_array_almost_equal(T[2].A, SE2(1, 0).A)
nt.assert_array_almost_equal(T[3].A, SE2(2, 0).A)
robot = ERobot2([
ELink2(ETS2.r(), name='link1'),
ELink2(ETS2.tx(1.2) * ETS2.ty(-0.5) * ETS2.r(),
name='link2',
parent='link1'),
ELink2(ETS2.tx(1), name='ee_1', parent='link2'),
ELink2(ETS2.tx(0.6) * ETS2.ty(0.5) * ETS2.r(),
name='link3',
parent='link1'),
ELink2(ETS2.tx(1), name='ee_2', parent='link3')
])
T = robot.fkine_all([0, 0, 0])
self.assertIsInstance(T, SE2)
self.assertEqual(len(T), 6)
nt.assert_array_almost_equal(T[0].A, SE2().A)
nt.assert_array_almost_equal(T[1].A, SE2().A)
nt.assert_array_almost_equal(T[2].A, SE2(1.2, -0.5).A)
nt.assert_array_almost_equal(T[3].A, SE2(2.2, -0.5).A)
nt.assert_array_almost_equal(T[4].A, SE2(0.6, 0.5).A)
nt.assert_array_almost_equal(T[5].A, SE2(1.6, 0.5).A)
def scanmap(self,
centre=(75, 50),
ngrid=3000,
cellsize=0.1,
maxrange=None):
self._centre = centre
self._cellsize = cellsize
self._ngrid = ngrid
# h = waitbar(0, 'rendering a map')
world = np.zeros((ngrid, ngrid), np.int32)
for i in range(0, self.graph.n):
# if i % 20 == 0:
# waitbar(i/self.graph.n, h)
xy = self.scanxy(i)
r, theta = self.scan(i)
if maxrange is not None:
xy = np.delete(xy, r > maxrange, axis=0)
xyt = self.vindex[i].coord
xy = SE2(xyt) * xy.T
# start of each ray
p1 = self.w2g(xyt[0:2])
for end in xy.T:
# end of each ray
p2 = self.w2g(end[0:2])
# all cells along the ray
x, y = bresenham(p1, p2)
try:
# decrement cells along the ray, these are free space
world[y[:-1], x[:-1]] = world[y[:-1], x[:-1]] - 1
# increment target cell
world[y[-1], x[-1]] = world[y[-1], x[-1]] + 1
except IndexError:
# silently ignore rays to points outside the grid map
pass
return world
def ty(cls, eta=None):
"""
An elementary transform (ET). A pure translation of eta along the
x-axis
L = Tty(eta) will instantiate an ET object which represents a pure
translation along the y-axis by amount eta.
L = Tty() as above except this ET representation a variable
translation, i.e. a joint
:param eta: The amount of translation along the x-axis
:type eta: float (metres)
:return: An ET object
:rtype: ET
"""
return cls(lambda y: SE2(0, y), axis='ty', eta=eta)
def r(cls, eta=None):
"""
An elementary transform (ET). A pure rotation of eta about the x-axis.
L = TRx(eta) will instantiate an ET object which represents a pure
rotation about the x-axis by amount eta.
L = TRx() as above except this ET representation a variable
rotation, i.e. a joint
:param eta: The amount of rotation about the x-axis
:type eta: float (radians)
:param joint: The joint number within the robot
:type joint: int
:return: An ET object
:rtype: ET
"""
return cls(lambda theta: SE2(theta), axis='R', eta=eta)
def eval(self, q):
"""
Evaluate an ETS with joint coordinate substitution
:param q: joint coordinates
:type q: array-like
:return: product of transformations
:rtype: SE2
Effectively the forward-kinematics of the ET sequence. Compounds the
transforms left to right, and substitutes in joint coordinates as
needed from consecutive elements of ``q``.
"""
T = SE2()
q = getvector(q, out='sequence')
for e in self:
if e.jtype == self.VARIABLE:
T *= e.T(q.pop(0))
else:
T *= e.T()
return T
def _update(self, x):
xy = SE2(x) * self._coords
self._object.set_xy(xy.T)
def test_2d_create_object():
g_canvas2 = gph.GraphicsCanvas2D()
obj = gph.Object2D(SE2(), g_canvas2.scene, 'c')
def eval(self, q=None, unit='rad'):
"""
Evaluate an ETS with joint coordinate substitution
:param q: joint coordinates
:type q: array-like
:param unit: angular unit, "rad" [default] or "deg"
:type unit: str
:return: The SE(3) or SE(2) matrix value of the ET sequence
:rtype: ndarray(4,4) or ndarray(3,3)
Effectively the forward-kinematics of the ET sequence. Compounds the
transforms left to right, and substitutes in joint coordinates as
needed from consecutive elements of ``q``.
.. note:: if ETs have an explicit joint index, this is used to index
into the vector ``q``.
.. warning:: do not mix ETs with and without explicit joint index.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
>>> print(e)
>>> len(e)
>>> e[1:3]
>>> e.eval([0, 0])
>>> e.eval([90, -90], 'deg')
>>> e = ETS.tx(j=1) * ETS.ty(j=0)
>>> e.eval([3, 4])
"""
if q is not None:
q = getvector(q, out='list')
first = True
for et in self:
if et.isjoint:
if et.jindex is None:
qj = q.pop(0)
else:
qj = q[et.jindex]
if et.isrevolute and unit == 'deg':
qj *= np.pi / 180.0
if et.isflip:
qj = -qj
Tk = et.T(qj)
else:
# for constants
Tk = et.T()
if first:
T = Tk
first = False
else:
T = T @ Tk
if isinstance(self, ETS):
T = SE3(T, check=False)
elif isinstance(self, ETS2):
T = SE2(T, check=False)
T.simplify()
return T
# pass
# else:
# raise ValueError('expecting SO3 or rotation matrix')
# if t is None:
# return cls(base.r2t(R))
# else:
# return cls(base.rt2tr(R, t))
if __name__ == '__main__': # pragma: no cover
import pathlib
a = SE3(1, 2, 3)
b = SO3(a)
print(b)
a = SO3.RPY([.1, .2, .3])
b = SE3(a)
print(b)
from spatialmath import SE2
a = SE2(1, 2, .4)
b = SE3(a)
print(b)
exec(
open(
pathlib.Path(__file__).parent.parent.absolute() / "tests" /
"test_pose3d.py").read()) # pylint: disable=exec-used
def test_constructor(self):
# null constructor
R = SE3()
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(4))
self.assertIsInstance(R, SE3)
# construct from matrix
R = SE3(trotx(0.2))
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
# construct from canonic rotation
R = SE3.Rx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
R = SE3.Ry(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, troty(0.2))
self.assertIsInstance(R, SE3)
R = SE3.Rz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, trotz(0.2))
self.assertIsInstance(R, SE3)
# construct from canonic translation
R = SE3.Tx(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0.2, 0, 0))
self.assertIsInstance(R, SE3)
R = SE3.Ty(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0, 0.2, 0))
self.assertIsInstance(R, SE3)
R = SE3.Tz(0.2)
nt.assert_equal(len(R), 1)
array_compare(R, transl(0, 0, 0.2))
self.assertIsInstance(R, SE3)
# triple angle
R = SE3.Eul([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.Eul(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.Eul([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, eul2tr([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SE3)
R = SE3.RPY([0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.RPY(np.r_[0.1, 0.2, 0.3])
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3]))
self.assertIsInstance(R, SE3)
R = SE3.RPY([10, 20, 30], unit='deg')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([10, 20, 30], unit='deg'))
self.assertIsInstance(R, SE3)
R = SE3.RPY([0.1, 0.2, 0.3], order='xyz')
nt.assert_equal(len(R), 1)
array_compare(R, rpy2tr([0.1, 0.2, 0.3], order='xyz'))
self.assertIsInstance(R, SE3)
# angvec
R = SE3.AngVec(0.2, [1, 0, 0])
nt.assert_equal(len(R), 1)
array_compare(R, trotx(0.2))
self.assertIsInstance(R, SE3)
R = SE3.AngVec(0.3, [0, 1, 0])
nt.assert_equal(len(R), 1)
array_compare(R, troty(0.3))
self.assertIsInstance(R, SE3)
# OA
R = SE3.OA([0, 1, 0], [0, 0, 1])
nt.assert_equal(len(R), 1)
array_compare(R, np.eye(4))
self.assertIsInstance(R, SE3)
# random
R = SE3.Rand()
nt.assert_equal(len(R), 1)
self.assertIsInstance(R, SE3)
# random
T = SE3.Rand()
R = T.R
t = T.t
T = SE3.Rt(R, t)
self.assertIsInstance(T, SE3)
self.assertEqual(T.A.shape, (4, 4))
nt.assert_equal(T.R, R)
nt.assert_equal(T.t, t)
# copy constructor
R = SE3.Rx(pi / 2)
R2 = SE3(R)
R = SE3.Ry(pi / 2)
array_compare(R2, trotx(pi / 2))
# SO3
T = SE3(SO3())
nt.assert_equal(len(T), 1)
self.assertIsInstance(T, SE3)
nt.assert_equal(T.A, np.eye(4))
# SE2
T = SE3(SE2(1, 2, 0.4))
nt.assert_equal(len(T), 1)
self.assertIsInstance(T, SE3)
self.assertEqual(T.A.shape, (4, 4))
nt.assert_equal(T.t, [1, 2, 0])
