def _binop(left, right, op):
out = []
if isinstance(right, Image):
# Image OP Image
if left.numimages == right.numimages:
# two sequences of equal length
for x, y in zip(left._imlist, right._imlist):
out.append(op(x, y))
elif left.numimages == 1:
# singleton OP sequence
for y in right._imlist:
out.append(op(left.image, y))
elif right.numimages == 1:
# sequence OP singleton
for x in left._imlist:
out.append(op(x, right.image))
else:
raise ValueError('cannot perform binary operation \
on sequences of unequal length')
elif isscalar(right):
# Image OP scalar
for x in left._imlist:
out.append(op(x, right))
else:
raise ValueError('right operand can only be scalar or Image')
return Image(out)
def exp(self, theta=None, units='rad'):
"""
Twist3.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 Twist3.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist3.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
return SE2(base.trexp2(self.S * theta))
else:
return SE2([base.trexp2(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'):
"""
Exponentiate a twist
:param theta: DESCRIPTION, defaults to None
:type theta: TYPE, optional
:param units: DESCRIPTION, defaults to 'rad'
:type units: TYPE, optional
:return: DESCRIPTION
:rtype: TYPE
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 Twist3.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist3.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
return SE3(base.trexp(self.S * theta))
else:
return SE3([base.trexp(self.S * t) for t in theta])
def __mul__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``*`` operator
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
- ``X * Y`` compounds the twists ``X`` and ``Y``
- ``X * s`` performs elementwise multiplication of the elements of ``X`` by ``s``
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
======== ==================== =================== ========================
Multiplicands Product
------------------------------ ---------------------------------------------
left right type operation
======== ==================== =================== ========================
Twist2 Twist2 Twist2 product of exponentials
Twist2 scalar Twist2 element-wise product
scalar Twist2 Twist2 element-wise product
Twist2 SE2 Twist2 exponential x SE2
======== ==================== =================== ========================
.. note::
#. scalar x Twist is handled by ``__rmul__``
#. scalar multiplication is commutative but the result is not a group
operation so the result will be a matrix
#. Any other input combinations result in a ValueError.
For pose composition the ``left`` and ``right`` operands may be a sequence
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left * right``
1 M M ``prod[i] = left * right[i]``
N 1 M ``prod[i] = left[i] * right``
M M M ``prod[i] = left[i] * right[i]``
========= ========== ==== ================================
"""
if isinstance(right, Twist2):
# twist composition -> Twist
return Twist2(
left.binop(
right,
lambda x, y: base.trlog2(base.trexp2(x) @ base.trexp2(y),
twist=True)))
elif isinstance(right, SE2):
# twist * SE2 -> SE2
return SE2(left.binop(right, lambda x, y: base.trexp2(x) @ y),
check=False)
elif base.isscalar(right):
# return Twist(left.S * right)
return Twist2(left.binop(right, lambda x, y: x * y))
else:
raise ValueError('Twist2 *, incorrect right operand')
def path(self, t=10, u=None, x0=None):
"""
Compute path by integration (superclass method)
:param t: [description], defaults to None
:type t: [type], optional
:param u: [description], defaults to None
:type u: [type], optional
:param x0: initial state, defaults to (0,0,0)
:type x0: array_like(3), optional
:return: time vector and state history
:rtype: (ndarray(1), ndarray(n,3))
% XF = V.path(TF, U) is the final state of the vehicle (3x1) from the initial
% state (0,0,0) with the control inputs U (vehicle specific). TF is a scalar to
% specify the total integration time.
%
% XP = V.path(TV, U) is the trajectory of the vehicle (Nx3) from the initial
% state (0,0,0) with the control inputs U (vehicle specific). T is a vector (N) of
% times for which elements of the trajectory will be computed.
%
% XP = V.path(T, U, X0) as above but specify the initial state.
%
% Notes::
% - Integration is performed using ODE45.
% - The ODE being integrated is given by the deriv method of the vehicle object.
# t, x = veh.path(5, u=control)
# print(t)
"""
if x0 is None:
x0 = np.zeros(3)
def dynamics(t, x, vehicle, demand):
u = vehicle.control(demand, x)
return vehicle.deriv(x, u)
if base.isscalar(t):
t_span = (0, t)
t_eval = np.linspace(0, t, 100)
elif isinstance(t, np.ndarray):
t_span = (t[0], t[-1])
t_eval = t
else:
raise ValueError('bad time argument')
sol = integrate.solve_ivp(dynamics,
t_span,
x0,
t_eval=t_eval,
method="RK45",
args=(self, u))
return (sol.t, sol.y)
def exp(self, theta=None, units='rad'):
"""
Exponentiate a 3D twist
:param theta: rotation magnitude, defaults to None
:type theta: float, optional
:param units: rotational units, defaults to 'rad'
:type units: str, optional
:return: SE(3) matrix
:rtype: SE3 instance
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
:math:`e^{[S]}`
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
:math:`e^{\theta[S]}`
Example:
.. runblock:: pycon
>>> from spatialmath import SE3, Twist3
>>> T = SE3(1, 2, 3) * SE3.Rx(0.3)
>>> S = Twist3(T)
>>> S.exp(0)
>>> S.exp(1)
.. notes::
- For the second form, the twist must, if rotational, have a unit
rotational component.
:seealso: :func:`spatialmath.base.trexp`
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
# theta is a scalar
return SE3(base.trexp(self.S * theta))
else:
# theta is a vector
if len(self) == 1:
return SE3([base.trexp(self.S * t) for t in theta])
elif len(self) == len(theta):
return SE3(
[base.trexp(S * t) for S, t in zip(self.data, theta)])
else:
raise ValueError('length of twist and theta not consistent')
def _format(self, l, name, ignorevalue=0, indices=None): # noqa # pragma nocover
v = getattr(self, name)
s = None
if v is None:
return
if isscalar(v) and v != ignorevalue:
s = f"{name}={v:.3g}"
elif isinstance(v, np.ndarray):
if np.linalg.norm(v, ord=np.inf) > 0:
if indices is not None:
flat = v.flatten()
v = np.r_[[flat[k] for k in indices]]
s = f"{name}=[" + ", ".join([f"{x:.3g}" for x in v]) + "]"
if s is not None:
l.append(s)
def __rmul(right, left): # pylint: disable=no-self-argument
"""
Overloaded ``*`` operator
:arg right: right multiplicand
:arg left: left multiplicand
:return: product
:raises: NotImplemented
Left-multiplication by a scalar
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
"""
if base.isscalar(left):
return Twist3(self.S * left)
else:
raise ValueError('twist *, incorrect left operand')
def exp(self, theta=None, units='rad'):
r"""
Exponentiate a 2D twist
:param theta: rotation magnitude, defaults to None
:type theta: float, optional
:param units: rotational units, defaults to 'rad'
:type units: str, optional
:return: SE(2) matrix
:rtype: SE2 instance
- ``X.exp()`` is the homogeneous transformation equivalent to the twist,
:math:`e^{[S]}`
- ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis,
:math:`e^{\theta[S]}`
Example:
.. runblock:: pycon
>>> from spatialmath import SE2, Twist2
>>> T = SE2(1, 2, 0.3)
>>> S = Twist2(T)
>>> S.exp(0)
>>> S.exp(1)
.. notes::
- For the second form, the twist must, if rotational, have a unit
rotational component.
:seealso: :func:`spatialmath.base.trexp2`
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
return SE2(base.trexp2(self.S * theta))
else:
return SE2([base.trexp2(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'):
"""
Exponentiate a 3D twist
:param theta: DESCRIPTION, defaults to None
:type theta: TYPE, optional
:param units: DESCRIPTION, defaults to 'rad'
:type units: TYPE, optional
:return: homogeneous transformation
:rtype: SE3
-``X.exp()`` is the homogeneous transformation equivalent to the twist.
-``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis.
.. notes::
- For the second form, the twist must, if rotational, have a unit
rotational component.
See also Twist3.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist3.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = base.getunit(theta, units)
if base.isscalar(theta):
# theta is a scalar
return SE3(base.trexp(self.S * theta))
else:
# theta is a vector
if len(self) == 1:
return SE3([base.trexp(self.S * t) for t in theta])
elif len(self) == len(theta):
return SE3(
[base.trexp(S * t) for S, t in zip(self.data, theta)])
else:
raise ValueError('length of twist and theta not consistent')
def __init__(self, arg=None, w=None, check=True):
r"""
Construct a new 2D Twist object
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
a 2x2 matrix.
- ``Twist2(T)`` is a 2D Twist object representing the SE(2) rigid-body motion expressed as
a 3x3 matrix.
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
moment vector (1 element) and direction vector (2 elements).
:References:
- **Robotics, Vision & Control**, Corke, Springer 2017.
- **Modern Robotics, Lynch & Park**, Cambridge 2017
.. note:: Compared to Lynch & Park this module implements twist vectors
with the translational components first, followed by rotational
components, ie. :math:`[\omega, \vec{v}]`.
"""
super().__init__()
if w is None:
# zero or one arguments passed
if super().arghandler(arg, convertfrom=(SE2, ), check=check):
return
elif w is not None and base.isscalar(w) and base.isvector(arg, 2):
# Twist(v, w)
self.data = [np.r_[arg, w]]
return
raise ValueError('bad twist value')
def __mul__(self, right):
"""
Twist3.mtimes Multiply twist by twist or scalar
TW1 * TW2 is a new Twist representing the composition of twists TW1 and
TW2.
TW * T is an SE2 or SE3 that is the composition of the twist TW and the
homogeneous transformation object T.
TW * S with its twist coordinates scaled by scalar S.
TW * T compounds a twist with an SE2/3 transformation
%
"""
left = self
if isinstance(right, Twist2):
# twist composition
return Twist2(left.exp() * right.exp())
elif base.isscalar(right):
return Twist2(left.S * right)
else:
raise ValueError('twist *, incorrect right operand')
def transl2(x, y=None):
"""
Create SE(2) pure translation, or extract translation from SE(2) matrix
**Create a translational SE(2) matrix**
:param x: translation along X-axis
:type x: float
:param y: translation along Y-axis
:type y: float
:return: SE(2) transform matrix or the translation elements of a homogeneous
transform :rtype: ndarray(3,3)
- ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3)
representing a pure translation.
- ``T = transl2( V )`` as above but the translation is given by a 2-element
list, dict, or a numpy array, row or column vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> transl2(3, 4)
>>> transl2([3, 4])
>>> transl2(np.array([3, 4]))
**Extract the translational part of an SE(2) matrix**
:param x: SE(2) transform matrix
:type x: ndarray(3,3)
:return: translation elements of SE(2) matrix
:rtype: ndarray(2)
- ``t = transl2(T)`` is the translational part of the SE(3) matrix ``T`` as a
2-element NumPy array.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
>>> transl2(T)
.. note:: This function is compatible with the MATLAB version of the Toolbox. It
is unusual/weird in doing two completely different things inside the one
function.
"""
if base.isscalar(x) and base.isscalar(y):
# (x, y) -> SE(2)
t = np.r_[x, y]
elif base.isvector(x, 2):
# R2 -> SE(2)
t = base.getvector(x, 2)
elif base.ismatrix(x, (3, 3)):
# SE(2) -> R2
return x[:2, 2]
else:
raise ValueError('bad argument')
if t.dtype != 'O':
t = t.astype('float64')
T = np.identity(3, dtype=t.dtype)
T[:2, 2] = t
return T
def gamma_decode(image, gamma):
"""
Gamma decoding
:param image: input image
:type image: ndarray(h,w) or ndarray(h,w,n)
:param gamma: string identifying srgb, or scalar to raise the image power
:type gamma: string or float TODO: variable input seems awkward
:return out: gamma corrected version of image
:rtype out: ndarray(h,w) or ndarray(h,w,n)
- ``gamma_decode(image, gamma)`` is the image with an inverse gamma correction based
on ``gamma`` applied.
Example:
.. runblock:: pycon
.. note::
- Gamma decoding should be applied to any color image prior to
colometric operations.
- The exception to this is colorspace conversion using COLORSPACE
which expects RGB images to be gamma encoded.
- Gamma encoding is typically performed in a camera with
GAMMA=0.45.
- Gamma decoding is typically performed in the display with
GAMMA=2.2.
- For images with multiple planes the gamma correction is applied
to all planes.
- For images sequences the gamma correction is applied to all
elements.
- For images of type double the pixels are assumed to be in the
range 0 to 1.
- For images of type int the pixels are assumed in the range 0 to
the maximum value of their class. Pixels are converted first to
double, processed, then converted back to the integer class.
:references:
- Robotics, Vision & Control, Chapter 10, P. Corke, Springer 2011.
"""
if not (base.isscalar(gamma) or isinstance(gamma, str)):
raise ValueError('gamma must be string or scalar')
if gamma == 'srgb':
imagef = ifloat(image)
if imagef.ndims == 2:
# greyscale
return _srgb_inv(imagef.image)
elif imagef.ndims == 3:
# multi-dimensional
out = np.alloc(imagef.shape, dtype=imagef.dtype)
for p in range(imagef.ndims):
out[:, :, p] = _srgb_inv(imagef[:, :, p])
else:
raise ValueError('expecting 2d or 3d image')
if np.issubdtype(image.dtype, np.float):
# original image was float, convert back
return iint(out)
else:
# normal power law:
if np.issubdtype(image.dtype, np.float):
return image**(1.0 / gamma)
else:
# int image
maxg = np.float32((np.iinfo(image.dtype).max))
return ((image.astype(np.float32) / maxg)
**(1 / gamma)) * maxg # original
# return ((image.astype(np.float32) / maxg) ** gamma) * maxg
def _srgb_inverse(self, Rg):
"""
Inverse sRGB gamma correction
:param Rg: 2D image
:type Rg: numpy array, shape (N,M)
:return: R
:rtype: numpy array
- ``_srgb_imverse(Rg)`` maps an sRGB gamma encoded image to linear
tristimulus values.
Example:
.. runblock:: pycon
.. note::
- Based on code from Pascal Getreuer 2005-2010
- And colorspace.m from Peter Corke's Machine Vision Toolbox
"""
R = np.alloc(Rg.shape, dtype=np.float32)
a = 0.0404482362771076
i = np.where(Rg <= a)
noti = np.where(Rg > a)
R[i] = Rg[i] / 12.92
R[noti] = np.real(((Rg[noti] + 0.055) / 1.055)**2.4)
return R
def _srgb(self, R):
"""
sRGB Gamma correction
:param R: 2D image
:type R: numpy array, shape (N,M)
:return: Rg
:rtype: numpy array
- ``_srgb(R)`` maps linear tristimulus values to an sRGB gamma encoded
image.
Example:
.. runblock:: pycon
.. note::
- Based on code from Pascal Getreuer 2005-2010
- And colorspace.m from Peter Corke's Machine Vision Toolbox
"""
Rg = np.alloc(R.shape, dtype=np.float32)
a = 0.0031306684425005883
b = 0.416666666666666667
i = np.where(R <= a)
noti = np.where(R > a)
Rg[i] = R[i] * 12.92
Rg[noti] = np.real(1.055 * (R[noti]**b) - 0.055)
return Rg
def gamma_encode(image, gamma):
"""
Inverse gamma correction
:param image: input image
:type image: ndarray(h,w) or ndarray(h,w,n)
:param gamma: string identifying srgb, or scalar to raise the image power
:type gamma: string or float TODO: variable input seems awkward
:return out: gamma corrected version of image
:rtype out: ndarray(h,w) or ndarray(h,w,n)
- ``gamma_encode(image, gamma)`` maps linear tristimulus values to a gamma encoded
image.
Example:
.. runblock:: pycon
.. note::
- Gamma decoding should be applied to any color image prior to
colometric operations.
- The exception to this is colorspace conversion using COLORSPACE
which expects RGB images to be gamma encoded.
- Gamma encoding is typically performed in a camera with
GAMMA=0.45.
- Gamma decoding is typically performed in the display with
GAMMA=2.2.
- For images with multiple planes the gamma correction is applied
to all planes.
- For images sequences the gamma correction is applied to all
elements.
- For images of type double the pixels are assumed to be in the
range 0 to 1.
- For images of type int the pixels are assumed in the range 0 to
the maximum value of their class. Pixels are converted first to
double, processed, then converted back to the integer class.
:references:
- Robotics, Vision & Control, Chapter 10, P. Corke, Springer 2011.
"""
if not (base.isscalar(gamma) or isinstance(gamma, str)):
raise ValueError('gamma must be string or scalar')
if gamma == 'srgb':
imagef = ifloat(image)
if imagef.ndims == 2:
# greyscale
return _srgb(imagef.image)
elif imagef.ndims == 3:
# multi-dimensional
out = np.alloc(imagef.shape, dtype=imagef.dtype)
for p in range(imagef.ndims):
out[:, :, p] = _srgb(imagef[:, :, p])
else:
raise ValueError('expecting 2d or 3d image')
if np.issubdtype(image.dtype, np.floating):
# original image was float, convert back
return iint(out)
else:
# normal power law:
# import code
# code.interact(local=dict(globals(), **locals()))
if np.issubdtype(image.dtype, np.float):
return image**gamma
else:
# int image
maxg = np.float32((np.iinfo(image.dtype).max))
return ((image.astype(np.float32) / maxg)**gamma) * maxg
def __mul__(left, right): # pylint: disable=no-self-argument
"""
Overloaded ``*`` operator
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
Twist composition or scaling:
- ``X * Y`` compounds the twists ``X`` and ``Y``
- ``X * s`` performs elementwise multiplication of the elements of ``X`` by ``s``
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
======== ==================== =================== ========================
Multiplicands Product
------------------------------- -----------------------------------
left right type operation
======== ==================== =================== ========================
Twist Twist Twist product of exponentials
Twist scalar Twist element-wise product
scalar Twist Twist element-wise product
Twist SE3 Twist exponential x SE3
Twist SpatialVelocity SpatialVelocity adjoint product
Twist SpatialAcceleration SpatialAcceleration adjoint product
Twist SpatialForce SpatialForce adjoint product
======== ==================== =================== ========================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar x Pose is handled by ``__rmul__``
#. scalar multiplication is commutative but the result is not a group
operation so the result will be a matrix
#. Any other input combinations result in a ValueError.
For pose composition the ``left`` and ``right`` operands may be a sequence
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left * right``
1 M M ``prod[i] = left * right[i]``
N 1 M ``prod[i] = left[i] * right``
M M M ``prod[i] = left[i] * right[i]``
========= ========== ==== ================================
"""
# TODO TW * T compounds a twist with an SE2/3 transformation
if isinstance(right, Twist3):
# twist composition -> Twist
return Twist3(
left.binop(
right,
lambda x, y: base.trlog(base.trexp(x) @ base.trexp(y),
twist=True)))
elif isinstance(right, SE3):
# twist * SE3 -> SE3
return SE3(left.binop(right, lambda x, y: base.trexp(x) @ y),
check=False)
elif base.isscalar(right):
# return Twist(left.S * right)
return Twist3(left.binop(right, lambda x, y: x * y))
elif isinstance(right, SpatialVelocity):
return SpatialVelocity(left.Ad @ right.V)
elif isinstance(right, SpatialAcceleration):
return SpatialAcceleration(left.Ad @ right.V)
elif isinstance(right, SpatialForce):
return SpatialForce(left.Ad @ right.V)
else:
raise ValueError('twist *, incorrect right operand')
def __pow__(self, other):
if not isscalar(other):
raise ValueError('exponent must be a scalar')
return Image._binop(self, other, lambda x, y: x**y)
def __rmul(self, left):
if base.isscalar(left):
return Twist2(self.S * left)
else:
raise ValueError('twist *, incorrect left operand')
def plot_cylinder(
radius,
height,
resolution=50,
centre=(0, 0, 0),
ends=False,
ax=None,
filled=False,
**kwargs
):
"""
Plot a cylinder using matplotlib
:param radius: radius of sphere, defaults to 1
:type radius: float, optional
:param height: height of cylinder in the z-direction
:type height: float or array_like(2)
:param resolution: number of points on circumferece, defaults to 50
:type resolution: int, optional
:param pose: pose of sphere, defaults to None
:type pose: SE3, optional
:param ax: axes to draw into, defaults to None
:type ax: Axes3D, optional
:param filled: draw filled polygon, else wireframe, defaults to False
:type filled: bool, optional
:param kwargs: arguments passed to ``plot_wireframe`` or ``plot_surface``
:return: matplotlib objects
:rtype: list of matplotlib object types
The axis of the cylinder is parallel to the z-axis and extends from z=0
to z=height, or z=height[0] to z=height[1].
The cylinder can be positioned by setting ``centre``, or positioned
and orientated by setting ``pose``.
:seealso: :func:`~matplotlib.pyplot.plot_surface`, :func:`~matplotlib.pyplot.plot_wireframe`
"""
if base.isscalar(height):
height = [0, height]
ax = axes_logic(ax, 3)
x = np.linspace(centre[0] - radius, centre[0] + radius, resolution)
z = height
X, Z = np.meshgrid(x, z)
Y = np.sqrt(radius ** 2 - (X - centre[0]) ** 2) + centre[1] # Pythagorean theorem
handles = []
handles.append(_render3D(ax, X, Y, Z, filled=filled, **kwargs))
handles.append(_render3D(ax, X, (2 * centre[1] - Y), Z, filled=filled, **kwargs))
if ends and kwargs.get("filled", default=False):
floor = Circle(centre[:2], radius, **kwargs)
handles.append(ax.add_patch(floor))
pathpatch_2d_to_3d(floor, z=height[0], zdir="z")
ceiling = Circle(centre[:2], radius, **kwargs)
handles.append(ax.add_patch(ceiling))
pathpatch_2d_to_3d(ceiling, z=height[1], zdir="z")
return handles
def B(self, B_new):
if isscalar(B_new):
self._B = B_new
else:
raise TypeError("B must be a scalar")
def fdyn(self,
T,
q0,
torqfun=None,
targs=None,
qd0=None,
solver='RK45',
sargs=None,
dt=None,
progress=False):
"""
Integrate forward dynamics
:param T: integration time
:type T: float
:param q0: initial joint coordinates
:type q0: array_like
:param qd0: initial joint velocities, assumed zero if not given
:type qd0: array_like
:param torqfun: a function that computes torque as a function of time
and/or state
:type torqfun: callable
:param targs: argumments passed to ``torqfun``
:type targs: dict
:type solver: name of scipy solver to use, RK45 is the default
:param solver: str
:type sargs: arguments passed to the solver
:param sargs: dict
:type dt: time step for results
:param dt: float
:param progress: show progress bar, default False
:type progress: bool
:return: robot trajectory
:rtype: namedtuple
- ``tg = R.fdyn(T, q)`` integrates the dynamics of the robot with zero
input torques over the time interval 0 to ``T`` and returns the
trajectory as a namedtuple with elements:
- ``t`` the time vector (M,)
- ``q`` the joint coordinates (M,n)
- ``qd`` the joint velocities (M,n)
- ``tg = R.fdyn(T, q, torqfun)`` as above but the torque applied to the
joints is given by the provided function::
tau = function(robot, t, q, qd, **args)
where the inputs are:
- the robot object
- current time
- current joint coordinates (n,)
- current joint velocity (n,)
- args, optional keyword arguments can be specified, these are
passed in from the ``targs`` kewyword argument.
The function must return a Numpy array (n,) of joint forces/torques.
Examples:
#. to apply zero joint torque to the robot without Coulomb
friction::
def myfunc(robot, t, q, qd):
return np.zeros((robot.n,))
tg = robot.nofriction().fdyn(5, q0, myfunc)
plt.figure()
plt.plot(tg.t, tg.q)
plt.show()
We could also use a lambda function::
tg = robot.nofriction().fdyn(
5, q0, lambda r, t, q, qd: np.zeros((r.n,)))
#. the robot is controlled by a PD controller. We first define a
function to compute the control which has additional parameters for
the setpoint and control gains (qstar, P, D)::
def myfunc(robot, t, q, qd, qstar, P, D):
return (qstar - q) * P + qd * D # P, D are (6,)
targs = {'qstar': VALUE, 'P': VALUE, 'D': VALUE}
tg = robot.fdyn(10, q0, myfunc, targs=targs) )
Many integrators have variable step length which is problematic if we
want to animate the result. If ``dt`` is specified then the solver
results are interpolated in time steps of ``dt``.
:notes:
- This function performs poorly with non-linear joint friction, such as
Coulomb friction. The R.nofriction() method can be used to set this
friction to zero.
- If the function is not specified then zero force/torque is
applied to the manipulator joints.
- Interpolation is performed using `ScipY integrate.ode
<https://docs.scipy.org/doc/scipy/reference/generated/scipy.integrate.ode.html>`
- The SciPy RK45 integrator is used by default
- Interpolation is performed using `SciPy interp1
<https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.interp1d.html>`
:seealso: :func:`DHRobot.accel`, :func:`DHRobot.nofriction`,
:func:`DHRobot.rne`.
"""
n = self.n
if not isscalar(T):
raise ValueError('T must be a scalar')
q0 = getvector(q0, n)
if qd0 is None:
qd0 = np.zeros((n, ))
else:
qd0 = getvector(qd0, n)
if torqfun is not None:
if not callable(torqfun):
raise ValueError('torque function must be callable')
if sargs is None:
sargs = {}
if targs is None:
targs = {}
# concatenate q and qd into the initial state vector
x0 = np.r_[q0, qd0]
# get user specified integrator
scipy_integrator = integrate.__dict__[solver]
integrator = scipy_integrator(
lambda t, y: self._fdyn(t, y, torqfun, targs),
t0=0.0,
y0=x0,
t_bound=T,
**sargs)
# initialize list of time and states
tlist = [0]
xlist = [np.r_[q0, qd0]]
if progress:
_printProgressBar(0,
prefix='Progress:',
suffix='complete',
length=60)
while integrator.status == 'running':
# step the integrator, calls _fdyn multiple times
integrator.step()
if integrator.status == 'failed':
raise RuntimeError('integration completed with failed status ')
# stash the results
tlist.append(integrator.t)
xlist.append(integrator.y)
# update the progress bar
if progress:
_printProgressBar(integrator.t / T,
prefix='Progress:',
suffix='complete',
length=60)
# cleanup the progress bar
if progress:
print('\r' + ' ' * 90 + '\r')
tarray = np.array(tlist)
xarray = np.array(xlist)
if dt is not None:
# interpolate data to equal time steps of dt
interp = interpolate.interp1d(tarray, xarray, axis=0)
tnew = np.arange(0, T, dt)
xnew = interp(tnew)
return namedtuple('fdyn', 't q qd')(tnew, xnew[:, :n], xnew[:, n:])
else:
return namedtuple('fdyn', 't q qd')(tarray, xarray[:, :n],
xarray[:, n:])
