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
49d7a522
Commit
49d7a522
authored
Feb 01, 2021
by
Joseph Mirabel
Browse files
[Doc] Add remarkable identities for Jlog6
parent
fcf0ea0a
Pipeline
#13012
passed with stage
in 133 minutes and 16 seconds
Changes
2
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/spatial/explog.hpp
View file @
49d7a522
...
...
@@ -461,6 +461,12 @@ namespace pinocchio
* \f]
* and
* \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f]
*
* \cheatsheet For \f$ M = log_6(a \times b) \f$,
* \f$ \frac{\partial M}{\partial a} = Jlog_6(M) \times X_b^{-1} \f$
* and \f$ \frac{\partial M}{\partial b} = Jlog_6(M) \f$. \n
* For \f$ M = log_6(a^{-1}) \f$,
* \f$ \frac{\partial M}{\partial a} = - Jlog_6(M) \times X_a \f$.
*/
template
<
typename
Scalar
,
int
Options
,
typename
Matrix6Like
>
void
Jlog6
(
const
SE3Tpl
<
Scalar
,
Options
>
&
M
,
...
...
unittest/explog.cpp
View file @
49d7a522
...
...
@@ -358,6 +358,32 @@ BOOST_AUTO_TEST_CASE(Jlog6_fd)
BOOST_CHECK
(
Jfd
.
isApprox
(
Jlog
,
sqrt
(
step
)));
}
BOOST_AUTO_TEST_CASE
(
Jlog6_of_product_fd
)
{
SE3
Ma
(
SE3
::
Random
());
SE3
Mb
(
SE3
::
Random
());
SE3
::
Matrix6
Jlog
,
Ja
,
Jb
,
Jfda
,
Jfdb
;
Jlog6
(
Ma
.
inverse
()
*
Mb
,
Jlog
);
Ja
=
-
Jlog
*
(
Ma
.
inverse
()
*
Mb
).
toActionMatrixInverse
();
Jb
=
Jlog
;
Jfda
.
setZero
();
Jfdb
.
setZero
();
Motion
dM
;
dM
.
setZero
();
double
step
=
1e-8
;
for
(
int
i
=
0
;
i
<
6
;
++
i
)
{
dM
.
toVector
()[
i
]
=
step
;
Jfda
.
col
(
i
)
=
(
log6
((
Ma
*
exp6
(
dM
)).
inverse
()
*
Mb
).
toVector
()
-
log6
(
Ma
.
inverse
()
*
Mb
).
toVector
())
/
step
;
Jfdb
.
col
(
i
)
=
(
log6
(
Ma
.
inverse
()
*
Mb
*
exp6
(
dM
)).
toVector
()
-
log6
(
Ma
.
inverse
()
*
Mb
).
toVector
())
/
step
;
dM
.
toVector
()[
i
]
=
0
;
}
BOOST_CHECK
(
Jfda
.
isApprox
(
Ja
,
sqrt
(
step
)));
BOOST_CHECK
(
Jfdb
.
isApprox
(
Jb
,
sqrt
(
step
)));
}
BOOST_AUTO_TEST_CASE
(
Jexplog6
)
{
Motion
v
(
Motion
::
Random
());
...
...
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