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
ac5520f9
Commit
ac5520f9
authored
Oct 02, 2020
by
Gabriele Buondonno
Browse files
[rpy] Implement rpyToJac
parent
c520069a
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/math/rpy.hpp
View file @
ac5520f9
...
...
@@ -7,6 +7,7 @@
#include
"pinocchio/math/fwd.hpp"
#include
"pinocchio/math/comparison-operators.hpp"
#include
"pinocchio/multibody/fwd.hpp"
#include
<Eigen/Geometry>
...
...
@@ -53,6 +54,25 @@ namespace pinocchio
template
<
typename
Matrix3Like
>
Eigen
::
Matrix
<
typename
Matrix3Like
::
Scalar
,
3
,
1
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Matrix3Like
)
::
Options
>
matrixToRpy
(
const
Eigen
::
MatrixBase
<
Matrix3Like
>
&
R
);
///
/// \brief Compute the 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 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
>
rpyToJac
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
rpy
,
const
ReferenceFrame
rf
=
LOCAL
);
}
// namespace rpy
}
...
...
src/math/rpy.hxx
0 → 100644
View file @
ac5520f9
//
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_math_rpy_hxx__
#define __pinocchio_math_rpy_hxx__
#include
"pinocchio/math/sincos.hpp"
namespace
pinocchio
{
namespace
rpy
{
template
<
typename
Scalar
>
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
rpyToMatrix
(
const
Scalar
r
,
const
Scalar
p
,
const
Scalar
y
)
{
typedef
Eigen
::
AngleAxis
<
Scalar
>
AngleAxis
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
1
>
Vector3s
;
return
(
AngleAxis
(
y
,
Vector3s
::
UnitZ
())
*
AngleAxis
(
p
,
Vector3s
::
UnitY
())
*
AngleAxis
(
r
,
Vector3s
::
UnitX
())
).
toRotationMatrix
();
}
template
<
typename
Vector3Like
>
Eigen
::
Matrix
<
typename
Vector3Like
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like
)
::
Options
>
rpyToMatrix
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
rpy
)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE
(
Vector3Like
,
rpy
,
3
,
1
);
return
rpyToMatrix
(
rpy
[
0
],
rpy
[
1
],
rpy
[
2
]);
}
template
<
typename
Matrix3Like
>
Eigen
::
Matrix
<
typename
Matrix3Like
::
Scalar
,
3
,
1
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Matrix3Like
)
::
Options
>
matrixToRpy
(
const
Eigen
::
MatrixBase
<
Matrix3Like
>
&
R
)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE
(
Matrix3Like
,
R
,
3
,
3
);
assert
(
R
.
isUnitary
()
&&
"R is not a unitary matrix"
);
typedef
typename
Matrix3Like
::
Scalar
Scalar
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
1
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Matrix3Like
)
::
Options
>
ReturnType
;
static
const
Scalar
pi
=
PI
<
Scalar
>
();
ReturnType
res
=
R
.
eulerAngles
(
2
,
1
,
0
).
reverse
();
if
(
res
[
1
]
<
-
pi
/
2
)
res
[
1
]
+=
2
*
pi
;
if
(
res
[
1
]
>
pi
/
2
)
{
res
[
1
]
=
pi
-
res
[
1
];
if
(
res
[
0
]
<
Scalar
(
0
))
res
[
0
]
+=
pi
;
else
res
[
0
]
-=
pi
;
// res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
res
[
2
]
-=
pi
;
}
return
res
;
}
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
)
{
typedef
typename
Vector3Like
::
Scalar
Scalar
;
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
J
;
const
Scalar
p
=
rpy
[
1
];
Scalar
sp
,
cp
;
SINCOS
(
p
,
&
sp
,
&
cp
);
switch
(
rf
)
{
case
LOCAL
:
{
const
Scalar
r
=
rpy
[
0
];
Scalar
sr
,
cr
;
SINCOS
(
r
,
&
sr
,
&
cr
);
J
<<
Scalar
(
1.0
),
Scalar
(
0.0
),
-
sp
,
Scalar
(
0.0
),
cr
,
sr
*
cp
,
Scalar
(
0.0
),
-
sr
,
cr
*
cp
;
return
J
;
}
case
WORLD
:
case
LOCAL_WORLD_ALIGNED
:
{
const
Scalar
y
=
rpy
[
2
];
Scalar
sy
,
cy
;
SINCOS
(
y
,
&
sy
,
&
cy
);
J
<<
cp
*
cy
,
-
sy
,
Scalar
(
0.0
),
cp
*
sy
,
cy
,
Scalar
(
0.0
),
-
sp
,
Scalar
(
0.0
),
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 @
ac5520f9
...
...
@@ -117,4 +117,68 @@ BOOST_AUTO_TEST_CASE(test_matrixToRpy)
}
}
BOOST_AUTO_TEST_CASE
(
test_rpyToJac
)
{
// Check identity at zero
Eigen
::
Vector3d
rpy
(
Eigen
::
Vector3d
::
Zero
());
Eigen
::
Matrix3d
j0
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
);
BOOST_CHECK
(
j0
.
isIdentity
());
Eigen
::
Matrix3d
jL
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL
);
BOOST_CHECK
(
jL
.
isIdentity
());
Eigen
::
Matrix3d
jW
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
WORLD
);
BOOST_CHECK
(
jW
.
isIdentity
());
Eigen
::
Matrix3d
jA
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL_WORLD_ALIGNED
);
BOOST_CHECK
(
jA
.
isIdentity
());
// 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
);
double
y
=
static_cast
<
double
>
(
rand
())
/
(
static_cast
<
double
>
(
RAND_MAX
/
(
2
*
M_PI
)))
-
M_PI
;
rpy
=
Eigen
::
Vector3d
(
r
,
p
,
y
);
Eigen
::
Matrix3d
R
=
pinocchio
::
rpy
::
rpyToMatrix
(
rpy
);
j0
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
);
jL
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL
);
jW
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
WORLD
);
jA
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL_WORLD_ALIGNED
);
BOOST_CHECK
(
j0
==
jL
);
BOOST_CHECK
(
jW
==
jA
);
BOOST_CHECK
(
jW
.
isApprox
(
R
*
jL
));
// Check against analytical formulas
Eigen
::
Vector3d
jL0Expected
=
Eigen
::
Vector3d
::
UnitX
();
Eigen
::
Vector3d
jL1Expected
=
Eigen
::
AngleAxisd
(
r
,
Eigen
::
Vector3d
::
UnitX
()).
toRotationMatrix
().
transpose
().
col
(
1
);
Eigen
::
Vector3d
jL2Expected
=
(
Eigen
::
AngleAxisd
(
p
,
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
r
,
Eigen
::
Vector3d
::
UnitX
())
).
toRotationMatrix
().
transpose
().
col
(
2
);
BOOST_CHECK
(
jL
.
col
(
0
).
isApprox
(
jL0Expected
));
BOOST_CHECK
(
jL
.
col
(
1
).
isApprox
(
jL1Expected
));
BOOST_CHECK
(
jL
.
col
(
2
).
isApprox
(
jL2Expected
));
Eigen
::
Vector3d
jW0Expected
=
(
Eigen
::
AngleAxisd
(
y
,
Eigen
::
Vector3d
::
UnitZ
())
*
Eigen
::
AngleAxisd
(
p
,
Eigen
::
Vector3d
::
UnitY
())
).
toRotationMatrix
().
col
(
0
);
Eigen
::
Vector3d
jW1Expected
=
Eigen
::
AngleAxisd
(
y
,
Eigen
::
Vector3d
::
UnitZ
()).
toRotationMatrix
().
col
(
1
);
Eigen
::
Vector3d
jW2Expected
=
Eigen
::
Vector3d
::
UnitZ
();
BOOST_CHECK
(
jW
.
col
(
0
).
isApprox
(
jW0Expected
));
BOOST_CHECK
(
jW
.
col
(
1
).
isApprox
(
jW1Expected
));
BOOST_CHECK
(
jW
.
col
(
2
).
isApprox
(
jW2Expected
));
// Check against finite differences
Eigen
::
Vector3d
rpydot
=
Eigen
::
Vector3d
::
Random
();
double
const
eps
=
1e-7
;
double
const
tol
=
1e-5
;
Eigen
::
Matrix3d
dRdr
=
(
pinocchio
::
rpy
::
rpyToMatrix
(
r
+
eps
,
p
,
y
)
-
R
)
/
eps
;
Eigen
::
Matrix3d
dRdp
=
(
pinocchio
::
rpy
::
rpyToMatrix
(
r
,
p
+
eps
,
y
)
-
R
)
/
eps
;
Eigen
::
Matrix3d
dRdy
=
(
pinocchio
::
rpy
::
rpyToMatrix
(
r
,
p
,
y
+
eps
)
-
R
)
/
eps
;
Eigen
::
Matrix3d
Rdot
=
dRdr
*
rpydot
[
0
]
+
dRdp
*
rpydot
[
1
]
+
dRdy
*
rpydot
[
2
];
Eigen
::
Vector3d
omegaL
=
jL
*
rpydot
;
BOOST_CHECK
(
Rdot
.
isApprox
(
R
*
pinocchio
::
skew
(
omegaL
),
tol
));
Eigen
::
Vector3d
omegaW
=
jW
*
rpydot
;
BOOST_CHECK
(
Rdot
.
isApprox
(
pinocchio
::
skew
(
omegaW
)
*
R
,
tol
));
}
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