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
04e7c245
Commit
04e7c245
authored
Nov 15, 2020
by
Gabriele Buondonno
Browse files
[rpy] Implement rpyToJacDerivative
parent
dc3814b7
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/math/rpy.hpp
View file @
04e7c245
...
...
@@ -93,6 +93,26 @@ namespace pinocchio
template
<
typename
Vector3Like
>
inline
Eigen
::
Matrix
<
typename
Vector3Like
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like
)
::
Options
>
rpyToJacInv
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
rpy
,
const
ReferenceFrame
rf
=
LOCAL
);
///
/// \brief Compute the time derivative 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] rpydot Time derivative of the Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The time derivative 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
Vector3Like0
,
typename
Vector3Like1
>
inline
Eigen
::
Matrix
<
typename
Vector3Like0
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like0
)
::
Options
>
rpyToJacDerivative
(
const
Eigen
::
MatrixBase
<
Vector3Like0
>
&
rpy
,
const
Eigen
::
MatrixBase
<
Vector3Like1
>
&
rpydot
,
const
ReferenceFrame
rf
=
LOCAL
);
}
// namespace rpy
}
...
...
src/math/rpy.hxx
View file @
04e7c245
...
...
@@ -141,6 +141,47 @@ namespace pinocchio
}
}
}
template
<
typename
Vector3Like0
,
typename
Vector3Like1
>
inline
Eigen
::
Matrix
<
typename
Vector3Like0
::
Scalar
,
3
,
3
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
Vector3Like0
)
::
Options
>
rpyToJacDerivative
(
const
Eigen
::
MatrixBase
<
Vector3Like0
>
&
rpy
,
const
Eigen
::
MatrixBase
<
Vector3Like1
>
&
rpydot
,
const
ReferenceFrame
rf
)
{
typedef
typename
Vector3Like0
::
Scalar
Scalar
;
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
J
;
const
Scalar
p
=
rpy
[
1
];
const
Scalar
dp
=
rpydot
[
1
];
Scalar
sp
,
cp
;
SINCOS
(
p
,
&
sp
,
&
cp
);
switch
(
rf
)
{
case
LOCAL
:
{
const
Scalar
r
=
rpy
[
0
];
const
Scalar
dr
=
rpydot
[
0
];
Scalar
sr
,
cr
;
SINCOS
(
r
,
&
sr
,
&
cr
);
J
<<
Scalar
(
0.0
),
Scalar
(
0.0
),
-
cp
*
dp
,
Scalar
(
0.0
),
-
sr
*
dr
,
cr
*
cp
*
dr
-
sr
*
sp
*
dp
,
Scalar
(
0.0
),
-
cr
*
dr
,
-
sr
*
cp
*
dr
-
cr
*
sp
*
dp
;
return
J
;
}
case
WORLD
:
case
LOCAL_WORLD_ALIGNED
:
{
const
Scalar
y
=
rpy
[
2
];
const
Scalar
dy
=
rpydot
[
2
];
Scalar
sy
,
cy
;
SINCOS
(
y
,
&
sy
,
&
cy
);
J
<<
-
sp
*
cy
*
dp
-
cp
*
sy
*
dy
,
-
cy
*
dy
,
Scalar
(
0.0
),
cp
*
cy
*
dy
-
sp
*
sy
*
dp
,
-
sy
*
dy
,
Scalar
(
0.0
),
-
cp
*
dp
,
Scalar
(
0.0
),
Scalar
(
0.0
);
return
J
;
}
default:
{
throw
std
::
invalid_argument
(
"Bad reference frame."
);
}
}
}
}
// namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__
unittest/rpy.cpp
View file @
04e7c245
...
...
@@ -4,6 +4,7 @@
#include
<pinocchio/math/rpy.hpp>
#include
<pinocchio/math/quaternion.hpp>
#include
<pinocchio/spatial/skew.hpp>
#include
<boost/variant.hpp>
// to avoid C99 warnings
...
...
@@ -208,4 +209,70 @@ BOOST_AUTO_TEST_CASE(test_rpyToJacInv)
BOOST_CHECK
(
jAinv
.
isApprox
(
jA
.
inverse
()));
}
BOOST_AUTO_TEST_CASE
(
test_rpyToJacDerivative
)
{
// Check zero at zero velocity
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
;
Eigen
::
Vector3d
rpy
(
r
,
p
,
y
);
Eigen
::
Vector3d
rpydot
(
Eigen
::
Vector3d
::
Zero
());
Eigen
::
Matrix3d
dj0
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
);
BOOST_CHECK
(
dj0
.
isZero
());
Eigen
::
Matrix3d
djL
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
,
pinocchio
::
LOCAL
);
BOOST_CHECK
(
djL
.
isZero
());
Eigen
::
Matrix3d
djW
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
,
pinocchio
::
WORLD
);
BOOST_CHECK
(
djW
.
isZero
());
Eigen
::
Matrix3d
djA
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
,
pinocchio
::
LOCAL_WORLD_ALIGNED
);
BOOST_CHECK
(
djA
.
isZero
());
// Check correct identities between different versions
rpydot
=
Eigen
::
Vector3d
::
Random
();
dj0
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
);
djL
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
,
pinocchio
::
LOCAL
);
djW
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
,
pinocchio
::
WORLD
);
djA
=
pinocchio
::
rpy
::
rpyToJacDerivative
(
rpy
,
rpydot
,
pinocchio
::
LOCAL_WORLD_ALIGNED
);
BOOST_CHECK
(
dj0
==
djL
);
BOOST_CHECK
(
djW
==
djA
);
Eigen
::
Matrix3d
R
=
pinocchio
::
rpy
::
rpyToMatrix
(
rpy
);
Eigen
::
Matrix3d
jL
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
LOCAL
);
Eigen
::
Matrix3d
jW
=
pinocchio
::
rpy
::
rpyToJac
(
rpy
,
pinocchio
::
WORLD
);
Eigen
::
Vector3d
omegaL
=
jL
*
rpydot
;
Eigen
::
Vector3d
omegaW
=
jW
*
rpydot
;
BOOST_CHECK
(
omegaW
.
isApprox
(
R
*
omegaL
));
BOOST_CHECK
(
djW
.
isApprox
(
pinocchio
::
skew
(
omegaW
)
*
R
*
jL
+
R
*
djL
));
BOOST_CHECK
(
djW
.
isApprox
(
R
*
pinocchio
::
skew
(
omegaL
)
*
jL
+
R
*
djL
));
// Check against finite differences
double
const
eps
=
1e-7
;
double
const
tol
=
1e-5
;
Eigen
::
Vector3d
rpyEps
=
rpy
;
rpyEps
[
0
]
+=
eps
;
Eigen
::
Matrix3d
djLdr
=
(
pinocchio
::
rpy
::
rpyToJac
(
rpyEps
,
pinocchio
::
LOCAL
)
-
jL
)
/
eps
;
rpyEps
[
0
]
=
rpy
[
0
];
rpyEps
[
1
]
+=
eps
;
Eigen
::
Matrix3d
djLdp
=
(
pinocchio
::
rpy
::
rpyToJac
(
rpyEps
,
pinocchio
::
LOCAL
)
-
jL
)
/
eps
;
rpyEps
[
1
]
=
rpy
[
1
];
rpyEps
[
2
]
+=
eps
;
Eigen
::
Matrix3d
djLdy
=
(
pinocchio
::
rpy
::
rpyToJac
(
rpyEps
,
pinocchio
::
LOCAL
)
-
jL
)
/
eps
;
rpyEps
[
2
]
=
rpy
[
2
];
Eigen
::
Matrix3d
djLf
=
djLdr
*
rpydot
[
0
]
+
djLdp
*
rpydot
[
1
]
+
djLdy
*
rpydot
[
2
];
BOOST_CHECK
(
djL
.
isApprox
(
djLf
,
tol
));
rpyEps
[
0
]
+=
eps
;
Eigen
::
Matrix3d
djWdr
=
(
pinocchio
::
rpy
::
rpyToJac
(
rpyEps
,
pinocchio
::
WORLD
)
-
jW
)
/
eps
;
rpyEps
[
0
]
=
rpy
[
0
];
rpyEps
[
1
]
+=
eps
;
Eigen
::
Matrix3d
djWdp
=
(
pinocchio
::
rpy
::
rpyToJac
(
rpyEps
,
pinocchio
::
WORLD
)
-
jW
)
/
eps
;
rpyEps
[
1
]
=
rpy
[
1
];
rpyEps
[
2
]
+=
eps
;
Eigen
::
Matrix3d
djWdy
=
(
pinocchio
::
rpy
::
rpyToJac
(
rpyEps
,
pinocchio
::
WORLD
)
-
jW
)
/
eps
;
rpyEps
[
2
]
=
rpy
[
2
];
Eigen
::
Matrix3d
djWf
=
djWdr
*
rpydot
[
0
]
+
djWdp
*
rpydot
[
1
]
+
djWdy
*
rpydot
[
2
];
BOOST_CHECK
(
djW
.
isApprox
(
djWf
,
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