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
Stack Of Tasks
pinocchio
Commits
acb40c65
Unverified
Commit
acb40c65
authored
Sep 25, 2019
by
Justin Carpentier
Committed by
GitHub
Sep 25, 2019
Browse files
Merge pull request #890 from jmirabel/fix
[Doc] Add a cheat sheet
parents
a28ec541
77cf434f
Changes
4
Hide whitespace changes
Inline
Side-by-side
doc/Doxyfile.extra.in
View file @
acb40c65
...
...
@@ -675,3 +675,9 @@ MATHJAX_RELPATH = MathJax/
# the tag USE_MATHJAX is set to YES.
MATHJAX_FORMAT = SVG
#---------------------------------------------------------------------------
# Aliases
#---------------------------------------------------------------------------
ALIASES += "cheatsheet=\xrefitem cheatsheet \"Remarkable identity\" \"Cheat sheet\""
src/multibody/liegroup/special-euclidean.hpp
View file @
acb40c65
...
...
@@ -439,6 +439,7 @@ namespace pinocchio
*
SE3
(
p1
.
matrix
(),
q1
.
derived
().
template
head
<
3
>())).
toVector
();
}
/// \cheatsheet \f$ \frac{\partial\ominus}{\partial q_1} {}^1X_0 = - \frac{\partial\ominus}{\partial q_0} \f$
template
<
ArgumentPosition
arg
,
class
ConfigL_t
,
class
ConfigR_t
,
class
JacobianOut_t
>
void
dDifference_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
...
...
src/spatial/se3-base.hpp
View file @
acb40c65
...
...
@@ -10,15 +10,17 @@ namespace pinocchio
{
/** The rigid transform aMb can be seen in two ways:
*
* - given a point p expressed in frame B by its coordinate vector
Bp, aMb
* computes its coordinates in frame A by
A
p = aMb
Bp
.
* -
aMb
displaces a solid S centered at frame A into the solid centered in
* B. In particular, the origin of A is displaced at the origin of B:
$^aM_b
* ^aA = ^aB$.
* - given a point p expressed in frame B by its coordinate vector
\f$ ^bp \f$, \f$ ^aM_b \f$
* computes its coordinates in frame A by
\f$ ^a
p =
{}^
aM
_
b
{}^bp \f$
.
* -
\f$ ^aM_b \f$
displaces a solid S centered at frame A into the solid centered in
* B. In particular, the origin of A is displaced at the origin of B:
*
\f$^aM_b {}
^aA =
{}
^aB
\f
$.
* The rigid displacement is stored as a rotation matrix and translation vector by:
* aMb (x) = aRb*x + aAB
* where aAB is the vector from origin A to origin B expressed in coordinates A.
* \f$ ^aM_b x = {}^aR_b x + {}^aAB \f$
* where \f$^aAB\f$ is the vector from origin A to origin B expressed in coordinates A.
*
* \cheatsheet \f$ {}^aM_c = {}^aM_b {}^bM_c \f$
*/
template
<
class
Derived
>
struct
SE3Base
...
...
@@ -41,12 +43,21 @@ namespace pinocchio
}
operator
HomogeneousMatrixType
()
const
{
return
toHomogeneousMatrix
();
}
/**
* @brief The action matrix \f$ {}^aX_b \f$ of \f$ {}^aM_b \f$.
*
* \cheatsheet \f$ {}^a\nu_c = {}^aX_b {}^b\nu_c \f$
*/
ActionMatrixType
toActionMatrix
()
const
{
return
derived
().
toActionMatrix_impl
();
}
operator
ActionMatrixType
()
const
{
return
toActionMatrix
();
}
/**
* @brief The action matrix \f$ {}^bX_a \f$ of \f$ {}^aM_b \f$.
* \sa toActionMatrix()
*/
ActionMatrixType
toActionMatrixInverse
()
const
{
return
derived
().
toActionMatrixInverse_impl
();
...
...
unittest/liegroups.cpp
View file @
acb40c65
...
...
@@ -16,6 +16,10 @@
BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
"check " #Va ".isApprox(" #Vb ") failed " \
"[\n" << (Va).transpose() << "\n!=\n" << (Vb).transpose() << "\n]")
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \
BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
"check " #Va ".isApprox(" #Vb ") failed " \
"[\n" << (Va) << "\n!=\n" << (Vb) << "\n]")
using
namespace
pinocchio
;
...
...
@@ -28,7 +32,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
typedef
double
Scalar
;
const
Scalar
prec
=
Eigen
::
NumTraits
<
Scalar
>::
dummy_precision
();
IFVERBOSE
std
::
cout
<<
"Testing Joint over "
<<
jmodel
.
shortname
()
<<
std
::
endl
;
BOOST_TEST_MESSAGE
(
"Testing Joint over "
<<
jmodel
.
shortname
()
)
;
typedef
typename
T
::
ConfigVector_t
ConfigVector_t
;
typedef
typename
T
::
TangentVector_t
TangentVector_t
;
...
...
@@ -176,7 +180,7 @@ struct LieGroup_Jdifference{
typedef
typename
T
::
Scalar
Scalar
;
T
lg
;
IFVERBOSE
std
::
cout
<<
lg
.
name
()
<<
std
::
endl
;
BOOST_TEST_MESSAGE
(
lg
.
name
())
;
ConfigVector_t
q
[
2
],
q_dv
[
2
];
q
[
0
]
=
lg
.
random
();
q
[
1
]
=
lg
.
random
();
...
...
@@ -190,7 +194,7 @@ struct LieGroup_Jdifference{
const
Scalar
eps
=
1e-6
;
for
(
int
k
=
0
;
k
<
2
;
++
k
)
{
IFVERBOSE
std
::
cout
<<
"Checking J"
<<
k
<<
'\n'
<<
J
[
k
]
<<
std
::
endl
;
BOOST_TEST_MESSAGE
(
"Checking J"
<<
k
<<
'\n'
<<
J
[
k
]
)
;
q_dv
[
0
]
=
q
[
0
];
q_dv
[
1
]
=
q
[
1
];
// Check J[k]
...
...
@@ -207,6 +211,36 @@ struct LieGroup_Jdifference{
dv
[
i
]
=
0
;
}
}
specificTests
(
lg
);
}
template
<
typename
T
>
void
specificTests
(
const
T
)
const
{}
template
<
typename
Scalar
,
int
Options
>
void
specificTests
(
const
SpecialEuclideanOperationTpl
<
3
,
Scalar
,
Options
>
)
const
{
typedef
SE3Tpl
<
Scalar
>
SE3
;
typedef
SpecialEuclideanOperationTpl
<
3
,
Scalar
,
Options
>
LG_t
;
typedef
typename
LG_t
::
ConfigVector_t
ConfigVector_t
;
typedef
typename
LG_t
::
JacobianMatrix_t
JacobianMatrix_t
;
LG_t
lg
;
ConfigVector_t
q
[
2
];
q
[
0
]
=
lg
.
random
();
q
[
1
]
=
lg
.
random
();
JacobianMatrix_t
J
[
2
];
lg
.
template
dDifference
<
ARG0
>
(
q
[
0
],
q
[
1
],
J
[
0
]);
lg
.
template
dDifference
<
ARG1
>
(
q
[
0
],
q
[
1
],
J
[
1
]);
SE3
om0
(
typename
SE3
::
Quaternion
(
q
[
0
].
template
tail
<
4
>()).
matrix
(),
q
[
0
].
template
head
<
3
>()),
om1
(
typename
SE3
::
Quaternion
(
q
[
1
].
template
tail
<
4
>()).
matrix
(),
q
[
1
].
template
head
<
3
>()),
_1m2
(
om1
.
actInv
(
om0
))
;
EIGEN_MATRIX_IS_APPROX
(
J
[
1
]
*
_1m2
.
toActionMatrix
(),
-
J
[
0
],
1e-8
);
}
};
...
...
@@ -269,7 +303,7 @@ struct LieGroup_JintegrateJdifference{
typedef
typename
T
::
JacobianMatrix_t
JacobianMatrix_t
;
T
lg
;
IFVERBOSE
std
::
cout
<<
lg
.
name
()
<<
std
::
endl
;
BOOST_TEST_MESSAGE
(
lg
.
name
())
;
ConfigVector_t
qa
,
qb
(
lg
.
nq
());
qa
=
lg
.
random
();
TangentVector_t
v
(
lg
.
nv
());
...
...
@@ -304,7 +338,7 @@ struct LieGroup_JintegrateCoeffWise
ConfigVector_t
q
=
lg
.
random
();
TangentVector_t
dv
(
TangentVector_t
::
Zero
(
lg
.
nv
()));
//
std::cout << "name: " << lg.name() << std::endl
;
BOOST_TEST_MESSAGE
(
lg
.
name
())
;
typedef
Eigen
::
Matrix
<
Scalar
,
T
::
NQ
,
T
::
NV
>
JacobianCoeffs
;
JacobianCoeffs
Jintegrate
(
JacobianCoeffs
::
Zero
(
lg
.
nq
(),
lg
.
nv
()));
lg
.
integrateCoeffWiseJacobian
(
q
,
Jintegrate
);
...
...
@@ -321,9 +355,7 @@ struct LieGroup_JintegrateCoeffWise
dv
[
i
]
=
0
;
}
BOOST_CHECK
(
Jintegrate
.
isApprox
(
Jintegrate_fd
,
sqrt
(
eps
)));
// std::cout << "Jintegrate\n" << Jintegrate << std::endl;
// std::cout << "Jintegrate_fd\n" << Jintegrate_fd << std::endl;
EIGEN_MATRIX_IS_APPROX
(
Jintegrate
,
Jintegrate_fd
,
sqrt
(
eps
));
}
};
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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