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:])