Verified Commit 06c7ae89 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

liegroup: fix identiation

parent 3849da15
......@@ -48,15 +48,16 @@ namespace pinocchio
CartesianProductOperation () : lg1_ (), lg2_ ()
{
}
/// Get dimension of Lie Group vector representation
///
/// For instance, for SO(3), the dimension of the vector representation is
/// 4 (quaternion) while the dimension of the tangent space is 3.
// Get dimension of Lie Group vector representation
//
// For instance, for SO(3), the dimension of the vector representation is
// 4 (quaternion) while the dimension of the tangent space is 3.
Index nq () const
{
return lg1_.nq () + lg2_.nq ();
}
/// Get dimension of Lie Group tangent space
// Get dimension of Lie Group tangent space
Index nv () const
{
return lg1_.nv () + lg2_.nv ();
......@@ -79,17 +80,17 @@ namespace pinocchio
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d) const
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d) const
{
lg1_.difference(Q1(q0), Q1(q1), Vo1(d));
lg2_.difference(Q2(q0), Q2(q1), Vo2(d));
}
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
void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
J12(J).setZero();
J21(J).setZero();
......@@ -100,8 +101,8 @@ namespace pinocchio
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout) const
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
lg1_.integrate(Q1(q), V1(v), Qo1(qout));
lg2_.integrate(Q2(q), V2(v), Qo2(qout));
......
......@@ -127,9 +127,9 @@ namespace pinocchio
}
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t>& J) const
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
Matrix2 R; // R0.transpose() * R1;
R(0,0) = R(1,1) = q0.dot(q1);
......@@ -271,6 +271,7 @@ namespace pinocchio
{
return NQ;
}
/// Get dimension of Lie Group tangent space
static Index nv()
{
......@@ -306,7 +307,7 @@ namespace pinocchio
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl (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
{
ConstQuaternionMap_t p0 (q0.derived().data());
ConstQuaternionMap_t p1 (q1.derived().data());
......@@ -369,7 +370,7 @@ namespace pinocchio
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J)
const Eigen::MatrixBase<JacobianOut_t> & J)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
Jout = exp3(-v);
......@@ -387,7 +388,7 @@ namespace pinocchio
static 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 Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConstQuaternionMap_t p0 (q0.derived().data());
ConstQuaternionMap_t p1 (q1.derived().data());
......@@ -406,7 +407,7 @@ namespace pinocchio
}
template <class Config_t>
static void normalize_impl(const Eigen::MatrixBase<Config_t>& qout)
static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
{
Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
qout_.normalize();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment