Verified Commit fd60b37a authored by Justin Carpentier's avatar Justin Carpentier
Browse files

joint/mimic: refactoting of the code

parent 01637494
...@@ -284,16 +284,16 @@ namespace pinocchio ...@@ -284,16 +284,16 @@ namespace pinocchio
JointDataMimic(const JointDataBase<JointData> & jdata, JointDataMimic(const JointDataBase<JointData> & jdata,
const Scalar & scaling) const Scalar & scaling)
: jdata_ref(jdata.derived()) : m_jdata_ref(jdata.derived())
, scaling(scaling) , m_scaling(scaling)
, S(jdata_ref.S,scaling) , m_S(m_jdata_ref.S,scaling)
{} {}
JointDataMimic & operator=(const JointDataMimic & other) JointDataMimic & operator=(const JointDataMimic & other)
{ {
jdata_ref = other.jdata_ref; m_jdata_ref = other.m_jdata_ref;
scaling = other.scaling; m_scaling = other.m_scaling;
S = Constraint_t(jdata_ref.S,other.scaling); m_S = Constraint_t(m_jdata_ref.S,other.m_scaling);
return *this; return *this;
} }
...@@ -304,7 +304,7 @@ namespace pinocchio ...@@ -304,7 +304,7 @@ namespace pinocchio
std::string shortname() const std::string shortname() const
{ {
return std::string("JointDataMimic<") + jdata_ref.shortname() + std::string(">"); return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">");
} }
// Accessors // Accessors
...@@ -332,20 +332,26 @@ namespace pinocchio ...@@ -332,20 +332,26 @@ namespace pinocchio
template<class JointModel> template<class JointModel>
friend struct JointModelMimic; friend struct JointModelMimic;
const JointData & jdata() const { return m_jdata_ref; }
JointData & jdata() { return m_jdata_ref; }
const Scalar & scaling() const { return m_scaling; }
Scalar & scaling() { return m_scaling; }
protected: protected:
JointData jdata_ref; JointData m_jdata_ref;
Scalar scaling; Scalar m_scaling;
/// \brief Transform configuration vector /// \brief Transform configuration vector
ConfigVector_t q_transform; ConfigVector_t m_q_transform;
/// \brief Transform velocity vector. /// \brief Transform velocity vector.
TangentVector_t v_transform; TangentVector_t m_v_transform;
public: public:
// data // data
Constraint_t S; Constraint_t m_S;
}; // struct JointDataMimic }; // struct JointDataMimic
...@@ -414,8 +420,8 @@ namespace pinocchio ...@@ -414,8 +420,8 @@ namespace pinocchio
typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
AffineTransform::run(qs.head(m_jmodel_ref.nq()), AffineTransform::run(qs.head(m_jmodel_ref.nq()),
m_scaling,m_offset,jdata.q_transform); m_scaling,m_offset,jdata.m_q_transform);
m_jmodel_ref.calc(jdata.jdata_ref,jdata.q_transform); m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform);
} }
template<typename ConfigVector, typename TangentVector> template<typename ConfigVector, typename TangentVector>
...@@ -427,11 +433,11 @@ namespace pinocchio ...@@ -427,11 +433,11 @@ namespace pinocchio
typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform;
AffineTransform::run(qs.head(m_jmodel_ref.nq()), AffineTransform::run(qs.head(m_jmodel_ref.nq()),
m_scaling,m_offset,jdata.q_transform); m_scaling,m_offset,jdata.m_q_transform);
jdata.v_transform = m_scaling * vs.head(m_jmodel_ref.nv()); jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
m_jmodel_ref.calc(jdata.jdata_ref, m_jmodel_ref.calc(jdata.m_jdata_ref,
jdata.q_transform, jdata.m_q_transform,
jdata.v_transform); jdata.m_v_transform);
} }
template<typename Matrix6Like> template<typename Matrix6Like>
...@@ -439,7 +445,8 @@ namespace pinocchio ...@@ -439,7 +445,8 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6Like> & I, const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I) const const bool update_I) const
{ {
m_jmodel_ref.calc_aba(data.jdata_ref, // TODO: fixme
m_jmodel_ref.calc_aba(data.m_jdata_ref,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
update_I); update_I);
} }
......
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