Commit 73c309ed authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub

Merge pull request #998 from jcarpent/devel

WIP: add Pickling of Data
parents 0708f314 f779b9f0
......@@ -6,9 +6,11 @@
#define __pinocchio_python_data_hpp__
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/serialization/data.hpp"
#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/bindings/python/serialization/serializable.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
#include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"
......@@ -21,7 +23,45 @@ namespace pinocchio
namespace python
{
namespace bp = boost::python;
template<typename Data>
struct PickleData : bp::pickle_suite
{
static bp::tuple getinitargs(const Data &)
{
return bp::make_tuple();
}
static bp::tuple getstate(const Data & data)
{
const std::string str(data.saveToString());
return bp::make_tuple(bp::str(str));
}
static void setstate(Data & data, bp::tuple tup)
{
if(bp::len(tup) == 0 || bp::len(tup) > 1)
{
throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
"The pickle data structure contains too many elements.");
}
bp::object py_obj = tup[0];
boost::python::extract<std::string> obj_as_string(py_obj.ptr());
if(obj_as_string.check())
{
const std::string str = obj_as_string;
data.loadFromString(str);
}
else
{
throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
"The entry is not a string.");
}
}
};
struct DataPythonVisitor
: public boost::python::def_visitor< DataPythonVisitor >
{
......@@ -52,6 +92,7 @@ namespace pinocchio
void visit(PyClass& cl) const
{
cl
.def(bp::init<>(bp::arg("self"),"Default constructor."))
.def(bp::init<Model>(bp::arg("model"),"Constructs a data structure from a given model."))
.ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a,"Joint spatial acceleration")
......@@ -111,15 +152,18 @@ namespace pinocchio
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(double,potential_energy,"Potential energy in [J] computed by potentialEnergy")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,impulse_c,"Lagrange Multipliers linked to contact impulses")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,dq_after,"Generalized velocity after the impact.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix3x,staticRegressor,"Static regressor.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,jointTorqueRegressor,"Joint Torque Regressor.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,jointTorqueRegressor,"Joint torque regressor.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
}
......@@ -127,11 +171,14 @@ namespace pinocchio
static void expose()
{
bp::class_<Data>("Data",
"Articulated rigid body data.\n"
"It contains all the data that can be modified by the algorithms.",
"Articulated rigid body data related to a Model.\n"
"It contains all the data that can be modified by the Pinocchio algorithms.",
bp::no_init)
.def(DataPythonVisitor())
.def(CopyableVisitor<Data>());
.def(CopyableVisitor<Data>())
.def(SerializableVisitor<Data>())
.def_pickle(PickleData<Data>());
StdAlignedVectorPythonVisitor<Vector3, true>::expose("StdVec_vec3d");
StdAlignedVectorPythonVisitor<Matrix6x, true>::expose("StdVec_Matrix6x");
StdVectorPythonVisitor<int>::expose("StdVec_int");
......
......@@ -7,7 +7,6 @@
#define __pinocchio_python_model_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/bindings/python/serialization/serializable.hpp"
#include "pinocchio/serialization/model.hpp"
#include <boost/python/suite/indexing/map_indexing_suite.hpp>
......@@ -16,6 +15,7 @@
#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/bindings/python/serialization/serializable.hpp"
#include "pinocchio/bindings/python/utils/eigen_container.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
......
......@@ -98,6 +98,11 @@ namespace pinocchio
{
return derived().motionAction(v);
}
bool operator==(const ConstraintBase<Derived> & other) const
{
return derived().isEqual(other.derived());
}
}; // class ConstraintBase
......
......@@ -74,6 +74,11 @@ namespace pinocchio
YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
}
static ConstraintTpl Zero(const int dim)
{
return ConstraintTpl(dim);
}
template<typename VectorLike>
JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & vj) const
{
......@@ -148,6 +153,11 @@ namespace pinocchio
void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
bool isEqual(const ConstraintTpl & other) const
{
return S == other.S;
}
protected:
DenseBase S;
}; // class ConstraintTpl
......
......@@ -17,6 +17,8 @@
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/serialization/serializable.hpp"
#include <iostream>
#include <Eigen/Cholesky>
......@@ -25,6 +27,7 @@ namespace pinocchio
template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
struct DataTpl
: serialization::Serializable< DataTpl<_Scalar,_Options,JointCollectionTpl> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -142,7 +145,7 @@ namespace pinocchio
container::aligned_vector<Inertia> Ycrb;
/// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
container::aligned_vector<Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs M;
......@@ -175,19 +178,19 @@ namespace pinocchio
Matrix6x IS;
/// \brief Right variation of the inertia matrix
container::aligned_vector<typename Inertia::Matrix6> vxI;
container::aligned_vector<Matrix6> vxI;
/// \brief Left variation of the inertia matrix
container::aligned_vector<typename Inertia::Matrix6> Ivx;
container::aligned_vector<Matrix6> Ivx;
/// \brief Inertia quantities expressed in the world frame
container::aligned_vector<Inertia> oYcrb;
/// \brief Time variation of the inertia quantities expressed in the world frame
container::aligned_vector<typename Inertia::Matrix6> doYcrb;
container::aligned_vector<Matrix6> doYcrb;
/// \brief Temporary for derivative algorithms
typename Inertia::Matrix6 Itmp;
Matrix6 Itmp;
/// \brief Temporary for derivative algorithms
Matrix6 M6tmp;
......@@ -199,7 +202,7 @@ namespace pinocchio
// ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
container::aligned_vector<typename Inertia::Matrix6> Yaba; // TODO: change with dense symmetric matrix6
container::aligned_vector<Matrix6> Yaba; // TODO: change with dense symmetric matrix6
/// \brief Intermediate quantity corresponding to apparent torque [ABA]
TangentVectorType u; // Joint Inertia
......@@ -361,6 +364,11 @@ namespace pinocchio
/// \param[in] model The model structure of the rigid body system.
///
explicit DataTpl(const Model & model);
///
/// \brief Default constructor
///
DataTpl() {}
private:
void computeLastChild(const Model & model);
......
This diff is collapsed.
......@@ -12,6 +12,11 @@
namespace pinocchio
{
/**
* \addtogroup multibody
* @{
*/
template<typename Scalar, int Options=0> struct FrameTpl;
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
......@@ -19,11 +24,6 @@ namespace pinocchio
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
struct DataTpl;
/**
* \addtogroup multibody
* @{
*/
typedef std::size_t Index;
typedef Index JointIndex;
typedef Index GeomIndex;
......
......@@ -2,8 +2,8 @@
// Copyright (c) 2016-2019 CNRS INRIA
//
#ifndef __pinocchio_joint_fwd_hpp__
#define __pinocchio_joint_fwd_hpp__
#ifndef __pinocchio_multibody_joint_fwd_hpp__
#define __pinocchio_multibody_joint_fwd_hpp__
#include "pinocchio/fwd.hpp"
......@@ -101,4 +101,6 @@ namespace pinocchio
// end of group joint
}
#endif // ifndef __pinocchio_joint_fwd_hpp__
#include "pinocchio/multibody/fwd.hpp"
#endif // ifndef __pinocchio_multibody_joint_fwd_hpp__
This diff is collapsed.
......@@ -77,9 +77,12 @@ namespace pinocchio
JointDataCompositeTpl()
: joints()
, iMlast(0), pjMi(0)
, iMlast(0)
, pjMi(0)
, S(0)
, M(),v(),c()
, M(Transformation_t::Identity())
, v(Motion_t::Zero())
, c(Motion_t::Zero())
, U(6,0), Dinv(0,0), UDinv(6,0)
, StU(0,0)
{}
......@@ -87,10 +90,14 @@ namespace pinocchio
JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
: joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
, S(nv)
, M(), v(), c()
, U(6,nv), Dinv(nv,nv), UDinv(6,nv)
, StU(nv,nv)
, S(Constraint_t::Zero(nv))
, M(Transformation_t::Identity())
, v(Motion_t::Zero())
, c(Motion_t::Zero())
, U(U_t::Zero(6,nv))
, Dinv(D_t::Zero(nv,nv))
, UDinv(UD_t::Zero(6,nv))
, StU(D_t::Zero(nv,nv))
{}
/// \brief Vector of joints
......
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_multibody_joint_data_base_hpp__
#define __pinocchio_multibody_joint_data_base_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-model-base.hpp"
#define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,TYPENAME) \
PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,TYPENAME); \
typedef TYPENAME traits<Joint>::ConstraintTypeConstRef ConstraintTypeConstRef; \
typedef TYPENAME traits<Joint>::ConstraintTypeRef ConstraintTypeRef; \
typedef TYPENAME traits<Joint>::TansformTypeConstRef TansformTypeConstRef; \
typedef TYPENAME traits<Joint>::TansformTypeRef TansformTypeRef; \
typedef TYPENAME traits<Joint>::MotionTypeConstRef MotionTypeConstRef; \
typedef TYPENAME traits<Joint>::MotionTypeRef MotionTypeRef; \
typedef TYPENAME traits<Joint>::BiasTypeConstRef BiasTypeConstRef; \
typedef TYPENAME traits<Joint>::BiasTypeRef BiasTypeRef; \
typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef; \
typedef TYPENAME traits<Joint>::UTypeRef UTypeRef; \
typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef; \
typedef TYPENAME traits<Joint>::DTypeRef DTypeRef; \
typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef; \
typedef TYPENAME traits<Joint>::UDTypeRef UDTypeRef
#ifdef __clang__
#define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
#define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
#else
#define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
#endif
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \
ConstraintTypeConstRef S_accessor() const { return S; } \
ConstraintTypeRef S_accessor() { return S; } \
TansformTypeConstRef M_accessor() const { return M; } \
TansformTypeRef M_accessor() { return M; } \
MotionTypeConstRef v_accessor() const { return v; } \
MotionTypeRef v_accessor() { return v; } \
BiasTypeConstRef c_accessor() const { return c; } \
BiasTypeRef c_accessor() { return c; } \
UTypeConstRef U_accessor() const { return U; } \
UTypeRef U_accessor() { return U; } \
DTypeConstRef Dinv_accessor() const { return Dinv; } \
DTypeRef Dinv_accessor() { return Dinv; } \
UDTypeConstRef UDinv_accessor() const { return UDinv; } \
UDTypeRef UDinv_accessor() { return UDinv; }
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
typedef const Constraint_t & ConstraintTypeConstRef; \
typedef Constraint_t & ConstraintTypeRef; \
typedef const Transformation_t & TansformTypeConstRef; \
typedef Transformation_t & TansformTypeRef; \
typedef const Motion_t & MotionTypeConstRef; \
typedef Motion_t & MotionTypeRef; \
typedef const Bias_t & BiasTypeConstRef; \
typedef Bias_t & BiasTypeRef; \
typedef const U_t & UTypeConstRef; \
typedef U_t & UTypeRef; \
typedef const D_t & DTypeConstRef; \
typedef D_t & DTypeRef; \
typedef const UD_t & UDTypeConstRef; \
typedef UD_t & UDTypeRef;
namespace pinocchio
{
template<typename Derived>
struct JointDataBase
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename traits<Derived>::JointDerived JointDerived;
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Derived & derived() { return *static_cast<Derived*>(this); }
const Derived & derived() const { return *static_cast<const Derived*>(this); }
ConstraintTypeConstRef S() const { return derived().S_accessor(); }
ConstraintTypeRef S() { return derived().S_accessor(); }
TansformTypeConstRef M() const { return derived().M_accessor(); }
TansformTypeRef M() { return derived().M_accessor(); }
MotionTypeConstRef v() const { return derived().v_accessor(); }
MotionTypeRef v() { return derived().v_accessor(); }
BiasTypeConstRef c() const { return derived().c_accessor(); }
BiasTypeRef c() { return derived().c_accessor(); }
UTypeConstRef U() const { return derived().U_accessor(); }
UTypeRef U() { return derived().U_accessor(); }
DTypeConstRef Dinv() const { return derived().Dinv_accessor(); }
DTypeRef Dinv() { return derived().Dinv_accessor(); }
UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); }
UDTypeRef UDinv() { return derived().UDinv_accessor(); }
std::string shortname() const { return derived().shortname(); }
static std::string classname() { return Derived::classname(); }
void disp(std::ostream & os) const
{
using namespace std;
os << shortname() << endl;
}
friend std::ostream & operator << (std::ostream & os, const JointDataBase<Derived> & joint)
{
joint.disp(os);
return os;
}
bool operator==(const JointDataBase<Derived> & other) const
{
return derived().isEqual(other.derived());
}
/// \brief Default operator== implementation
bool isEqual(const JointDataBase<Derived> & other) const
{
return S() == other.S()
&& M() == other.M()
&& v() == other.v()
&& c() == other.c()
&& U() == other.U()
&& Dinv() == other.Dinv()
&& UDinv() == other.UDinv()
;
}
bool operator!=(const JointDataBase<Derived> & other) const
{
return derived().isNotEqual(other.derived());
}
/// \brief Default operator!= implementation
bool isNotEqual(const JointDataBase<Derived> & other) const
{
return !(derived() == other.derived());
}
protected:
/// \brief Default constructor: protected.
inline JointDataBase() {}
}; // struct JointDataBase
} // namespace pinocchio
#endif // ifndef __pinocchio_multibody_joint_data_base_hpp__
......@@ -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>
......@@ -147,7 +149,7 @@ namespace pinocchio
typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionTpl<Scalar,Options> Motion_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef MotionZeroTpl<Scalar,Options> Bias_t;
// [ABA]
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
......@@ -186,7 +188,13 @@ namespace pinocchio
D_t Dinv;
UD_t UDinv;
JointDataFreeFlyerTpl() : M(1), U(), Dinv(), UDinv(UD_t::Identity()) {}
JointDataFreeFlyerTpl()
: M(Transformation_t::Identity())
, v(Motion_t::Zero())
, U(U_t::Zero())
, Dinv(D_t::Zero())
, UDinv(UD_t::Identity())
{}
static std::string classname() { return std::string("JointDataFreeFlyer"); }
std::string shortname() const { return classname(); }
......
//
// Copyright (c) 2016,2018 CNRS
// Copyright (c) 2016-2019 CNRS INRIA
//
#ifndef __pinocchio_joint_generic_hpp__
......@@ -44,13 +44,19 @@ namespace pinocchio
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
typedef Constraint_t ConstraintTypeConstRef;
typedef Constraint_t ConstraintTypeRef;
typedef Transformation_t TansformTypeConstRef;
typedef Transformation_t TansformTypeRef;
typedef Motion_t MotionTypeConstRef;
typedef Motion_t MotionTypeRef;
typedef Bias_t BiasTypeConstRef;
typedef Bias_t BiasTypeRef;
typedef U_t UTypeConstRef;
typedef U_t UTypeRef;
typedef D_t DTypeConstRef;
typedef D_t DTypeRef;
typedef UD_t UDTypeConstRef;
typedef UD_t UDTypeRef;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
......@@ -79,6 +85,9 @@ namespace pinocchio
typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
typedef typename JointCollection::JointDataVariant JointDataVariant;
using Base::operator==;
using Base::operator!=;
JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
......@@ -92,7 +101,9 @@ namespace pinocchio
D_t Dinv() const { return dinv_inertia(*this); }
UD_t UDinv() const { return udinv_inertia(*this); }
JointDataTpl() : JointDataVariant() {}
JointDataTpl()
: JointDataVariant()
{}
JointDataTpl(const JointDataVariant & jdata_variant)
: JointDataVariant(jdata_variant)
......@@ -105,7 +116,7 @@ namespace pinocchio
BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
}
/// Define all the standard accessors
// Define all the standard accessors
Constraint_t S_accessor() const { return S(); }
Transformation_t M_accessor() const { return M(); }
Motion_t v_accessor() const { return v(); }
......@@ -116,6 +127,12 @@ namespace pinocchio
static std::string classname() { return "JointData"; }
std::string shortname() const { return ::pinocchio::shortname(*this); }
bool isEqual(const JointDataTpl & other) const
{
return Base::isEqual(other)
&& toVariant() == other.toVariant();
}
};
......@@ -135,7 +152,7 @@ namespace pinocchio
typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
PINOCCHIO_JOINT_USE_INDEXES;
PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl);
typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef typename JointCollection::JointDataVariant JointDataVariant;
......
......@@ -71,6 +71,10 @@ namespace pinocchio
ScaledConstraint() {}
explicit ScaledConstraint(const Scalar & scaling_factor)
: m_scaling_factor(scaling_factor)
{}
ScaledConstraint(const Constraint & constraint,
const Scalar & scaling_factor)
: m_constraint(constraint)
......@@ -153,7 +157,16 @@ namespace pinocchio
}
inline const Scalar & scaling() const { return m_scaling_factor; }
inline Scalar & scaling() { return m_scaling_factor; }
inline const Constraint & constraint() const { return m_constraint; }
inline Constraint & constraint() { return m_constraint; }
bool isEqual(const ScaledConstraint & other) const
{
return m_constraint == other.m_constraint
&& m_scaling_factor == other.m_scaling_factor;
}
protected:
......@@ -271,23 +284,40 @@ namespace pinocchio
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
JointDataMimic() {}
JointDataMimic()
: m_scaling((Scalar)0)
, m_q_transform(ConfigVector_t::Zero())
, m_v_transform(TangentVector_t::Zero())
, S((Scalar)0)
{}
JointDataMimic(const JointDataBase<JointData> & jdata,
const Scalar & scaling)
: jdata_ref(jdata.derived())
, scaling(scaling)
, S(jdata_ref.S,scaling)
: m_jdata_ref(jdata.derived())
, m_scaling(scaling)
, S(m_jdata_ref.S,scaling)
{}
JointDataMimic & operator=(const JointDataMimic & other)
{
jdata_ref = other.jdata_ref;
scaling = other.scaling;
S = Constraint_t(jdata_ref.S,other.scaling);
m_jdata_ref = other.m_jdata_ref;
m_scaling = other.m_scaling;
m_q_transform = other.m_q_transform;
m_v_transform = other.m_v_transform;
S = Constraint_t(m_jdata_ref.S,other.m_scaling);
return *this;
}
bool isEqual(const JointDataMimic & other) const
{
return Base::isEqual(other)
&& m_jdata_ref == other.m_jdata_ref
&& m_scaling == other.m_scaling
&& m_q_transform == other.m_q_transform
&& m_v_transform == other.m_v_transform
;
}
static std::string classname()
{
return std::string("JointDataMimic<") + JointData::classname() + std::string(">");
......@@ -295,31 +325,55 @@ namespace pinocchio
std::string shortname() const
{
return std::string("JointDataMimic<") + jdata_ref.shortname() + std::string(">");
return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">");
}
// Accessors
ConstraintTypeConstRef S_accessor() const { return S; }
TansformTypeConstRef M_accessor() const { return jdata_ref.M; }
MotionTypeConstRef v_accessor() const { return jdata_ref.v; }
BiasTypeConstRef c_accessor() const { return jdata_ref.c; }
UTypeConstRef U_accessor() const { return jdata_ref.U; }
UTypeRef U_accessor() { return jdata_ref.U; }
DTypeConstRef Dinv_accessor() const { return jdata_ref.Dinv; }
UDTypeConstRef UDinv_accessor() const { return jdata_ref.UDinv; }
ConstraintTypeRef S_accessor() { return S; }
TansformTypeConstRef M_accessor() const { return m_jdata_ref.M; }
TansformTypeRef M_accessor() { return m_jdata_ref.M; }
MotionTypeConstRef v_accessor() const { return m_jdata_ref.v; }
MotionTypeRef v_accessor() { return m_jdata_ref.v; }
BiasTypeConstRef c_accessor() const { return m_jdata_ref.c; }
BiasTypeRef c_accessor() { return m_jdata_ref.c; }
UTypeConstRef U_accessor() const { return m_jdata_ref.U; }
UTypeRef U_accessor() { return m_jdata_ref.U; }
DTypeConstRef Dinv_accessor() const { return m_jdata_ref.Dinv; }
DTypeRef Dinv_accessor() { return m_jdata_ref.Dinv; }
UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; }
UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; }
template<class JointModel>
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; }
ConfigVector_t & jointConfiguration() { return m_q_transform; }
const ConfigVector_t & jointConfiguration() const { return m_q_transform; }
TangentVector_t & jointVelocity() { return m_v_transform; }
const TangentVector_t & jointVelocity() const { return m_v_transform; }
protected:
JointData jdata_ref;
Scalar scaling;
JointData m_jdata_ref;