Commit 66c8370d authored by Justin Carpentier's avatar Justin Carpentier

joints: add operator== to JointConstraint

parent c0969180
......@@ -98,6 +98,11 @@ namespace pinocchio
{
return derived().motionAction(v);
}
bool operator==(const ConstraintBase<Derived> & other) const
{
return derived().isEqual(other.derived());
}
}; // class ConstraintBase
......
......@@ -94,6 +94,8 @@ namespace pinocchio
motionAction(const MotionBase<MotionDerived> & v) const
{ return v.toActionMatrix(); }
bool isEqual(const ConstraintIdentityTpl &) const { return true; }
}; // struct ConstraintIdentityTpl
template<typename Scalar, int Options, typename Vector6Like>
......
......@@ -155,6 +155,12 @@ namespace pinocchio
inline const Scalar & scaling() const { return m_scaling_factor; }
inline const Constraint & constraint() const { return m_constraint; }
bool isEqual(const ScaledConstraint & other) const
{
return m_constraint == other.m_constraint
&& m_scaling_factor == other.m_scaling_factor;
}
protected:
Constraint m_constraint;
......
......@@ -327,6 +327,9 @@ namespace pinocchio
return res;
}
bool isEqual(const ConstraintPlanarTpl &) const { return true; }
}; // struct ConstraintPlanarTpl
template<typename MotionDerived, typename S2, int O2>
......
......@@ -328,6 +328,11 @@ namespace pinocchio
const Vector3 & axis() const { return m_axis; }
Vector3 & axis() { return m_axis; }
bool isEqual(const ConstraintPrismaticUnalignedTpl & other) const
{
return m_axis == other.m_axis;
}
protected:
Vector3 m_axis;
......
......@@ -378,6 +378,8 @@ namespace pinocchio
v = m.cross(Axis());
return res;
}
bool isEqual(const ConstraintPrismaticTpl &) const { return true; }
}; // struct ConstraintPrismaticTpl
......
......@@ -346,6 +346,11 @@ namespace pinocchio
const Vector3 & axis() const { return m_axis; }
Vector3 & axis() { return m_axis; }
bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const
{
return m_axis == other.m_axis;
}
protected:
Vector3 m_axis;
......
......@@ -450,6 +450,9 @@ namespace pinocchio
v = m.cross(Axis());
return res;
}
bool isEqual(const ConstraintRevoluteTpl &) const { return true; }
}; // struct ConstraintRevoluteTpl
template<typename _Scalar, int _Options, int _axis>
......
......@@ -166,6 +166,10 @@ namespace pinocchio
// data
Matrix3 S_minimal;
bool isEqual(const ConstraintSphericalZYXTpl & other) const
{
return m_S == other.m_S;
}
}; // struct ConstraintSphericalZYXTpl
......
......@@ -281,6 +281,8 @@ namespace pinocchio
return res;
}
bool isEqual(const ConstraintSphericalTpl &) const { return true; }
}; // struct ConstraintSphericalTpl
......
......@@ -360,6 +360,8 @@ namespace pinocchio
return res;
}
bool isEqual(const ConstraintTranslationTpl &) const { return true; }
}; // struct ConstraintTranslationTpl
template<typename MotionDerived, typename S2, int O2>
......
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