Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit 45daa55

Browse filesBrowse files
petercorkepetercorke
committed
updates
add dependencies spatialvector working enough for inverse dynamics
1 parent fc57ebc commit 45daa55
Copy full SHA for 45daa55

File tree

Expand file treeCollapse file tree

6 files changed

+68
-55
lines changed
Filter options
Expand file treeCollapse file tree

6 files changed

+68
-55
lines changed

‎RELEASE

Copy file name to clipboard
+1-1Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
0.8.3
1+
0.8.4

‎requirements.txt

Copy file name to clipboardExpand all lines: requirements.txt
+1Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,3 +8,4 @@ codecov
88
sphinx
99
sphinx_rtd_theme
1010
matplotlib
11+
ansitable

‎setup.py

Copy file name to clipboardExpand all lines: setup.py
+1-1Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,6 @@
6464

6565
packages=find_packages(exclude=["test_*", "TODO*"]),
6666

67-
install_requires=['numpy', 'scipy', 'matplotlib']
67+
install_requires=['numpy', 'scipy', 'matplotlib', 'colored', 'ansitable']
6868

6969
)

‎spatialmath/smuserlist.py

Copy file name to clipboardExpand all lines: spatialmath/smuserlist.py
+1-1Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,7 @@ def Alloc(cls, n=1):
132132
where ``X`` is any of the SMTB classes.
133133
"""
134134
x = cls()
135-
x.data = x.data * n
135+
x.data = [cls._identity() for i in range(n)] # make n copies of the data
136136
return x
137137

138138
def arghandler(self, arg, convertfrom=(), check=True):

‎spatialmath/spatialvector.py

Copy file name to clipboardExpand all lines: spatialmath/spatialvector.py
+61-49Lines changed: 61 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -12,10 +12,9 @@
1212
"""
1313

1414
from abc import abstractmethod
15-
import spatialmath.base.argcheck as arg
16-
import spatialmath.base as tr
1715
import numpy as np
1816
from spatialmath.smuserlist import SMUserList
17+
import spatialmath.base as base
1918

2019
class SpatialVector(SMUserList):
2120
"""
@@ -75,12 +74,14 @@ def __init__(self, value):
7574

7675
if value is None:
7776
self.data = [np.zeros((6,))]
78-
elif arg.isvector(value, 6):
77+
elif base.isvector(value, 6):
7978
self.data = [np.array(value)]
79+
elif isinstance(value, SpatialVector):
80+
self.data = [value.A]
8081
elif isinstance(value, list):
81-
assert all(map(lambda x: arg.isvector(x, 6), value)), 'all elements of list must have valid shape and value for the class'
82+
assert all(map(lambda x: base.isvector(x, 6), value)), 'all elements of list must have valid shape and value for the class'
8283
self.data = [np.array(x) for x in value]
83-
elif arg.ismatrix(value, (6, None)):
84+
elif base.ismatrix(value, (6, None)):
8485
self.data = [x for x in value.T]
8586
else:
8687
raise ValueError('bad arguments to constructor')
@@ -258,9 +259,9 @@ def cross(self, other):
258259
[0, 0, 0, -v[4], v[3], 0]
259260
])
260261
if isinstance(other, SpatialVelocity):
261-
return SpatialM6(vcross @ other.A) # * operator
262+
return SpatialAcceleration(vcross @ other.A) # * operator
262263
elif isinstance(other, SpatialF6):
263-
return SpatialF6(-vcross @ other.A) # x* operator
264+
return SpatialForce(-vcross @ other.A) # x* operator
264265
else:
265266
raise TypeError('type mismatch')
266267

@@ -279,6 +280,9 @@ class SpatialF6(SpatialVector):
279280
def __init__(self, value):
280281
super().__init__(value)
281282

283+
def dot(self, value):
284+
return np.dot(self.A, base.getvector(value, 6))
285+
282286
# ------------------------------------------------------------------------- #
283287

284288
class SpatialVelocity(SpatialM6):
@@ -298,28 +302,30 @@ class SpatialVelocity(SpatialM6):
298302
def __init__(self, value=None):
299303
super().__init__(value)
300304

301-
def cross(self, other):
302-
r"""
303-
Spatial vector cross product
305+
# def cross(self, other):
306+
# r"""
307+
# Spatial vector cross product
304308

305-
:param other: spatial velocity vector
306-
:type other: SpatialVelocity or SpatialMomentum instance
307-
:return: cross product of spatial vectors
308-
:rtype: SpatialAcceleration instance if ``other`` is SpatialVelocity instance
309-
:rtype: SpatialMomentum instance if ``other`` is SpatialForce instance
309+
# :param other: spatial velocity vector
310+
# :type other: SpatialVelocity or SpatialMomentum instance
311+
# :return: cross product of spatial vectors
312+
# :rtype: SpatialAcceleration instance if ``other`` is SpatialVelocity instance
313+
# :rtype: SpatialMomentum instance if ``other`` is SpatialForce instance
310314

311-
- ``v1.cross(v2)`` is spatial acceleration given spatial velocities
312-
``v1`` and ``v2`` or :math:`\vec{v}_1 \times \vec{v}_2`
313-
- ``v1.cross(m2)`` is spatial force given spatial velocity
314-
``v1`` and spatial momentum ``m2`` or :math:`\vec{v}_1 \times^* \vec{m}_2`
315+
# - ``v1.cross(v2)`` is spatial acceleration given spatial velocities
316+
# ``v1`` and ``v2`` or :math:`\vec{v}_1 \times \vec{v}_2`
317+
# - ``v1.cross(m2)`` is spatial force given spatial velocity
318+
# ``v1`` and spatial momentum ``m2`` or :math:`\vec{v}_1 \times^* \vec{m}_2`
315319

316-
:seealso: :func:`~spatialmath.spatialvector.SpatialM6`, :func:`~spatialmath.spatialvector.SpatialVelocity.__xor__`
317-
"""
318-
return SpatialAcceleration(super().cross(other))
320+
# :seealso: :func:`~spatialmath.spatialvector.SpatialM6`, :func:`~spatialmath.spatialvector.SpatialVelocity.__xor__`
321+
# """
322+
# if not len(self) == 1 or not len(other) == 1:
323+
# raise ValueError("can only perform cross product on single-valued spatial vectors")
324+
# return SpatialAcceleration(super().cross(other))
319325

320-
def __xor__(self, other):
326+
def __matmul__(self, other):
321327
r"""
322-
Overloaded ``^`` operator (superclass method)
328+
Overloaded ``@`` operator (superclass method)
323329
324330
:param other: spatial velocity vector
325331
:type other: SpatialVelocity or SpatialMomentum instance
@@ -334,7 +340,7 @@ def __xor__(self, other):
334340
335341
:seealso: :func:`~spatialmath.spatialvector.SpatialVelocity.cross`
336342
"""
337-
return cross(self, other)
343+
return self.cross(other)
338344

339345
def __rmul(right, left): # pylint: disable=no-self-argument
340346
if isinstance(left, SpatialInertia):
@@ -451,16 +457,17 @@ def __init__(self, m=None, c=None, I=None):
451457
super().__init__()
452458

453459
if m is None and c is None and I is None:
454-
I = np.zeros((6,6))
455-
elif m is None and c is None and I is not None:
456-
I = arg.getmatrix(I, (6,6))
460+
# no arguments
461+
I = SpatialInertia._identity()
462+
elif m is not None and c is None and I is None and base.ismatrix(m, (6,6)):
463+
I = base.getmatrix(m, (6,6))
457464
elif m is not None and c is not None:
458-
c = arg.getvector(c, 3)
465+
c = base.getvector(c, 3)
459466
if I is None:
460467
I = np.zeros((3,3))
461468
else:
462-
I = arg.getmatrix(I, (3,3))
463-
C = tr.skew(c)
469+
I = base.getmatrix(I, (3,3))
470+
C = base.skew(c)
464471
I = np.block([
465472
[m * np.eye(3), m * C.T],
466473
[m * C, I + m * C @ C.T]
@@ -577,28 +584,33 @@ def __rmul__(self, left): # pylint: disable=no-self-argument
577584
import numpy.testing as nt
578585
import pathlib
579586

580-
v = SpatialVelocity()
581-
print(v)
582-
print(len(v))
583-
v.append(v)
584-
print(v)
585-
print(len(v))
586-
587-
I = SpatialInertia()
588-
print(I)
589-
print(len(I))
590-
I.append(I)
591-
print(I)
592-
print(len(I))
593-
594-
z = SpatialForce([1,2,3,4,5,6])
595-
print(z)
596-
z = SpatialMomentum([1,2,3,4,5,6])
597-
print(z)
587+
# v = SpatialVelocity()
588+
# print(v)
589+
# print(len(v))
590+
# v.append(v)
591+
# print(v)
592+
# print(len(v))
593+
594+
vj = SpatialVelocity()
595+
x = vj ^ vj
596+
print(x)
597+
598+
# I = SpatialInertia()
599+
# print(I)
600+
# print(len(I))
601+
# I.append(I)
602+
# print(I)
603+
# print(len(I))
604+
605+
# z = SpatialForce([1,2,3,4,5,6])
606+
# print(z)
607+
# z = SpatialMomentum([1,2,3,4,5,6])
608+
# print(z)
598609

599610
v = SpatialVelocity()
600611
a = SpatialAcceleration()
601612
I = SpatialInertia()
613+
x = I * v
602614
print(I*v)
603615
print(I*a)
604616

‎spatialmath/twist.py

Copy file name to clipboardExpand all lines: spatialmath/twist.py
+3-3Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -559,11 +559,11 @@ def __mul__(left, right): # pylint: disable=no-self-argument
559559
# return Twist(left.S * right)
560560
return Twist3(left.binop(right, lambda x, y: x * y))
561561
elif isinstance(right, SpatialVelocity):
562-
return SpatialVelocity(left.Ad() @ right.V)
562+
return SpatialVelocity(left.Ad() @ right.A)
563563
elif isinstance(right, SpatialAcceleration):
564-
return SpatialAcceleration(left.Ad() @ right.V)
564+
return SpatialAcceleration(left.Ad() @ right.A)
565565
elif isinstance(right, SpatialForce):
566-
return SpatialForce(left.Ad().T @ right.V)
566+
return SpatialForce(left.Ad().T @ right.A)
567567
else:
568568
raise ValueError('twist *, incorrect right operand')
569569

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.