Skip to content
GitLab
Menu
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
9d42031d
Commit
9d42031d
authored
Mar 17, 2016
by
Justin Carpentier
Browse files
Merge pull request #133 from nim65s/devel
[Python] Some propositions about Format & Tests
parents
c2bc2f56
a9c02fa1
Changes
11
Hide whitespace changes
Inline
Side-by-side
.travis.yml
View file @
9d42031d
language
:
cpp
language
:
python
python
:
-
"
2.7"
sudo
:
required
compiler
:
-
gcc
...
...
@@ -28,6 +30,9 @@ script:
-
export CMAKE_ADDITIONAL_OPTIONS="-DCMAKE_BUILD_TYPE=${BUILDTYPE}"
-
sudo free -m -t
-
travis_wait ./.travis/run ../travis_custom/custom_build
-
export PYTHONPATH=/tmp/_ci/install/lib/python2.7/site-packages
-
python ./python/bindings.py
-
python ./python/test_model.py
after_success
:
./.travis/run after_success
after_failure
:
./.travis/run after_failure
before_install
:
./travis_custom/custom_before_install
lab/python/exp_integration.py
View file @
9d42031d
...
...
@@ -5,11 +5,12 @@ interpolating SE(3) movements.
'''
import
robotviewer
viewer
=
robotviewer
.
client
(
'XML-RPC'
)
viewer
=
robotviewer
.
client
(
'XML-RPC'
)
try
:
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,[
1
,
0
,
0
,
0
,
0
,
0
])
except
:
viewer
.
updateElementConfig
=
lambda
a
,
b
:
a
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
[
1
,
0
,
0
,
0
,
0
,
0
])
except
:
viewer
.
updateElementConfig
=
lambda
a
,
b
:
a
def
se3ToRpy
(
m
):
return
M
.
translation
.
T
.
tolist
()[
0
]
+
matrixToRpy
(
M
.
rotation
).
T
.
tolist
()[
0
]
...
...
@@ -23,62 +24,63 @@ N = 1000
M
=
se3
.
SE3
.
Identity
()
# Integrate a constant body velocity.
v
=
zero
(
3
);
v
[
2
]
=
1.0
/
N
w
=
zero
(
3
);
w
[
1
]
=
1.0
/
N
nu
=
se3
.
Motion
(
v
,
w
)
v
=
zero
(
3
)
v
[
2
]
=
1.0
/
N
w
=
zero
(
3
)
w
[
1
]
=
1.0
/
N
nu
=
se3
.
Motion
(
v
,
w
)
for
i
in
range
(
N
):
M
=
se3
.
exp
(
nu
)
*
M
M
=
se3
.
exp
(
nu
)
*
M
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
se3ToRpy
(
M
))
time
.
sleep
(
1
)
# Integrate a velocity of the body that is constant in the world frame.
for
i
in
range
(
N
):
Mc
=
se3
.
SE3
(
M
.
rotation
,
zero
(
3
))
M
=
M
*
se3
.
exp
(
Mc
.
actInv
(
nu
))
Mc
=
se3
.
SE3
(
M
.
rotation
,
zero
(
3
))
M
=
M
*
se3
.
exp
(
Mc
.
actInv
(
nu
))
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
se3ToRpy
(
M
))
time
.
sleep
(
1
)
# Integrate a constant "log" velocity in body frame.
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.7
,
-
0.6
,
0.1
,
0.4
).
normalized
().
matrix
(),
np
.
matrix
([
1
,
-
1
,
2
],
np
.
double
).
T
)
nu
=
se3
.
Motion
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
/
N
)
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.7
,
-
0.6
,
0.1
,
0.4
).
normalized
().
matrix
(),
np
.
matrix
([
1
,
-
1
,
2
],
np
.
double
).
T
)
nu
=
se3
.
Motion
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
/
N
)
for
i
in
range
(
N
):
M
=
M
*
se3
.
exp
(
nu
)
M
=
M
*
se3
.
exp
(
nu
)
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
se3ToRpy
(
M
))
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
)
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
())
time
.
sleep
(
1
)
# Integrate a constant "log" velocity in reference frame.
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.3
,
-
0.2
,
0.6
,
0.5
).
normalized
().
matrix
(),
np
.
matrix
([
-
1
,
1
,
0.6
],
np
.
double
).
T
)
nu
=
se3
.
Motion
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
/
N
)
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.3
,
-
0.2
,
0.6
,
0.5
).
normalized
().
matrix
(),
np
.
matrix
([
-
1
,
1
,
0.6
],
np
.
double
).
T
)
nu
=
se3
.
Motion
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
/
N
)
for
i
in
range
(
N
):
M
=
M
*
se3
.
exp
(
nu
)
M
=
M
*
se3
.
exp
(
nu
)
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
se3ToRpy
(
M
))
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
)
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
())
time
.
sleep
(
1
)
# Integrate an exponential decay vector field toward ME.
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.9
,
-
0.1
,
0.1
,
0.1
).
normalized
().
matrix
(),
np
.
matrix
([
1
,
0.2
,
1.9
],
np
.
double
).
T
)
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.9
,
-
0.1
,
0.1
,
0.1
).
normalized
().
matrix
(),
np
.
matrix
([
1
,
0.2
,
1.9
],
np
.
double
).
T
)
for
i
in
range
(
N
):
nu
=
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
*
1e-2
M
=
M
*
se3
.
exp
(
nu
)
nu
=
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
*
1e-2
M
=
M
*
se3
.
exp
(
nu
)
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
se3ToRpy
(
M
))
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
)
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
())
time
.
sleep
(
1
)
# Integrate a straight-line vector field toward ME.
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.1
,
-
0.6
,
0.6
,
0.1
).
normalized
().
matrix
(),
np
.
matrix
([.
1
,
-
1.2
,
0.3
],
np
.
double
).
T
)
ME
=
se3
.
SE3
(
se3
.
Quaternion
(
0.1
,
-
0.6
,
0.6
,
0.1
).
normalized
().
matrix
(),
np
.
matrix
([.
1
,
-
1.2
,
0.3
],
np
.
double
).
T
)
for
i
in
range
(
N
):
ERR
=
ME
.
inverse
()
*
M
v
=
ERR
.
rotation
.
T
*
ERR
.
translation
*
-
9e-3
w
=
ERR
.
rotation
.
T
*
se3
.
log
(
ERR
.
rotation
)
*
-
9e-3
nu
=
se3
.
Motion
(
v
,
w
)
M
=
M
*
se3
.
exp
(
nu
)
ERR
=
ME
.
inverse
()
*
M
v
=
ERR
.
rotation
.
T
*
ERR
.
translation
*
-
9e-3
w
=
ERR
.
rotation
.
T
*
se3
.
log
(
ERR
.
rotation
)
*
-
9e-3
nu
=
se3
.
Motion
(
v
,
w
)
M
=
M
*
se3
.
exp
(
nu
)
viewer
.
updateElementConfig
(
'RomeoTrunkYaw'
,
se3ToRpy
(
M
))
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
()
)
print
"Residuals = "
,
norm
(
se3
.
log
(
M
.
inverse
()
*
ME
).
vector
())
time
.
sleep
(
1
)
lab/python/kineinv.py
View file @
9d42031d
...
...
@@ -4,7 +4,8 @@ abscissia), dimension 3 (x,y,z coordinates), dimension 6 (SE3 exp integration)
and dimension 6 (R3xSO3 integration, straightline in the cartesian space).
'''
def
kineinv_dim1
(
q0
,
xdes
,
N
=
100
):
def
kineinv_dim1
(
q0
,
xdes
,
N
=
100
):
'''
Bring the robot hand to X=xdes, using a gradient descent (which is also a
Gauss-Newton descent in this trivial case).
...
...
@@ -13,17 +14,18 @@ def kineinv_dim1(q0,xdes,N=100):
for
i
in
range
(
N
):
Mrh
=
robot
.
Mrh
(
q
)
e
=
Mrh
.
translation
[
0
,
0
]
-
xdes
J
=
Mrh
.
rotation
*
robot
.
Jrh
(
q
)[:
3
,:]
J
=
J
[
0
,:]
e
=
Mrh
.
translation
[
0
,
0
]
-
xdes
J
=
Mrh
.
rotation
*
robot
.
Jrh
(
q
)[:
3
,
:]
J
=
J
[
0
,
:]
qdot
=
-
J
.
T
*
e
qdot
=
-
J
.
T
*
e
robot
.
increment
(
q
,
qdot
*
5e-2
)
robot
.
increment
(
q
,
qdot
*
5e-2
)
robot
.
display
(
q
)
print
"Residuals = "
,
robot
.
Mrh
(
q
).
translation
[
0
,
0
]
-
xdes
print
"Residuals = "
,
robot
.
Mrh
(
q
).
translation
[
0
,
0
]
-
xdes
def
kineinv_dim3
(
q0
,
xdes
,
N
=
100
):
def
kineinv_dim3
(
q0
,
xdes
,
N
=
100
):
'''
Bring the robot hand to [x,y,z]=xdes using Gauss-Newton descent. Arg q0 is
the initial robot position (np.matrix Nx1); xdes is the desired position
...
...
@@ -33,16 +35,17 @@ def kineinv_dim3(q0,xdes,N=100):
for
i
in
range
(
N
):
Mrh
=
robot
.
Mrh
(
q
)
e
=
Mrh
.
translation
[
0
:
3
,
0
]
-
xdes
J
=
Mrh
.
rotation
*
robot
.
Jrh
(
q
)[:
3
,:]
e
=
Mrh
.
translation
[:
3
,
0
]
-
xdes
J
=
Mrh
.
rotation
*
robot
.
Jrh
(
q
)[:
3
,
:]
qdot
=
-
npl
.
pinv
(
J
)
*
e
qdot
=
-
npl
.
pinv
(
J
)
*
e
robot
.
increment
(
q
,
qdot
*
5e-2
)
robot
.
increment
(
q
,
qdot
*
5e-2
)
robot
.
display
(
q
)
print
"Residuals = "
,
npl
.
norm
(
robot
.
Mrh
(
q
).
translation
[:
3
]
-
xdes
)
print
"Residuals = "
,
npl
.
norm
(
robot
.
Mrh
(
q
).
translation
[:
3
]
-
xdes
)
def
kineinv_dim6
(
q0
,
Mdes
,
straight
=
True
,
N
=
100
):
def
kineinv_dim6
(
q0
,
Mdes
,
straight
=
True
,
N
=
100
):
'''
Bring the robot hand to the reference placement (position+orientation) Mdes
using Gauss-Newton descent. Arg q0 is the initial robot position (np.matrix
...
...
@@ -55,30 +58,31 @@ def kineinv_dim6(q0,Mdes,straight=True,N=100):
q
=
np
.
copy
(
q0
)
for
i
in
range
(
100
):
Mrh
=
robot
.
Mrh
(
q
)
if
straight
:
ERR
=
Mdes
.
inverse
()
*
Mrh
v
=
ERR
.
rotation
.
T
*
ERR
.
translation
*
-
1
w
=
ERR
.
rotation
.
T
*
se3
.
log
(
ERR
.
rotation
)
*
-
1
nu
=
se3
.
Motion
(
v
,
w
)
ERR
=
Mdes
.
inverse
()
*
Mrh
v
=
-
ERR
.
rotation
.
T
*
ERR
.
translation
w
=
-
ERR
.
rotation
.
T
*
se3
.
log
(
ERR
.
rotation
)
nu
=
se3
.
Motion
(
v
,
w
)
else
:
nu
=
se3
.
log
(
Mrh
.
inverse
()
*
Mdes
)
nu
=
se3
.
log
(
Mrh
.
inverse
()
*
Mdes
)
Jrh
=
robot
.
Jrh
(
q
)
qdot
=
npl
.
pinv
(
Jrh
)
*
nu
.
vector
()
robot
.
increment
(
q
,
qdot
*
5e-2
)
qdot
=
npl
.
pinv
(
Jrh
)
*
nu
.
vector
()
robot
.
increment
(
q
,
qdot
*
5e-2
)
robot
.
display
(
q
)
print
"Residuals = "
,
npl
.
norm
(
se3
.
log
(
robot
.
Mrh
(
q
).
inverse
()
*
Mdes
).
vector
())
print
"Residuals = "
,
npl
.
norm
(
se3
.
log
(
robot
.
Mrh
(
q
).
inverse
()
*
Mdes
).
vector
())
# --- MAIN ------------------------------------------------------
import
pinocchio
as
se3
from
pinocchio.utils
import
*
if
__name__
==
'__main__'
:
import
pinocchio
as
se3
from
pinocchio.utils
import
*
robot
=
se3
.
RobotWrapper
(
'../../models/romeo.urdf'
)
robot
.
initDisplay
()
robot
=
se3
.
RobotWrapper
(
'../../models/romeo.urdf'
)
robot
.
initDisplay
()
kineinv_dim1
(
robot
.
q0
,
2.0
)
kineinv_dim3
(
robot
.
q0
,
np
.
matrix
([
2.0
,
0.5
,
1.0
]).
T
)
kineinv_dim6
(
robot
.
q0
,
se3
.
SE3
(
eye
(
3
),
np
.
matrix
([
2.0
,
0.5
,
1.0
]).
T
),
straight
=
False
)
kineinv_dim6
(
robot
.
q0
,
se3
.
SE3
(
eye
(
3
),
np
.
matrix
([
2.0
,
0.5
,
1.0
]).
T
),
straight
=
True
)
kineinv_dim1
(
robot
.
q0
,
2.0
)
kineinv_dim3
(
robot
.
q0
,
np
.
matrix
([
2.0
,
0.5
,
1.0
]).
T
)
kineinv_dim6
(
robot
.
q0
,
se3
.
SE3
(
eye
(
3
),
np
.
matrix
([
2.0
,
0.5
,
1.0
]).
T
),
straight
=
False
)
kineinv_dim6
(
robot
.
q0
,
se3
.
SE3
(
eye
(
3
),
np
.
matrix
([
2.0
,
0.5
,
1.0
]).
T
),
straight
=
True
)
python/bindings.py
100644 → 100755
View file @
9d42031d
#!/usr/bin/env python
import
pinocchio
as
se3
from
pinocchio.utils
import
*
from
pinocchio.utils
import
isapprox
,
np
,
npl
,
rand
,
skew
,
zero
# --- SE3
R
=
rand
([
3
,
3
]);
[
R
,
tmp
,
tmp
]
=
npl
.
svd
(
R
)
R
=
rand
([
3
,
3
])
[
R
,
tmp
,
tmp
]
=
npl
.
svd
(
R
)
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
)
)
M
=
np
.
vstack
(
[
np
.
hstack
([
R
,
p
]),
np
.
matrix
(
'0 0 0 1'
,
np
.
double
)
]
)
assert
(
isapprox
(
m
.
homogeneous
(),
M
)
)
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
)
M
=
np
.
vstack
(
[
np
.
hstack
([
R
,
p
]),
np
.
matrix
(
'0 0 0 1'
,
np
.
double
)
]
)
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
)
)
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
)
)
p
=
np
.
vstack
([
p
,
1
])
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
()))
vv
=
v
.
linear
;
vw
=
v
.
angular
assert
(
isapprox
(
v
.
vector
(),
np
.
vstack
([
vv
,
vw
])
))
assert
(
isapprox
((
v
**
v
).
vector
(),
zero
(
6
)))
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
))
# --- 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
])
))
ff
=
f
.
linear
ft
=
f
.
angular
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
()))
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
)
python/test_model.py
100644 → 100755
View file @
9d42031d
#!/usr/bin/env python
import
pinocchio
as
se3
from
pinocchio.utils
import
*
from
pinocchio.utils
import
isapprox
,
np
,
zero
model
=
se3
.
Model
.
BuildEmptyModel
()
assert
(
model
.
nbody
==
1
and
model
.
nq
==
0
and
model
.
nv
==
0
)
assert
model
.
nbody
==
1
and
model
.
nq
==
0
and
model
.
nv
==
0
model
=
se3
.
Model
.
BuildHumanoidSimple
()
nb
=
28
# We should have 28 bodies, thus 27 joints, one of them a free-flyer.
assert
(
model
.
nbody
==
nb
and
model
.
nq
==
nb
-
1
+
6
and
model
.
nv
==
nb
-
1
+
5
)
nb
=
28
# We should have 28 bodies, thus 27 joints, one of them a free-flyer.
assert
model
.
nbody
==
nb
and
model
.
nq
==
nb
-
1
+
6
and
model
.
nv
==
nb
-
1
+
5
model
.
inertias
[
1
]
=
model
.
inertias
[
2
]
assert
(
isapprox
(
model
.
inertias
[
1
].
np
,
model
.
inertias
[
2
].
np
)
)
assert
isapprox
(
model
.
inertias
[
1
].
np
,
model
.
inertias
[
2
].
np
)
model
.
jointPlacements
[
1
]
=
model
.
jointPlacements
[
2
]
assert
(
isapprox
(
model
.
jointPlacements
[
1
].
np
,
model
.
jointPlacements
[
2
].
np
)
)
assert
(
model
.
parents
[
0
]
==
0
and
model
.
parents
[
1
]
==
0
)
assert
isapprox
(
model
.
jointPlacements
[
1
].
np
,
model
.
jointPlacements
[
2
].
np
)
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
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'
)
.
T
)
data
=
model
.
createData
()
...
...
@@ -23,10 +25,11 @@ data = model.createData()
q
=
zero
(
model
.
nq
)
qdot
=
zero
(
model
.
nv
)
qddot
=
zero
(
model
.
nv
)
for
i
in
range
(
model
.
nbody
):
data
.
a
[
i
]
=
se3
.
Motion
.
Zero
()
for
i
in
range
(
model
.
nbody
):
data
.
a
[
i
]
=
se3
.
Motion
.
Zero
()
se3
.
rnea
(
model
,
data
,
q
,
qdot
,
qddot
)
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
.
v
[
i
].
np
,
zero
(
6
))
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 @
9d42031d
...
...
@@ -15,61 +15,63 @@
# <http://www.gnu.org/licenses/>.
import
numpy
as
np
from
pinocchio.robot_wrapper
import
RobotWrapper
import
libpinocchio_pywrap
as
se3
import
utils
from
explog
import
exp
,
log
from
libpinocchio_pywrap
import
*
se3
.
SE3
.
__repr__
=
se3
.
SE3
.
__str__
se3
.
Motion
.
__repr__
=
se3
.
Motion
.
__str__
se3
.
AngleAxis
.
__repr__
=
lambda
s
:
'AngleAxis('
+
s
.
vector
().
__str__
()
+
')'
se3
.
AngleAxis
.
__repr__
=
lambda
s
:
'AngleAxis(%s)'
%
s
.
vector
()
# --- SE3 action ---
def
SE3act
(
m
,
x
):
assert
(
isinstance
(
m
,
se3
.
SE3
)
)
if
isinstance
(
x
,
np
.
ndarray
):
if
x
.
shape
[
0
]
==
3
:
return
m
.
rotation
*
x
+
m
.
translation
el
if
x
.
shape
[
0
]
==
4
:
return
m
.
homogeneous
()
*
x
el
if
x
.
shape
[
0
]
==
6
:
return
m
.
action
()
*
x
else
:
raise
Exception
(
'Error: m can only act on linear object of size 3, 4 and 6.'
)
el
if
'se3Action'
in
x
.
__class__
.
__dict__
:
def
SE3act
(
m
,
x
):
assert
isinstance
(
m
,
se3
.
SE3
)
if
isinstance
(
x
,
np
.
ndarray
):
if
x
.
shape
[
0
]
==
3
:
return
m
.
rotation
*
x
+
m
.
translation
if
x
.
shape
[
0
]
==
4
:
return
m
.
homogeneous
*
x
if
x
.
shape
[
0
]
==
6
:
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
)
else
:
#print 'Error: SE3 cannot act on the given object'
return
m
.
oldmult
(
x
)
setattr
(
se3
.
SE3
,
'oldmult'
,
se3
.
SE3
.
__mul__
)
setattr
(
se3
.
SE3
,
'__mul__
'
,
SE3act
)
setattr
(
se3
.
SE3
,
'act'
,
SE3act
)
return
m
.
oldmult
(
x
)
setattr
(
se3
.
SE3
,
'oldmult'
,
se3
.
SE3
.
__mul__
)
setattr
(
se3
.
SE3
,
'__mul__'
,
SE3act
)
setattr
(
se3
.
SE3
,
'act
'
,
SE3act
)
def
SE3actinv
(
m
,
x
):
assert
(
isinstance
(
m
,
se3
.
SE3
)
)
if
isinstance
(
x
,
np
.
ndarray
):
if
x
.
shape
[
0
]
==
3
:
return
m
.
rotation
.
T
*
x
-
m
.
rotation
.
T
*
m
.
translation
el
if
x
.
shape
[
0
]
==
4
:
return
m
.
inverse
().
homogeneous
()
*
x
el
if
x
.
shape
[
0
]
==
6
:
return
m
.
inverse
().
action
()
*
x
else
:
raise
Exception
(
'Error: m can only act on linear object of size 3, 4 and 6.'
)
el
if
'se3Action'
in
x
.
__class__
.
__dict__
:
def
SE3actinv
(
m
,
x
):
assert
isinstance
(
m
,
se3
.
SE3
)
if
isinstance
(
x
,
np
.
ndarray
):
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
if
x
.
shape
[
0
]
==
6
:
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
)
else
:
print
'Error: SE3 cannot act on the given object'
setattr
(
se3
.
SE3
,
'actInv'
,
SE3actinv
)
raise
ValueError
(
'Error: SE3 cannot act on the given object'
)
setattr
(
se3
.
SE3
,
'actInv'
,
SE3actinv
)
# --- M6/F6 cross product --
def
SE3cross
(
self
,
y
):
assert
(
isinstance
(
self
,
se3
.
Motion
)
)
if
isinstance
(
y
,
se3
.
Motion
):
def
SE3cross
(
self
,
y
):
assert
isinstance
(
self
,
se3
.
Motion
)
if
isinstance
(
y
,
se3
.
Motion
):
return
self
.
cross_motion
(
y
)
el
if
isinstance
(
y
,
se3
.
Force
):
if
isinstance
(
y
,
se3
.
Force
):
return
self
.
cross_force
(
y
)
else
:
raise
Exception
(
'Error: SE3 cross product only apply on M6xM6 or M6xF6.'
)
setattr
(
se3
.
Motion
,
'__pow__'
,
SE3cross
)
setattr
(
se3
.
Motion
,
'cross'
,
SE3cross
)
from
libpinocchio_pywrap
import
*
from
pinocchio.robot_wrapper
import
RobotWrapper
from
explog
import
exp
,
log
raise
ValueException
(
'Error: SE3 cross product only apply on M6xM6 or M6xF6.'
)
setattr
(
se3
.
Motion
,
'__pow__'
,
SE3cross
)
setattr
(
se3
.
Motion
,
'cross'
,
SE3cross
)
src/python/explog.py
View file @
9d42031d
...
...
@@ -19,23 +19,32 @@ import libpinocchio_pywrap as se3
import
numpy
as
np
import
math
def
exp
(
x
):
if
isinstance
(
x
,
se3
.
Motion
):
return
se3
.
exp6FromMotion
(
x
)
elif
np
.
isscalar
(
x
):
return
math
.
exp
(
x
)
elif
isinstance
(
x
,
np
.
matrix
):
if
len
(
x
)
==
6
:
return
se3
.
exp6FromVector
(
x
)
elif
len
(
x
)
==
3
:
return
se3
.
exp3
(
x
)
else
:
print
'Error only 3 and 6 vectors are allowed.'
else
:
print
'Error exp is only defined for real, vector3, vector6 and se3.Motion objects.'
if
isinstance
(
x
,
se3
.
Motion
):
return
se3
.
exp6FromMotion
(
x
)
if
np
.
isscalar
(
x
):
return
math
.
exp
(
x
)
if
isinstance
(
x
,
np
.
matrix
):
if
len
(
x
)
==
6
:
return
se3
.
exp6FromVector
(
x
)
if
len
(
x
)
==
3
:
return
se3
.
exp3
(
x
)
raise
ValueError
(
'Error only 3 and 6 vectors are allowed.'
)
raise
ValueError
(
'Error exp is only defined for real, vector3, vector6 and se3.Motion objects.'
)
def
log
(
x
):
if
isinstance
(
x
,
se3
.
SE3
):
return
se3
.
log6FromSE3
(
x
)
elif
np
.
isscalar
(
x
):
return
math
.
log
(
x
)
elif
isinstance
(
x
,
np
.
matrix
):
if
len
(
x
)
==
4
:
return
se3
.
log6FromMatrix
(
x
)
elif
len
(
x
)
==
3
:
return
se3
.
log3
(
x
)
else
:
print
'Error only 3 and 4 matrices are allowed.'
else
:
print
'Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.'
def
log
(
x
):
if
isinstance
(
x
,
se3
.
SE3
):
return
se3
.
log6FromSE3
(
x
)
if
np
.
isscalar
(
x
):
return
math
.
log
(
x
)
if
isinstance
(
x
,
np
.
matrix
):
if
len
(
x
)
==
4
:
return
se3
.
log6FromMatrix
(
x
)
if
len
(
x
)
==
3
:
return
se3
.
log3
(
x
)
raise
ValueError
(
'Error only 3 and 4 matrices are allowed.'
)
raise
ValueError
(
'Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.'
)
__all__
=
[
'exp'
,
'log'
]
__all__
=
[
'exp'
,
'log'
]
src/python/robot_wrapper.py
View file @
9d42031d
...
...
@@ -14,43 +14,44 @@
# Pinocchio If not, see
# <http://www.gnu.org/licenses/>.
import
numpy
as
np
import
time
import
libpinocchio_pywrap
as
se3
import
utils
from
explog
import
exp
import
time
class
RobotWrapper
:
def
__init__
(
self
,
filename
,
package_dirs
=
None
,
root_joint
=
None
):
if
(
root_joint
is
None
):