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
Guilhem Saurel
pinocchio
Commits
b40e7d15
Commit
b40e7d15
authored
Dec 12, 2016
by
jcarpent
Browse files
[Spatial] Add SE3 method to generate the dual action matrix
parent
6a9490b5
Changes
3
Hide whitespace changes
Inline
Side-by-side
bindings/python/spatial/se3.hpp
View file @
b40e7d15
...
...
@@ -69,8 +69,9 @@ namespace se3
"The translation part of the transformation."
)
.
add_property
(
"homogeneous"
,
&
SE3
::
toHomogeneousMatrix
,
"Returns the homegeneous matrix of *this."
)
.
add_property
(
"action"
,
&
SE3
::
toActionMatrix
,
"Returns the action matrix of *this."
)
.
add_property
(
"homogeneous"
,
&
SE3
::
toHomogeneousMatrix
,
"Returns the homegeneous matrix of *this (acting on SE3)."
)
.
add_property
(
"action"
,
&
SE3
::
toActionMatrix
,
"Returns the action matrix of *this (acting on Motion)."
)
.
add_property
(
"dualAction"
,
&
SE3
::
toDualActionMatrix
,
"Returns the dual action matrix of *this (acting on Force)."
)
.
def
(
"setIdentity"
,
&
SE3PythonVisitor
::
setIdentity
,
"Set *this to the identity placement."
)
.
def
(
"setRandom"
,
&
SE3PythonVisitor
::
setRandom
,
"Set *this to a random placement."
)
...
...
src/spatial/se3.hpp
View file @
b40e7d15
...
...
@@ -77,6 +77,8 @@ namespace se3
return
derived
().
toActionMatrix_impl
();
}
operator
Matrix6
()
const
{
return
toActionMatrix
();
}
Matrix6
toDualActionMatrix
()
const
{
return
derived
().
toDualActionMatrix_impl
();
}
...
...
@@ -254,6 +256,21 @@ namespace se3
B
.
col
(
2
)
=
trans
.
cross
(
rot
.
col
(
2
));
return
M
;
}
Matrix6
toDualActionMatrix_impl
()
const
{
typedef
Eigen
::
Block
<
Matrix6
,
3
,
3
>
Block3
;
Matrix6
M
;
M
.
template
block
<
3
,
3
>(
ANGULAR
,
ANGULAR
)
=
M
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
)
=
rot
;
M
.
template
block
<
3
,
3
>(
LINEAR
,
ANGULAR
).
setZero
();
Block3
B
=
M
.
template
block
<
3
,
3
>(
ANGULAR
,
LINEAR
);
B
.
col
(
0
)
=
trans
.
cross
(
rot
.
col
(
0
));
B
.
col
(
1
)
=
trans
.
cross
(
rot
.
col
(
1
));
B
.
col
(
2
)
=
trans
.
cross
(
rot
.
col
(
2
));
return
M
;
}
void
disp_impl
(
std
::
ostream
&
os
)
const
{
...
...
unittest/tspatial.cpp
View file @
b40e7d15
...
...
@@ -65,10 +65,13 @@ BOOST_AUTO_TEST_CASE ( test_SE3 )
Matrix6
bXc
=
bmc
;
Matrix6
aXc
=
amc
;
BOOST_CHECK
(
aXc
.
isApprox
(
aXb
*
bXc
,
1e-12
));
Matrix6
bXa
=
amb
.
inverse
();
BOOST_CHECK
(
bXa
.
isApprox
(
aXb
.
inverse
(),
1e-12
));
// Test dual action matrix
BOOST_CHECK
(
aXb
.
inverse
().
transpose
().
isApprox
(
amb
.
toDualActionMatrix
(),
1e-12
));
// Test isIdentity
SE3
identity
=
SE3
::
Identity
();
BOOST_CHECK
(
identity
.
isIdentity
());
...
...
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