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
7672ce5b
Commit
7672ce5b
authored
Sep 27, 2017
by
jcarpent
Browse files
[Spatial] Add dualActionMatrix to Motion class
parent
68982317
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/spatial/motion.hpp
View file @
7672ce5b
...
...
@@ -55,6 +55,7 @@ namespace se3
operator
Vector6
()
const
{
return
toVector
();
}
ActionMatrix_t
toActionMatrix
()
const
{
return
derived
().
toActionMatrix_impl
();
}
ActionMatrix_t
toDualActionMatrix
()
const
{
return
derived
().
toDualActionMatrix_impl
();
}
operator
Matrix6
()
const
{
return
toActionMatrix
();
}
bool
operator
==
(
const
Derived_t
&
other
)
const
{
return
derived
().
isEqual
(
other
);}
...
...
@@ -160,6 +161,16 @@ namespace se3
return
X
;
}
ActionMatrix_t
toDualActionMatrix_impl
()
const
{
ActionMatrix_t
X
;
X
.
template
block
<
3
,
3
>
(
ANGULAR
,
ANGULAR
)
=
X
.
template
block
<
3
,
3
>
(
LINEAR
,
LINEAR
)
=
skew
(
angular_impl
());
X
.
template
block
<
3
,
3
>
(
ANGULAR
,
LINEAR
)
=
skew
(
linear_impl
());
X
.
template
block
<
3
,
3
>
(
LINEAR
,
ANGULAR
).
setZero
();
return
X
;
}
// Getters
ConstAngular_t
angular_impl
()
const
{
return
data
.
template
segment
<
3
>
(
ANGULAR
);
}
...
...
unittest/tspatial.cpp
View file @
7672ce5b
...
...
@@ -134,8 +134,21 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
Motion
vxv
=
bv
.
cross
(
bv
);
BOOST_CHECK_SMALL
(
vxv
.
toVector
().
tail
(
3
).
norm
(),
1e-3
);
//previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
// Test Action Matrix
Motion
v2xv
=
bv2
.
cross
(
bv
);
Motion
::
ActionMatrix_t
actv2
=
bv2
.
toActionMatrix
();
BOOST_CHECK
(
v2xv
.
toVector
().
isApprox
(
actv2
*
bv
.
toVector
()));
// Test Dual Action Matrix
Force
f
(
bv
.
toVector
());
Force
v2xf
=
bv2
.
cross
(
f
);
Motion
::
ActionMatrix_t
dualactv2
=
bv2
.
toDualActionMatrix
();
BOOST_CHECK
(
v2xf
.
toVector
().
isApprox
(
dualactv2
*
f
.
toVector
()));
BOOST_CHECK
(
dualactv2
.
isApprox
(
-
actv2
.
transpose
()));
// Simple test for cross product vxf
Force
f
=
Force
(
bv
.
toVector
());
Force
vxf
=
bv
.
cross
(
f
);
BOOST_CHECK
(
vxf
.
linear
().
isApprox
(
bv
.
angular
().
cross
(
f
.
linear
()),
1e-12
));
BOOST_CHECK_SMALL
(
vxf
.
angular
().
norm
(),
1e-3
);
//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));
...
...
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