Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gabriele Buondonno
pinocchio
Commits
9e3b335f
Commit
9e3b335f
authored
Mar 15, 2016
by
Guilhem Saurel
Browse files
fix python tests
parent
626a16ca
Changes
3
Hide whitespace changes
Inline
Side-by-side
python/bindings.py
100644 → 100755
View file @
9e3b335f
#!/usr/bin/env python
import
pinocchio
as
se3
from
pinocchio.utils
import
isapprox
,
np
,
npl
,
rand
,
skew
,
zero
...
...
@@ -7,52 +9,52 @@ R = rand([3, 3])
p
=
rand
(
3
)
m
=
se3
.
SE3
(
R
,
p
)
X
=
np
.
vstack
([
np
.
hstack
([
R
,
skew
(
p
)
*
R
]),
np
.
hstack
([
zero
([
3
,
3
]),
R
])])
assert
isapprox
(
m
.
action
()
,
X
)
assert
isapprox
(
m
.
action
,
X
)
M
=
np
.
vstack
([
np
.
hstack
([
R
,
p
]),
np
.
matrix
(
'0 0 0 1'
,
np
.
double
)])
assert
isapprox
(
m
.
homogeneous
()
,
M
)
assert
isapprox
(
m
.
homogeneous
,
M
)
m2
=
se3
.
SE3
.
Random
()
assert
isapprox
((
m
*
m2
).
homogeneous
()
,
m
.
homogeneous
()
*
m2
.
homogeneous
()
)
assert
isapprox
((
~
m
).
homogeneous
()
,
npl
.
inv
(
m
.
homogeneous
()
))
assert
isapprox
((
m
*
m2
).
homogeneous
,
m
.
homogeneous
*
m2
.
homogeneous
)
assert
isapprox
((
~
m
).
homogeneous
,
npl
.
inv
(
m
.
homogeneous
))
p
=
rand
(
3
)
assert
isapprox
(
m
*
p
,
m
.
rotation
*
p
+
m
.
translation
)
assert
isapprox
(
m
.
actInv
(
p
),
m
.
rotation
.
T
*
p
-
m
.
rotation
.
T
*
m
.
translation
)
p
=
np
.
vstack
([
p
,
1
])
assert
isapprox
(
m
*
p
,
m
.
homogeneous
()
*
p
)
assert
isapprox
(
m
.
actInv
(
p
),
npl
.
inv
(
m
.
homogeneous
()
)
*
p
)
assert
isapprox
(
m
*
p
,
m
.
homogeneous
*
p
)
assert
isapprox
(
m
.
actInv
(
p
),
npl
.
inv
(
m
.
homogeneous
)
*
p
)
p
=
rand
(
6
)
assert
isapprox
(
m
*
p
,
m
.
action
()
*
p
)
assert
isapprox
(
m
.
actInv
(
p
),
npl
.
inv
(
m
.
action
()
)
*
p
)
assert
isapprox
(
m
*
p
,
m
.
action
*
p
)
assert
isapprox
(
m
.
actInv
(
p
),
npl
.
inv
(
m
.
action
)
*
p
)
# --- Motion
assert
isapprox
(
se3
.
Motion
.
Zero
().
vector
()
,
zero
(
6
))
assert
isapprox
(
se3
.
Motion
.
Zero
().
vector
,
zero
(
6
))
v
=
se3
.
Motion
.
Random
()
assert
isapprox
((
m
*
v
).
vector
()
,
m
.
action
()
*
v
.
vector
()
)
assert
isapprox
((
m
.
actInv
(
v
)).
vector
()
,
npl
.
inv
(
m
.
action
()
)
*
v
.
vector
()
)
assert
isapprox
((
m
*
v
).
vector
,
m
.
action
*
v
.
vector
)
assert
isapprox
((
m
.
actInv
(
v
)).
vector
,
npl
.
inv
(
m
.
action
)
*
v
.
vector
)
vv
=
v
.
linear
vw
=
v
.
angular
assert
isapprox
(
v
.
vector
()
,
np
.
vstack
([
vv
,
vw
]))
assert
isapprox
((
v
**
v
).
vector
()
,
zero
(
6
))
assert
isapprox
(
v
.
vector
,
np
.
vstack
([
vv
,
vw
]))
assert
isapprox
((
v
**
v
).
vector
,
zero
(
6
))
# --- Force ---
assert
isapprox
(
se3
.
Force
.
Zero
().
vector
()
,
zero
(
6
))
assert
isapprox
(
se3
.
Force
.
Zero
().
vector
,
zero
(
6
))
f
=
se3
.
Force
.
Random
()
ff
=
f
.
linear
ft
=
f
.
angular
assert
isapprox
(
f
.
vector
()
,
np
.
vstack
([
ff
,
ft
]))
assert
isapprox
(
f
.
vector
,
np
.
vstack
([
ff
,
ft
]))
assert
isapprox
((
m
*
f
).
vector
()
,
npl
.
inv
(
m
.
action
()
.
T
)
*
f
.
vector
()
)
assert
isapprox
((
m
.
actInv
(
f
)).
vector
()
,
m
.
action
()
.
T
*
f
.
vector
()
)
f
=
se3
.
Force
(
np
.
vstack
([
v
.
vector
()
[
3
:],
v
.
vector
()
[:
3
]]))
assert
isapprox
((
v
**
f
).
vector
()
,
zero
(
6
))
assert
isapprox
((
m
*
f
).
vector
,
npl
.
inv
(
m
.
action
.
T
)
*
f
.
vector
)
assert
isapprox
((
m
.
actInv
(
f
)).
vector
,
m
.
action
.
T
*
f
.
vector
)
f
=
se3
.
Force
(
np
.
vstack
([
v
.
vector
[
3
:],
v
.
vector
[:
3
]]))
assert
isapprox
((
v
**
f
).
vector
,
zero
(
6
))
# --- Inertia ---
Y1
=
se3
.
Inertia
.
Random
()
Y2
=
se3
.
Inertia
.
Random
()
Y
=
Y1
+
Y2
assert
isapprox
(
Y1
.
matrix
()
+
Y2
.
matrix
(),
Y
.
matrix
())
assert
isapprox
((
Y
*
v
).
vector
()
,
Y
.
matrix
()
*
v
.
vector
()
)
assert
isapprox
((
m
*
Y
).
matrix
(),
m
.
inverse
().
action
()
.
T
*
Y
.
matrix
()
*
m
.
inverse
().
action
()
)
assert
isapprox
((
m
.
actInv
(
Y
)).
matrix
(),
m
.
action
()
.
T
*
Y
.
matrix
()
*
m
.
action
()
)
assert
isapprox
((
Y
*
v
).
vector
,
Y
.
matrix
()
*
v
.
vector
)
assert
isapprox
((
m
*
Y
).
matrix
(),
m
.
inverse
().
action
.
T
*
Y
.
matrix
()
*
m
.
inverse
().
action
)
assert
isapprox
((
m
.
actInv
(
Y
)).
matrix
(),
m
.
action
.
T
*
Y
.
matrix
()
*
m
.
action
)
python/test_model.py
100644 → 100755
View file @
9e3b335f
#!/usr/bin/env python
import
pinocchio
as
se3
from
pinocchio.utils
import
isapprox
,
np
,
zero
...
...
@@ -15,7 +17,7 @@ assert model.parents[0] == 0 and model.parents[1] == 0
model
.
parents
[
2
]
=
model
.
parents
[
1
]
assert
model
.
parents
[
2
]
==
model
.
parents
[
1
]
assert
model
.
names
[
0
]
==
"universe"
assert
isapprox
(
model
.
gravity
.
np
,
np
.
matrix
(
'0
;
0
;
-9.81
;
0
;
0
;
0'
))
assert
isapprox
(
model
.
gravity
.
np
,
np
.
matrix
(
'0 0 -9.81 0 0 0'
)
.
T
)
data
=
model
.
createData
()
...
...
@@ -29,5 +31,5 @@ for i in range(model.nbody):
se3
.
rnea
(
model
,
data
,
q
,
qdot
,
qddot
)
for
i
in
range
(
model
.
nbody
):
assert
isapprox
(
data
.
v
[
i
].
np
,
zero
(
6
))
assert
isapprox
(
data
.
a
[
0
].
np
,
-
model
.
gravity
.
np
)
assert
isapprox
(
data
.
f
[
-
1
],
model
.
inertias
[
-
1
]
*
data
.
a
[
-
1
])
assert
isapprox
(
data
.
a
_gf
[
0
].
np
,
-
model
.
gravity
.
np
)
assert
isapprox
(
data
.
f
[
-
1
],
model
.
inertias
[
-
1
]
*
data
.
a
_gf
[
-
1
])
src/python/__init__.py
View file @
9e3b335f
...
...
@@ -34,9 +34,9 @@ def SE3act(m, x):
if
x
.
shape
[
0
]
==
3
:
return
m
.
rotation
*
x
+
m
.
translation
if
x
.
shape
[
0
]
==
4
:
return
m
.
homogeneous
()
*
x
return
m
.
homogeneous
*
x
if
x
.
shape
[
0
]
==
6
:
return
m
.
action
()
*
x
return
m
.
action
*
x
raise
ValueError
(
'Error: m can only act on linear object of size 3, 4 and 6.'
)
if
'se3Action'
in
x
.
__class__
.
__dict__
:
return
x
.
se3Action
(
m
)
...
...
@@ -53,9 +53,9 @@ def SE3actinv(m, x):
if
x
.
shape
[
0
]
==
3
:
return
m
.
rotation
.
T
*
x
-
m
.
rotation
.
T
*
m
.
translation
if
x
.
shape
[
0
]
==
4
:
return
m
.
inverse
().
homogeneous
()
*
x
return
m
.
inverse
().
homogeneous
*
x
if
x
.
shape
[
0
]
==
6
:
return
m
.
inverse
().
action
()
*
x
return
m
.
inverse
().
action
*
x
raise
ValueError
(
'Error: m can only act on linear object of size 3, 4 and 6.'
)
if
'se3Action'
in
x
.
__class__
.
__dict__
:
return
x
.
se3ActionInverse
(
m
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment