Skip to content

Commit adf34ea

Browse files
committed
doco updates
1 parent fc88453 commit adf34ea

File tree

3 files changed

+62
-29
lines changed

3 files changed

+62
-29
lines changed

spatialmath/base/graphics.py

+48-13
Original file line numberDiff line numberDiff line change
@@ -401,11 +401,50 @@ def plot_box(
401401
return r
402402

403403
def plot_arrow(start, end, ax=None, **kwargs):
404+
"""
405+
Plot 2D arrow
406+
407+
:param start: start point, arrow tail
408+
:type start: array_like(2)
409+
:param end: end point, arrow head
410+
:type end: array_like(2)
411+
:param ax: axes to draw into, defaults to None
412+
:type ax: Axes, optional
413+
:param kwargs: argumetns to pass to :class:`matplotlib.patches.Arrow`
414+
415+
Example:
416+
417+
.. runblock:: pycon
418+
419+
>>> from spatialmath.base import plotvol2, plot_arrow
420+
>>> plotvol2(5)
421+
>>> plot_arrow((-2, 2), (3, 4), color='r', width=0.1) # red arrow
422+
"""
404423
ax = axes_logic(ax, 2)
405424

406425
ax.arrow(start[0], start[1], end[0] - start[0], end[1] - start[1], length_includes_head=True, **kwargs)
407426

408427
def plot_polygon(vertices, *fmt, close=False, **kwargs):
428+
"""
429+
Plot polygon
430+
431+
:param vertices: vertices
432+
:type vertices: ndarray(2,N)
433+
:param close: close the polygon, defaults to False
434+
:type close: bool, optional
435+
:param kwargs: arguments passed to Patch
436+
:return: Matplotlib artist
437+
:rtype: line or patch
438+
439+
Example:
440+
441+
.. runblock:: pycon
442+
443+
>>> from spatialmath.base import plotvol2, plot_polygon
444+
>>> plotvol2(5)
445+
>>> vertices = np.array([[-1, 2, -1], [1, 0, -1]])
446+
>>> plot_polygon(vertices, filled=True, facecolor='g') # green filled triangle
447+
"""
409448

410449
if close:
411450
vertices = np.hstack((vertices, vertices[:, [0]]))
@@ -471,7 +510,7 @@ def plot_circle(
471510
:param args:
472511
:param radius: radius of circle
473512
:type radius: float
474-
:param resolution: number of points on circumferece, defaults to 50
513+
:param resolution: number of points on circumference, defaults to 50
475514
:type resolution: int, optional
476515
:return: the matplotlib object
477516
:rtype: list of Line2D or Patch.Polygon
@@ -1168,17 +1207,13 @@ def plotvol2(dim, ax=None, equal=True, grid=False, labels=True):
11681207
11691208
Initialize axes with dimensions given by ``dim`` which can be:
11701209
1171-
* A (scalar), -A:A x -A:A
1172-
* [A,B], A:B x A:B
1173-
* [A,B,C,D], A:B x C:D
1174-
1175-
================== ====== ======
1176-
input xrange yrange
1177-
================== ====== ======
1178-
A (scalar) -A:A -A:A
1179-
[A, B] A:B A:B
1180-
[A, B, C, D, E, F] A:B C:D
1181-
================== ====== ======
1210+
============== ====== ======
1211+
input xrange yrange
1212+
============== ====== ======
1213+
A (scalar) -A:A -A:A
1214+
[A, B] A:B A:B
1215+
[A, B, C, D] A:B C:D
1216+
============== ====== ======
11821217
11831218
:seealso: :func:`plotvol3`, :func:`expand_dims`
11841219
"""
@@ -1259,7 +1294,7 @@ def plotvol3(
12591294

12601295
def expand_dims(dim=None, nd=2):
12611296
"""
1262-
Expact compact axis dimensions
1297+
Expand compact axis dimensions
12631298
12641299
:param dim: dimensions, defaults to None
12651300
:type dim: scalar, array_like(2), array_like(4), array_like(6), optional

spatialmath/base/transforms3d.py

+8-12
Original file line numberDiff line numberDiff line change
@@ -2242,29 +2242,25 @@ def rotvelxform_inv_dot(𝚪, 𝚪d, full=False, representation="rpy/xyz"):
22422242
:return: derivative of inverse angular velocity transformation matrix
22432243
:rtype: ndarray(6,6) or ndarray(3,3)
22442244
2245-
The angular rate transformation matrix :math:`\mat{A}` is such that
2245+
The angular rate transformation matrix :math:`\mat{A} \in \mathbb{R}^{6 \times 6}` is such that
22462246
22472247
.. math::
22482248
22492249
\dvec{x} = \mat{A}^{-1}(\Gamma) \vec{\nu}
22502250
2251-
where :math:`\vec{\Gamma} \in \mathbb{R}^3` is a minimal rotational
2252-
representation and is used to transform a geometric Jacobian to an analytic Jacobians.
2251+
where :math:`\dvec{x} \in \mathbb{R}^6` is analytic velocity :math:`(\vec{v}, \dvec{\Gamma})`,
2252+
:math:`\vec{\nu} \in \mathbb{R}^6` is spatial velocity :math:`(\vec{v}, \vec{\omega})`, and
2253+
:math:`\vec{\Gamma} \in \mathbb{R}^3` is a minimal rotational
2254+
representation.
22532255
22542256
The relationship between spatial and analytic acceleration is
22552257
22562258
.. math::
22572259
2258-
\ddvec{x} = \dmat{A}^{-1}(\Gamma) \vec{\nu} + \mat{A}^{-1}(\Gamma) \dvec{\nu}
2260+
\ddvec{x} = \dmat{A}^{-1}(\Gamma, \dot{\Gamma) \vec{\nu} + \mat{A}^{-1}(\Gamma) \dvec{\nu}
22592261
2260-
which requires
2262+
and :math:`\dmat{A}^{-1}(\Gamma, \dot{\Gamma)` is computed by this function.
22612263
2262-
.. math::
2263-
2264-
\frac{d}{dt} \mat{A}^{-1}(\Gamma) = \mat{A}^{-1}(\Gamma, \dot{\Gamma})
2265-
2266-
This matrix is a function of :math:`\vec{\Gamma}` and :math:`\dvec{\Gamma}`,
2267-
and is also required to compute the derivative of an analytic Jacobian.
22682264
22692265
============================ ========================================
22702266
``representation`` Rotational representation
@@ -2276,7 +2272,7 @@ def rotvelxform_inv_dot(𝚪, 𝚪d, full=False, representation="rpy/xyz"):
22762272
``"exp"`` exponential coordinate rates
22772273
============================ ========================================
22782274
2279-
If ``full=False`` the lower-right 3x3 matrix is returned which transforms
2275+
If ``full=True`` a block diagonal 6x6 matrix is returned which transforms analytic
22802276
analytic rotational acceleration to angular acceleration.
22812277
22822278
Reference:

spatialmath/pose2d.py

+6-4
Original file line numberDiff line numberDiff line change
@@ -251,10 +251,12 @@ def __init__(self, x=None, y=None, theta=None, *, unit='rad', check=True):
251251
"""
252252
Construct new SE(2) object
253253
254-
:param unit: angular units 'deg' or 'rad' [default] if applicable :type
255-
unit: str, optional :param check: check for valid SE(2) elements if
256-
applicable, default to True :type check: bool :return: homogeneous
257-
rigid-body transformation matrix :rtype: SE2 instance
254+
:param unit: angular units 'deg' or 'rad' [default] if applicable
255+
:type unit: str, optional
256+
:param check: check for valid SE(2) elements if applicable, default to True
257+
:type check: bool
258+
:return: SE(2) matrix
259+
:rtype: SE2 instance
258260
259261
- ``SE2()`` is an SE2 instance representing a null motion -- the
260262
identity matrix

0 commit comments

Comments
 (0)