def _getq(self, q=None):
"""
Get joint coordinates (Robot superclass)
:param q: passed value, defaults to None
:type q: array_like, optional
:return: passed or value from robot state
:rtype: ndarray(n,)
"""
if q is None:
return self.q
elif isvector(q, self.n):
return getvector(q, self.n)
else:
return getmatrix(q, (None, self.n))
def _plot2(robot,
block,
q,
dt,
limits=None,
vellipse=False,
fellipse=False,
eeframe=True,
name=True):
# Make an empty 2D figure
env = rp.backends.PyPlot2()
q = getmatrix(q, (None, robot.n))
# Add the robot to the figure in readonly mode
if q.shape[0] == 1:
env.launch(robot.name + ' Plot', limits)
else:
env.launch(robot.name + ' Trajectory Plot', limits)
env.add(robot, readonly=True, eeframe=eeframe, name=name)
if vellipse:
vell = robot.vellipse(centre='ee')
env.add(vell)
if fellipse:
fell = robot.fellipse(centre='ee')
env.add(fell)
for qk in q:
robot.q = qk
env.step()
time.sleep(dt / 1000)
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env
def _plot_swift(self, q, block): # pragma nocover
# Make an empty 3D figure
env = Swift()
q = getmatrix(q, (None, self.n))
self.q = q[0, :]
# Add the self to the figure in readonly mode
env.launch()
env.add(self, readonly=True)
for qk in q:
self.q = qk
env.step()
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env
def qmincon(self, q=None):
"""
Move away from joint limits
:param q: Joint coordinates
:type q: ndarray(n)
:retrun qs: The calculated joint values
:rtype qs: ndarray(n)
:return: Optimisation solved (True) or failed (False)
:rtype: bool
:return: Final value of the objective function
:rtype: float
``qs, success, err = qmincon(q)`` exploits null space motion and returns
a set of joint angles ``qs`` (n) that result in the same end-effector
pose but are away from the joint coordinate limits. ``n`` is the number
of robot joints. ``success`` is True for successful optimisation.
``err`` is the scalar final value of the objective function.
**Trajectory operation**
In all cases if ``q`` is (m,n) it is taken as a pose sequence and
``qmincon()`` returns the adjusted joint coordinates (m,n) corresponding
to each of the configurations in the sequence.
``err`` and ``success`` are also (m) and indicate the results of
optimisation for the corresponding trajectory step.
.. note:: Robot must be redundant.
"""
def sumsqr(A):
return np.sum(A**2)
def cost(x, ub, lb, qm, N):
return sumsqr((2 * (N @ x + qm) - ub - lb) / (ub - lb))
q = getmatrix(q, (None, self.n))
qstar = np.zeros((q.shape[0], self.n))
error = np.zeros(q.shape[0])
success = np.zeros(q.shape[0])
lb = self.qlim[0, :]
ub = self.qlim[1, :]
for k, qk in enumerate(q):
J = self.jacobe(qk)
N = null(J)
x0 = np.zeros(N.shape[1])
A = np.r_[N, -N]
b = np.r_[ub - qk, qk - lb].reshape(A.shape[0], )
con = LinearConstraint(A, -np.inf, b)
res = minimize(lambda x: cost(x, ub, lb, qk, N),
x0,
constraints=con)
qstar[k, :] = qk + N @ res.x
error[k] = res.fun
success[k] = res.success
if q.shape[0] == 1:
return qstar[0, :], success[0], error[0]
else:
return qstar, success, error
def _plot(robot,
block,
q,
dt,
limits=None,
vellipse=False,
fellipse=False,
jointaxes=True,
eeframe=True,
shadow=True,
name=True,
movie=None):
# Make an empty 3D figure
env = rp.backends.PyPlot()
q = getmatrix(q, (None, robot.n))
# Add the robot to the figure in readonly mode
if q.shape[0] == 1:
env.launch(robot.name + ' Plot', limits)
else:
env.launch(robot.name + ' Trajectory Plot', limits)
env.add(robot,
readonly=True,
jointaxes=jointaxes,
eeframe=eeframe,
shadow=shadow,
name=name)
if vellipse:
vell = robot.vellipse(centre='ee')
env.add(vell)
if fellipse:
fell = robot.fellipse(centre='ee')
env.add(fell)
if movie is not None:
if not _pil_exists:
raise RuntimeError(
'to save movies PIL must be installed:\npip3 install PIL')
images = [] # list of images saved from each plot
# make the background white, looks better than grey stipple
env.ax.w_xaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
env.ax.w_yaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
env.ax.w_zaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
for qk in q:
robot.q = qk
env.step()
#time.sleep(dt/1000)
if movie is not None:
# render the frame and save as a PIL image in the list
canvas = env.fig.canvas
img = PIL.Image.frombytes('RGB', canvas.get_width_height(),
canvas.tostring_rgb())
images.append(img)
if movie is not None:
# save it as an animated GIF
images[0].save(movie,
save_all=True,
append_images=images[1:],
optimize=False,
duration=dt,
loop=0)
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env
def fkine(self, q, endlink=None, startlink=None, tool=None):
'''
Forward kinematics
:param q: Joint coordinates
:type q: ndarray(n) or ndarray(m,n)
:param endlink: end-effector to compute forward kinematics to
:type endlink: str or ELink
:param startlink: the link to compute forward kinematics from
:type startlink: str or ELink
:param tool: tool transform, optional
:type tool: SE3
:return: The transformation matrix representing the pose of the
end-effector
:rtype: SE3 instance
- ``T = robot.fkine(q)`` evaluates forward kinematics for the robot at
joint configuration ``q``.
**Trajectory operation**:
If ``q`` has multiple rows (mxn), it is considered a trajectory and the
result is an ``SE3`` instance with ``m`` values.
.. note::
- For a robot with a single end-effector there is no need to
specify ``endlink``
- For a robot with multiple end-effectors, the ``endlink`` must
be specified.
- The robot's base tool transform, if set, is incorporated
into the result.
- A tool transform, if provided, is incorporated into the result.
- Works from the end-effector link to the base
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
'''
q = getmatrix(q, (None, self.n))
if tool is not None:
Ttool = tool.A
endlink, startlink = self._get_limit_links(endlink, startlink)
T = SE3.Empty()
for k, qk in enumerate(q):
link = endlink # start with last link
# add tool if provided
if tool is None:
Tk = link.A(qk[link.jindex], fast=True)
else:
Tk = link.A(qk[link.jindex], fast=True) @ Ttool
# add remaining links, back toward the base
while True:
link = link.parent
if link is None:
break
Tk = link.A(qk[link.jindex], fast=True) @ Tk
if link is startlink:
break
# add base transform if it is set
if self.base is not None:
Tk = self.base.A @ Tk
T.append(SE3(Tk))
return T
def plot2(self,
q,
block=True,
dt=0.05,
limits=None,
vellipse=False,
fellipse=False,
eeframe=True,
name=False):
"""
2D Graphical display and animation
:param block: Block operation of the code and keep the figure open
:type block: bool
:param q: The joint configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:param dt: if q is a trajectory, this describes the delay in
milliseconds between frames
:type dt: int
:param limits: Custom view limits for the plot. If not supplied will
autoscale, [x1, x2, y1, y2, z1, z2]
:type limits: ndarray(6)
:param vellipse: (Plot Option) Plot the velocity ellipse at the
end-effector
:type vellipse: bool
:param vellipse: (Plot Option) Plot the force ellipse at the
end-effector
:type vellipse: bool
:param eeframe: (Plot Option) Plot the end-effector coordinate frame
at the location of the end-effector. Uses three arrows, red,
green and blue to indicate the x, y, and z-axes.
:type eeframe: bool
:param name: (Plot Option) Plot the name of the robot near its base
:type name: bool
:return: A reference to the PyPlot object which controls the
matplotlib figure
:rtype: PyPlot
- ``robot.plot2(q)`` displays a 2D graphical view of a robot based on
the kinematic model and the joint configuration ``q``. This is a
stick figure polyline which joins the origins of the link coordinate
frames. The plot will autoscale with an aspect ratio of 1.
If ``q`` (m,n) representing a joint-space trajectory it will create an
animation with a pause of ``dt`` seconds between each frame.
.. note::
- By default this method will block until the figure is dismissed.
To avoid this set ``block=False``.
- The polyline joins the origins of the link frames, but for
some Denavit-Hartenberg models those frames may not actually
be on the robot, ie. the lines to not neccessarily represent
the links of the robot.
:seealso: :func:`teach2`
"""
if isinstance(self, rtb.ERobot): # pragma nocover
raise NotImplementedError(
"2D Plotting of ERobot's not implemented yet")
# Make an empty 2D figure
env = PyPlot2()
q = getmatrix(q, (None, self.n))
# Add the self to the figure in readonly mode
if q.shape[0] == 1:
env.launch(self.name + ' Plot', limits)
else:
env.launch(self.name + ' Trajectory Plot', limits)
env.add(self, readonly=True, eeframe=eeframe, name=name)
if vellipse:
vell = self.vellipse(centre='ee')
env.add(vell)
if fellipse:
fell = self.fellipse(centre='ee')
env.add(fell)
for qk in q:
self.q = qk
env.step()
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env