joint.hpp 4.77 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

18
19
#ifndef __se3_joint_model_hpp__
#define __se3_joint_model_hpp__
20
21

#include "pinocchio/assert.hpp"
22
#include "pinocchio/multibody/joint/joint-collection.hpp"
23
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
24
#include "pinocchio/container/aligned-vector.hpp"
25
26
27
28

namespace se3
{

29
30
31
  struct Joint;
  struct JointModel;
  struct JointData;
32
33

  template<>
34
  struct traits<Joint>
35
36
37
38
39
  {
    enum {
      NQ = -1, // Dynamic because unknown at compilation
      NV = -1
    };
40
    typedef double Scalar;
41
42
    typedef JointData JointDataDerived;
    typedef JointModel JointModelDerived;
43
44
45
46
47
48
49
50
51
52
53
54
55
56
    typedef ConstraintXd Constraint_t;
    typedef SE3 Transformation_t;
    typedef Motion Motion_t;
    typedef Motion Bias_t;

    typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
    // [ABA]
    typedef Eigen::Matrix<double,6,Eigen::Dynamic> U_t;
    typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> D_t;
    typedef Eigen::Matrix<double,6,Eigen::Dynamic> UD_t;

    typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
    typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
  };
jcarpent's avatar
jcarpent committed
57
  
58
59
  template<> struct traits<JointData> { typedef Joint JointDerived; };
  template<> struct traits<JointModel> { typedef Joint JointDerived; };
60

61
  struct JointData : public JointDataBase<JointData> , JointDataVariant
62
  {
jcarpent's avatar
jcarpent committed
63
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
    typedef Joint JointDerived;
jcarpent's avatar
jcarpent committed
65
    
66
67
    SE3_JOINT_TYPEDEF;

jcarpent's avatar
jcarpent committed
68
69
    JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
    const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
70

71
    const Constraint_t      S() const  { return constraint_xd(*this); }
jcarpent's avatar
jcarpent committed
72
    const Transformation_t  M() const  { return joint_transform(*this); }
73
74
    const Motion_t          v() const  { return motion(*this); }
    const Bias_t            c() const  { return bias(*this); }
75
76
    
    // // [ABA CCRBA]
77
78
79
80
    const U_t               U()     const { return u_inertia(*this); }
    U_t                     U()           { return u_inertia(*this); }
    const D_t               Dinv()  const { return dinv_inertia(*this); }
    const UD_t              UDinv() const { return udinv_inertia(*this); }
81

jcarpent's avatar
jcarpent committed
82
83
    JointData() : JointDataVariant() {}
    JointData(const JointDataVariant & jdata) : JointDataVariant(jdata) {}
84
85
86

  };

87
  struct JointModel : public JointModelBase<JointModel> , JointModelVariant
88
  {
jcarpent's avatar
jcarpent committed
89
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
90
    typedef JointModelVariant JointModelBoostVariant;
91
    typedef Joint JointDerived;
jcarpent's avatar
jcarpent committed
92
    
93
94
    SE3_JOINT_TYPEDEF;
    SE3_JOINT_USE_INDEXES;
95
96
97
    using Base::id;
    using Base::setIndexes;
    using Base::operator==;
98

jcarpent's avatar
jcarpent committed
99
100
    JointModel() : JointModelVariant() {}
    JointModel(const JointModelVariant & model_variant) : JointModelVariant(model_variant)
jcarpent's avatar
jcarpent committed
101
102
    {}
    
103
104
    JointModelVariant& toVariant() { return *static_cast<JointModelVariant*>(this); }
    const JointModelVariant& toVariant() const { return *static_cast<const JointModelVariant*>(this); }
105

jcarpent's avatar
jcarpent committed
106
    JointDataVariant createData() { return ::se3::createData(*this); }
107

jcarpent's avatar
jcarpent committed
108
    void calc(JointData & data,const Eigen::VectorXd & q) const { calc_zero_order(*this,data,q); }
109

jcarpent's avatar
jcarpent committed
110
111
    void calc(JointData & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) const
    { calc_first_order(*this,data,q,v); }
112
113
    
    void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
jcarpent's avatar
jcarpent committed
114
    { ::se3::calc_aba(*this,data,I,update_I); }
115
116
    std::string shortname() const { return ::se3::shortname(*this); }
    static std::string classname() { return "JointModel"; }
117

118
119
    int     nq_impl() const { return ::se3::nq(*this); }
    int     nv_impl() const { return ::se3::nv(*this); }
120

121
122
    int     idx_q()   const { return ::se3::idx_q(*this); }
    int     idx_v()   const { return ::se3::idx_v(*this); }
123

124
    JointIndex     id()      const { return ::se3::id(*this); }
125

jcarpent's avatar
jcarpent committed
126
    void setIndexes(JointIndex id,int nq,int nv) { ::se3::setIndexes(*this,id, nq, nv); }
127
128
  };
  
129
130
  typedef container::aligned_vector<JointData> JointDataVector;
  typedef container::aligned_vector<JointModel> JointModelVector;
131
132
133

} // namespace se3

134
#endif // ifndef __se3_joint_model_hpp__