Skip to content
Snippets Groups Projects
Verified Commit 87f83cd0 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

multibody/lie-group: minor uniformization

parent 6d1d1366
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/liegroup/liegroup-base.hpp
+ 10
− 10
View file @ 87f83cd0
......@@ -62,7 +62,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout) const;
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
/**
* @brief Computes the Jacobian of the integrate operator around zero.
......@@ -95,10 +95,10 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
return dIntegrate(q,v,J,arg);
return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
}
/**
......@@ -133,7 +133,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const;
const Eigen::MatrixBase<JacobianOut_t> & J) const;
/**
* @brief Computes the Jacobian of a small variation of the tangent vector into tangent space at identity.
......@@ -149,7 +149,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const;
const Eigen::MatrixBase<JacobianOut_t> & J) const;
/**
* @brief Interpolation between two joint's configurations
......@@ -164,7 +164,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const;
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
/**
* @brief Normalize the joint configuration given as input.
......@@ -173,7 +173,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @param[out] qout the normalized joint configuration.
*/
template <class Config_t>
void normalize(const Eigen::MatrixBase<Config_t>& qout) const;
void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
......@@ -184,7 +184,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @param[out] qout the random joint configuration.
*/
template <class Config_t>
void random(const Eigen::MatrixBase<Config_t>& qout) const;
void random(const Eigen::MatrixBase<Config_t> & qout) const;
/**
* @brief Generate a configuration vector uniformly sampled among
......@@ -225,7 +225,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t>& J) const;
const Eigen::MatrixBase<JacobianOut_t> & J) const
/**
* @brief Squared distance between two joint configurations.
......@@ -295,7 +295,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const;
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
template <class Config_t>
void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
......
src/multibody/liegroup/liegroup-base.hxx
+ 13
− 13
View file @ 87f83cd0
......@@ -16,7 +16,7 @@ namespace pinocchio {
void LieGroupBase<Derived>
::integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout) const
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
......@@ -40,7 +40,7 @@ namespace pinocchio {
template<class Config_t, class Tangent_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J,
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg) const
{
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
......@@ -59,7 +59,7 @@ namespace pinocchio {
void LieGroupBase<Derived>::dIntegrate_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
......@@ -72,7 +72,7 @@ namespace pinocchio {
void LieGroupBase<Derived>::dIntegrate_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
......@@ -94,8 +94,8 @@ namespace pinocchio {
void LieGroupBase<Derived>::interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const
const Scalar & u,
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
......@@ -106,7 +106,7 @@ namespace pinocchio {
template <class Derived>
template <class Config_t>
void LieGroupBase<Derived>::normalize
(const Eigen::MatrixBase<Config_t>& qout) const
(const Eigen::MatrixBase<Config_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return derived().normalize_impl (qout);
......@@ -123,7 +123,7 @@ namespace pinocchio {
template <class Derived>
template <class Config_t>
void LieGroupBase<Derived>::random
(const Eigen::MatrixBase<Config_t>& qout) const
(const Eigen::MatrixBase<Config_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return derived().random_impl (qout);
......@@ -147,7 +147,7 @@ namespace pinocchio {
void LieGroupBase<Derived>::difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d) const
const Eigen::MatrixBase<Tangent_t> & d) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
......@@ -160,7 +160,7 @@ namespace pinocchio {
void LieGroupBase<Derived>::dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
......@@ -222,7 +222,7 @@ namespace pinocchio {
typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u) const
const Scalar & u) const
{
ConfigVector_t qout;
interpolate(q0, q1, u, qout);
......@@ -267,8 +267,8 @@ namespace pinocchio {
void LieGroupBase<Derived>::interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const
const Scalar & u,
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
if (u == 0) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q0;
else if(u == 1) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q1;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment