Program Listing for File model.hpp
↰ Return to documentation for file (include/pinocchio/multibody/model.hpp
)
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_multibody_model_hpp__
#define __pinocchio_multibody_model_hpp__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/serialization/serializable.hpp"
#include <map>
#include <iterator>
namespace pinocchio
{
template<
typename NewScalar,
typename Scalar,
int Options,
template<typename, int> class JointCollectionTpl>
struct CastType<NewScalar, ModelTpl<Scalar, Options, JointCollectionTpl>>
{
typedef ModelTpl<NewScalar, Options, JointCollectionTpl> type;
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct traits<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
{
typedef _Scalar Scalar;
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct ModelTpl
: serialization::Serializable<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
, NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
enum
{
Options = _Options
};
typedef JointCollectionTpl<Scalar, Options> JointCollection;
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
typedef SE3Tpl<Scalar, Options> SE3;
typedef MotionTpl<Scalar, Options> Motion;
typedef ForceTpl<Scalar, Options> Force;
typedef InertiaTpl<Scalar, Options> Inertia;
typedef FrameTpl<Scalar, Options> Frame;
typedef pinocchio::Index Index;
typedef pinocchio::JointIndex JointIndex;
typedef pinocchio::GeomIndex GeomIndex;
typedef pinocchio::FrameIndex FrameIndex;
typedef std::vector<Index> IndexVector;
typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel;
typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) InertiaVector;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(SE3) SE3Vector;
typedef VectorXs ConfigVectorType;
typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
typedef VectorXs TangentVectorType;
int nq;
int nv;
int njoints;
int nbodies;
int nframes;
InertiaVector inertias;
SE3Vector jointPlacements;
JointModelVector joints;
std::vector<int> idx_qs;
std::vector<int> nqs;
std::vector<int> idx_vs;
std::vector<int> nvs;
std::vector<JointIndex> parents;
std::vector<IndexVector> children;
std::vector<std::string> names;
ConfigVectorMap referenceConfigurations;
VectorXs armature;
TangentVectorType rotorInertia;
TangentVectorType rotorGearRatio;
TangentVectorType friction;
TangentVectorType damping;
TangentVectorType effortLimit;
TangentVectorType velocityLimit;
ConfigVectorType lowerPositionLimit;
ConfigVectorType upperPositionLimit;
FrameVector frames;
std::vector<IndexVector> supports;
std::vector<IndexVector> subtrees;
Motion gravity;
static const Vector3 gravity981;
std::string name;
ModelTpl()
: nq(0)
, nv(0)
, njoints(1)
, nbodies(1)
, nframes(0)
, inertias(1, Inertia::Zero())
, jointPlacements(1, SE3::Identity())
, joints(1)
, idx_qs(1, 0)
, nqs(1, 0)
, idx_vs(1, 0)
, nvs(1, 0)
, parents(1, 0)
, children(1)
, names(1)
, supports(1, IndexVector(1, 0))
, subtrees(1)
, gravity(gravity981, Vector3::Zero())
{
names[0] = "universe"; // Should be "universe joint (trivial)"
// FIXME Should the universe joint be a FIXED_JOINT even if it is
// in the list of joints ? See comment in definition of
// Model::addJointFrame and Model::addBodyFrame
addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
}
template<typename S2, int O2>
explicit ModelTpl(const ModelTpl<S2, O2> & other)
{
*this = other.template cast<Scalar>();
}
template<typename NewScalar>
typename CastType<NewScalar, ModelTpl>::type cast() const;
bool operator==(const ModelTpl & other) const;
bool operator!=(const ModelTpl & other) const
{
return !(*this == other);
}
JointIndex addJoint(
const JointIndex parent,
const JointModel & joint_model,
const SE3 & joint_placement,
const std::string & joint_name);
JointIndex addJoint(
const JointIndex parent,
const JointModel & joint_model,
const SE3 & joint_placement,
const std::string & joint_name,
const VectorXs & max_effort,
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config);
JointIndex addJoint(
const JointIndex parent,
const JointModel & joint_model,
const SE3 & joint_placement,
const std::string & joint_name,
const VectorXs & max_effort,
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config,
const VectorXs & friction,
const VectorXs & damping);
FrameIndex addJointFrame(const JointIndex & joint_index, int previous_frame_index = -1);
void appendBodyToJoint(
const JointIndex joint_index,
const Inertia & Y,
const SE3 & body_placement = SE3::Identity());
FrameIndex addBodyFrame(
const std::string & body_name,
const JointIndex & parentJoint,
const SE3 & body_placement = SE3::Identity(),
int parentFrame = -1);
FrameIndex getBodyId(const std::string & name) const;
bool existBodyName(const std::string & name) const;
JointIndex getJointId(const std::string & name) const;
bool existJointName(const std::string & name) const;
FrameIndex getFrameId(
const std::string & name,
const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;
bool existFrame(
const std::string & name,
const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;
FrameIndex addFrame(const Frame & frame, const bool append_inertia = true);
template<typename D>
bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
{
return checker.checkModel(*this);
}
std::vector<bool> hasConfigurationLimit();
std::vector<bool> hasConfigurationLimitInTangent();
bool check() const;
bool check(const Data & data) const;
protected:
void addJointIndexToParentSubtrees(const JointIndex joint_id);
};
} // namespace pinocchio
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/model.hxx"
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
#include "pinocchio/multibody/model.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
#endif // ifndef __pinocchio_multibody_model_hpp__