joints-model.hpp

//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_serialization_joints_model_hpp__
#define __pinocchio_serialization_joints_model_hpp__

namespace pinocchio
{
 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct Serialize<JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>>
 {
 template<typename Archive>
 static void
 run(Archive & ar, JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> & joint)
 {
 using boost::serialization::make_nvp;

 ar & make_nvp("m_nq", joint.m_nq);
 ar & make_nvp("m_nv", joint.m_nv);
 ar & make_nvp("m_nvExtended", joint.m_nvExtended);
 ar & make_nvp("m_idx_q", joint.m_idx_q);
 ar & make_nvp("m_nqs", joint.m_nqs);
 ar & make_nvp("m_idx_v", joint.m_idx_v);
 ar & make_nvp("m_nvs", joint.m_nvs);
 ar & make_nvp("m_idx_vExtended", joint.m_idx_vExtended);
 ar & make_nvp("m_nvExtendeds", joint.m_nvExtendeds);
 ar & make_nvp("njoints", joint.njoints);

 ar & make_nvp("joints", joint.joints);
 ar & make_nvp("jointPlacements", joint.jointPlacements);
 ar & make_nvp("jointNames", joint.jointNames);
 }
 };
} // namespace pinocchio

namespace boost
{
 namespace serialization
 {

 // For some older version of gcc, we have to rely on an additional namespace
 // to avoid ambiguous call to boost::serialization::serialize
 namespace fix
 {
 template<class Archive, typename Derived>
 void serialize(
 Archive & ar, pinocchio::JointModelBase<Derived> & joint, const unsigned int version)
 {
 split_free(ar, joint, version);
 }
 } // namespace fix

 template<class Archive, typename Derived>
 void save(
 Archive & ar,
 const pinocchio::JointModelBase<Derived> & joint,
 const unsigned int /*version*/)
 {
 const pinocchio::JointIndex i_id = joint.id();
 const int i_q = joint.idx_q(), i_v = joint.idx_v(), i_vExtended = joint.idx_vExtended();

 ar & make_nvp("i_id", i_id);
 ar & make_nvp("i_q", i_q);
 ar & make_nvp("i_v", i_v);
 ar & make_nvp("i_vExtended", i_vExtended);
 }

 template<typename Derived>
 class SetJointIndexes
 {
 Derived & joint;

 public:
 explicit SetJointIndexes(Derived & joint)
 : joint(joint) {};

 void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_vExtended)
 {
 joint.setIndexes(i_id, i_q, i_v, i_vExtended);
 }
 };

 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 class SetJointIndexes<pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
 {
 pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl> & joint_m;

 public:
 explicit SetJointIndexes(
 pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl> & joint)
 : joint_m(joint) {};

 void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_vExtended)
 {
 joint_m.setMimicIndexes(joint_m.jmodel().id(), i_q, i_v, joint_m.jmodel().idx_vExtended());
 joint_m.setIndexes(i_id, 0, 0, i_vExtended);
 }
 };

 template<class Archive, typename Derived>
 void
 load(Archive & ar, pinocchio::JointModelBase<Derived> & joint, const unsigned int /*version*/)
 {
 pinocchio::JointIndex i_id;
 int i_q, i_v, i_vExtended;

 ar & make_nvp("i_id", i_id);
 ar & make_nvp("i_q", i_q);
 ar & make_nvp("i_v", i_v);
 ar & make_nvp("i_vExtended", i_vExtended);

 SetJointIndexes<Derived>(joint.derived()).run(i_id, i_q, i_v, i_vExtended);
 }

 template<class Archive, typename Scalar, int Options, int axis>
 void serialize(
 Archive & ar,
 pinocchio::JointModelRevoluteTpl<Scalar, Options, axis> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, axis> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options, int axis>
 void serialize(
 Archive & ar,
 pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options, int axis>
 void serialize(
 Archive & ar,
 pinocchio::JointModelPrismaticTpl<Scalar, Options, axis> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelPrismaticTpl<Scalar, Options, axis> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options, int axis>
 void serialize(
 Archive & ar,
 pinocchio::JointModelHelicalTpl<Scalar, Options, axis> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelHelicalTpl<Scalar, Options, axis> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("m_pitch", joint.m_pitch);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("axis", joint.axis);
 ar & make_nvp("m_pitch", joint.m_pitch);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelFreeFlyerTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelFreeFlyerTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelPlanarTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelPlanarTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelSphericalTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelSphericalTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelSphericalZYXTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelSphericalZYXTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelEllipsoidTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelEllipsoidTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("radius_x", joint.radius_x);
 ar & make_nvp("radius_y", joint.radius_y);
 ar & make_nvp("radius_z", joint.radius_z);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelTranslationTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelTranslationTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("axis", joint.axis);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("axis", joint.axis);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("axis", joint.axis);
 }

 template<class Archive, typename Scalar, int Options>
 void serialize(
 Archive & ar,
 pinocchio::JointModelUniversalTpl<Scalar, Options> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelUniversalTpl<Scalar, Options> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 ar & make_nvp("axis1", joint.axis1);
 ar & make_nvp("axis2", joint.axis2);
 }

 template<
 class Archive,
 typename Scalar,
 int Options,
 template<typename S, int O> class JointCollectionTpl>
 void serialize(
 Archive & ar,
 pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);

 ::pinocchio::Serialize<JointType>::run(ar, joint);
 }

 template<
 class Archive,
 typename Scalar,
 int Options,
 template<typename S, int O> class JointCollectionTpl>
 void serialize(
 Archive & ar,
 pinocchio::JointModelTpl<Scalar, Options, JointCollectionTpl> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelTpl<Scalar, Options, JointCollectionTpl> JointType;
 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);

 typedef typename JointCollectionTpl<Scalar, Options>::JointModelVariant JointModelVariant;
 ar & make_nvp("base_variant", base_object<JointModelVariant>(joint));
 }

 template<
 class Archive,
 typename Scalar,
 int Options,
 template<typename S, int O> class JointCollectionTpl>
 void serialize(
 Archive & ar,
 pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl> & joint,
 const unsigned int version)
 {
 typedef pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointType;
 ar & make_nvp("jmodel", joint.jmodel());
 ar & make_nvp("scaling", joint.scaling());
 ar & make_nvp("offset", joint.offset());

 fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
 }

 } // namespace serialization
} // namespace boost

#endif // ifndef __pinocchio_serialization_joints_model_hpp__