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
28cfaad1
Commit
28cfaad1
authored
Sep 27, 2017
by
jcarpent
Browse files
[Spatial] Add time variation of a Inertia according to the current spatial velocity of a the body
parent
2e414ab5
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/spatial/inertia.hpp
View file @
28cfaad1
//
// Copyright (c) 2015-201
6
CNRS
// Copyright (c) 2015-201
7
CNRS
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
...
...
@@ -61,6 +61,7 @@ namespace se3
Force
operator
*
(
const
Motion
&
v
)
const
{
return
derived
().
__mult__
(
v
);
}
Scalar
vtiv
(
const
Motion
&
v
)
const
{
return
derived
().
vtiv_impl
(
v
);
}
Matrix6
variation
(
const
Motion
&
v
)
const
{
return
derived
().
variation_impl
(
v
);
}
void
setZero
()
{
derived
().
setZero
();
}
void
setIdentity
()
{
derived
().
setIdentity
();
}
...
...
@@ -306,6 +307,27 @@ namespace se3
return
res
;
}
Matrix6
variation
(
const
Motion
&
v
)
const
{
Matrix6
res
;
const
Motion
mv
(
v
*
m
);
res
.
template
block
<
3
,
3
>(
LINEAR
,
ANGULAR
)
=
-
skew
(
mv
.
linear
())
-
skewSquare
(
mv
.
angular
(),
c
)
+
skewSquare
(
c
,
mv
.
angular
());
res
.
template
block
<
3
,
3
>(
ANGULAR
,
LINEAR
)
=
res
.
template
block
<
3
,
3
>(
LINEAR
,
ANGULAR
).
transpose
();
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
)
=
mv
.
linear
()
*
c
.
transpose
();
// use as temporary variable
res
.
template
block
<
3
,
3
>(
ANGULAR
,
ANGULAR
)
=
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
)
-
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
).
transpose
();
res
.
template
block
<
3
,
3
>(
ANGULAR
,
ANGULAR
)
=
-
skewSquare
(
mv
.
linear
(),
c
)
-
skewSquare
(
c
,
mv
.
linear
());
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
)
=
(
typename
Symmetric3
::
Matrix3
)(
I
-
AlphaSkewSquare
(
m
,
c
));
res
.
template
block
<
3
,
3
>(
ANGULAR
,
ANGULAR
)
-=
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
)
*
skew
(
v
.
angular
());
res
.
template
block
<
3
,
3
>(
ANGULAR
,
ANGULAR
)
+=
cross
(
v
.
angular
(),
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
));
res
.
template
block
<
3
,
3
>(
LINEAR
,
LINEAR
).
setZero
();
return
res
;
}
// Getters
Scalar
mass
()
const
{
return
m
;
}
...
...
unittest/tspatial.cpp
View file @
28cfaad1
...
...
@@ -341,6 +341,19 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
Inertia
aI_approx
(
aI
);
aI_approx
.
mass
()
+=
eps
;
BOOST_CHECK
(
aI_approx
.
isApprox
(
aI
,
eps
));
// Test Variation
Inertia
::
Matrix6
aIvariation
=
aI
.
variation
(
v
);
Motion
::
ActionMatrix_t
vAction
=
v
.
toActionMatrix
();
Motion
::
ActionMatrix_t
vDualAction
=
v
.
toDualActionMatrix
();
Inertia
::
Matrix6
aImatrix
=
aI
.
matrix
();
Inertia
::
Matrix6
aIvariation_ref
=
vDualAction
*
aImatrix
-
aImatrix
*
vAction
;
BOOST_CHECK
(
aIvariation
.
isApprox
(
aIvariation_ref
));
BOOST_CHECK
(
vxIv
.
isApprox
(
Force
(
aIvariation
*
v
.
toVector
())));
}
BOOST_AUTO_TEST_CASE
(
test_ActOnSet
)
...
...
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