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
83d50c0e
Commit
83d50c0e
authored
Apr 20, 2020
by
Rohan Budhiraja
Browse files
ligroup/SE3: dIntegrateTransport_dq_impl
parent
8da6cc78
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/multibody/liegroup/special-euclidean.hpp
View file @
83d50c0e
...
...
@@ -351,8 +351,19 @@ namespace pinocchio
void
dIntegrateTransport_dq_impl
(
const
Eigen
::
MatrixBase
<
Config_t
>
&
q
,
const
Eigen
::
MatrixBase
<
Tangent_t
>
&
v
,
const
Eigen
::
MatrixBase
<
JacobianIn_t
>
&
Jin
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
Jout
)
const
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
J
_
out
)
const
{
JacobianOut_t
&
Jout
=
PINOCCHIO_EIGEN_CONST_CAST
(
JacobianOut_t
,
J_out
);
Matrix2
R
;
Vector2
t
;
exp
(
v
,
R
,
t
);
typename
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector2Like
)
tinv
((
R
.
transpose
()
*
t
).
reverse
());
tinv
[
0
]
*=
Scalar
(
-
1.
);
Jout
.
template
topRows
<
2
>().
noalias
()
=
R
.
transpose
()
*
Jin
.
template
topRows
<
2
>();
Jout
.
template
topRows
<
2
>().
noalias
()
+=
tinv
*
Jin
.
template
bottomRows
<
1
>();
Jout
.
template
bottomRows
<
1
>()
=
Jin
.
template
bottomRows
<
1
>();
}
template
<
class
Config_t
,
class
Tangent_t
,
class
JacobianIn_t
,
class
JacobianOut_t
>
...
...
@@ -649,8 +660,15 @@ namespace pinocchio
void
dIntegrateTransport_dq_impl
(
const
Eigen
::
MatrixBase
<
Config_t
>
&
q
,
const
Eigen
::
MatrixBase
<
Tangent_t
>
&
v
,
const
Eigen
::
MatrixBase
<
JacobianIn_t
>
&
Jin
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
Jout
)
const
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
J
_
out
)
const
{
JacobianOut_t
&
Jout
=
PINOCCHIO_EIGEN_CONST_CAST
(
JacobianOut_t
,
J_out
);
Eigen
::
Matrix
<
Scalar
,
6
,
6
>
Jtmp6
;
Jtmp6
=
exp6
(
MotionRef
<
const
Tangent_t
>
(
v
.
derived
())).
toDualActionMatrix
().
transpose
();
Jout
.
template
topRows
<
3
>().
noalias
=
Jtmp6
.
template
topLeftCorner
<
3
,
3
>()
*
Jin
.
template
topRows
<
3
>();
Jout
.
template
topRows
<
3
>().
noalias
+=
Jtmp6
.
template
topRightCorner
<
3
,
3
>()
*
Jin
.
template
bottomRows
<
3
>();
Jout
.
template
bottomRows
<
3
>().
noalias
=
Jtmp6
.
template
bottomRightCorner
<
3
,
3
>()
*
Jin
.
template
bottomRows
<
3
>();
}
template
<
class
Config_t
,
class
Tangent_t
,
class
JacobianIn_t
,
class
JacobianOut_t
>
...
...
@@ -659,12 +677,13 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
JacobianIn_t
>
&
Jin
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
Jout
)
const
{
JacobianOut_t
&
Jout
=
PINOCCHIO_EIGEN_CONST_CAST
(
JacobianOut_t
,
J_out
);
Eigen
::
Matrix
<
Scalar
,
6
,
6
>
Jtmp6
;
Jexp6
<
SETTO
>
(
MotionRef
<
const
Tangent_t
>
(
v
.
derived
()),
Jtmp6
);
Jout
.
template
topRows
<
3
>().
noalias
=
Jtmp6
.
template
topLeftCorner
<
3
,
3
>()
*
Jin
.
template
topRows
<
3
>();
Jout
.
template
topRows
<
3
>().
noalias
+=
Jtmp6
.
template
topRightCorner
<
3
,
3
>()
*
Jin
.
template
bottomRows
<
3
>();
Jout
.
template
bottomRows
<
3
>().
noalias
+
=
Jtmp6
.
template
bottomRightCorner
<
3
,
3
>()
*
Jin
.
template
bottomRows
<
3
>();
Jout
.
template
bottomRows
<
3
>().
noalias
=
Jtmp6
.
template
bottomRightCorner
<
3
,
3
>()
*
Jin
.
template
bottomRows
<
3
>();
}
...
...
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