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
Stack Of Tasks
pinocchio
Commits
0ca25deb
Commit
0ca25deb
authored
Nov 15, 2020
by
Gabriele Buondonno
Browse files
[rpy] Implement rpyToJacInv
parent
ac5520f9
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/math/rpy.hpp
View file @
0ca25deb
...
...
@@ -73,6 +73,25 @@ namespace pinocchio
template
<
typename
Vector3Like
>
Eigen
::
Matrix
<
typename
Vector3Like
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like
)
::
Options
>
rpyToJac
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
rpy
,
const
ReferenceFrame
rf
=
LOCAL
);
///
/// \brief Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion
///
/// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD),
/// the Jacobian is such that \f$ {}^F\omega = J_F(\phi)\dot{\phi} \f$,
/// where \f$ {}^F\omega \f$ is the angular velocity expressed in frame F
/// and \f$ J_F \f$ is the Jacobian computed with reference frame F
///
/// \param[in] rpy Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The inverse of the Jacobian of the Roll-Pitch-Yaw conversion in the appropriate frame
///
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template
<
typename
Vector3Like
>
Eigen
::
Matrix
<
typename
Vector3Like
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like
)
::
Options
>
rpyToJacInv
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
rpy
,
const
ReferenceFrame
rf
=
LOCAL
);
}
// namespace rpy
}
...
...
src/math/rpy.hxx
View file @
0ca25deb
...
...
@@ -101,6 +101,45 @@ namespace pinocchio
}
}
}
template
<
typename
Vector3Like
>
Eigen
::
Matrix
<
typename
Vector3Like
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like
)
::
Options
>
rpyToJacInv
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
rpy
,
const
ReferenceFrame
rf
)
{
typedef
typename
Vector3Like
::
Scalar
Scalar
;
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
J
;
const
Scalar
p
=
rpy
[
1
];
Scalar
sp
,
cp
;
SINCOS
(
p
,
&
sp
,
&
cp
);
Scalar
tp
=
sp
/
cp
;
switch
(
rf
)
{
case
LOCAL
:
{
const
Scalar
r
=
rpy
[
0
];
Scalar
sr
,
cr
;
SINCOS
(
r
,
&
sr
,
&
cr
);
J
<<
Scalar
(
1.0
),
sr
*
tp
,
cr
*
tp
,
Scalar
(
0.0
),
cr
,
-
sr
,
Scalar
(
0.0
),
sr
/
cp
,
cr
/
cp
;
return
J
;
}
case
WORLD
:
case
LOCAL_WORLD_ALIGNED
:
{
const
Scalar
y
=
rpy
[
2
];
Scalar
sy
,
cy
;
SINCOS
(
y
,
&
sy
,
&
cy
);
J
<<
cy
/
cp
,
sy
/
cp
,
Scalar
(
0.0
),
-
sy
,
cy
,
Scalar
(
0.0
),
cy
*
tp
,
sy
*
tp
,
Scalar
(
1.0
);
return
J
;
}
default:
{
throw
std
::
invalid_argument
(
"Bad reference frame."
);
}
}
}
}
// namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__
unittest/rpy.cpp
View file @
0ca25deb
...
...
@@ -181,4 +181,31 @@ BOOST_AUTO_TEST_CASE(test_rpyToJac)
BOOST_CHECK
(
Rdot
.
isApprox
(
pinocchio
::
skew
(
omegaW
)
*
R
,
tol
));
}
BOOST_AUTO_TEST_CASE
(
test_rpyToJacInv
)
{
// Check correct identities between different versions
double
r
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
double
p
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
M_PI
))
-
(
M_PI
/
2
);
p
*=
0.999
;
// ensure we are not too close to a singularity
double
y
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
Eigen
::
Vector3d
rpy
(
r
,
p
,
y
);
Eigen
::
Matrix3d
j0
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
);
Eigen
::
Matrix3d
j0inv
=
pinocchio
::
rpy
::
rpyToJacInv
(
rpy
);
BOOST_CHECK
(
j0inv
.
isApprox
(
j0
.
inverse
()));
Eigen
::
Matrix3d
jL
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL
);
Eigen
::
Matrix3d
jLinv
=
pinocchio
::
rpy
::
rpyToJacInv
(
rpy
,
pinocchio
::
LOCAL
);
BOOST_CHECK
(
jLinv
.
isApprox
(
jL
.
inverse
()));
Eigen
::
Matrix3d
jW
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
WORLD
);
Eigen
::
Matrix3d
jWinv
=
pinocchio
::
rpy
::
rpyToJacInv
(
rpy
,
pinocchio
::
WORLD
);
BOOST_CHECK
(
jWinv
.
isApprox
(
jW
.
inverse
()));
Eigen
::
Matrix3d
jA
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL_WORLD_ALIGNED
);
Eigen
::
Matrix3d
jAinv
=
pinocchio
::
rpy
::
rpyToJacInv
(
rpy
,
pinocchio
::
LOCAL_WORLD_ALIGNED
);
BOOST_CHECK
(
jAinv
.
isApprox
(
jA
.
inverse
()));
}
BOOST_AUTO_TEST_SUITE_END
()
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