joint.hpp 6.2 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
22
23
24
25
26
27

#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"

namespace se3
{

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

  template<>
33
  struct traits<Joint>
34
35
36
37
38
39
  {

    enum {
      NQ = -1, // Dynamic because unknown at compilation
      NV = -1
    };
40
41
    typedef JointData JointDataDerived;
    typedef JointModel JointModelDerived;
42
43
44
45
46
47
48
49
50
51
52
53
54
55
    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;
  };
56
57
  template<> struct traits<JointData> { typedef Joint JointDerived; };
  template<> struct traits<JointModel> { typedef Joint JointDerived; };
58

59
  struct JointData : public JointDataBase<JointData> , JointDataVariant
60
  {
61
    typedef JointDataVariant JointDataBoostVariant;
62

63
    typedef Joint JointDerived;
64
65
    SE3_JOINT_TYPEDEF;

66
67
    JointDataVariant& toVariant() { return *static_cast<JointDataVariant*>(this); }
    const JointDataVariant& toVariant() const { return *static_cast<const JointDataVariant*>(this); }
68

69
70
71
72
    const Constraint_t      S() const  { return constraint_xd(*this); }
    const Transformation_t  M() const  { return joint_transform(*this); } // featherstone book, page 78 (sXp)
    const Motion_t          v() const  { return motion(*this); }
    const Bias_t            c() const  { return bias(*this); }
73
74
    
    // // [ABA CCRBA]
75
76
77
78
    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); }
79
80


81
82
    JointData() : JointDataBoostVariant() {}
    JointData(const JointDataVariant & jdata ) : JointDataBoostVariant(jdata){}
83
84
85

  };

86
  struct JointModel : public JointModelBase<JointModel> , JointModelVariant
87
  {
88
    typedef JointModelVariant JointModelBoostVariant;
89

90
    typedef Joint JointDerived;
91
92
    SE3_JOINT_TYPEDEF;
    SE3_JOINT_USE_INDEXES;
93
94
    using JointModelBase<JointModel>::id;
    using JointModelBase<JointModel>::setIndexes;
95

96
97
    JointModelVariant& toVariant() { return *static_cast<JointModelVariant*>(this); }
    const JointModelVariant& toVariant() const { return *static_cast<const JointModelVariant*>(this); }
98
99
100

    JointDataVariant createData() 
    {
101
      return ::se3::createData(*this);
102
103
104
105
106
107
    }


    void calc (JointData & data,
               const Eigen::VectorXd & qs) const
    {
108
      calc_zero_order(*this, data, qs);
109
110
111
112
113
114
115

    }

    void calc (JointData & data,
               const Eigen::VectorXd & qs,
               const Eigen::VectorXd & vs ) const
    {
116
      calc_first_order(*this, data, qs, vs);
117
118
119
120
    }
    
    void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
    {
121
      ::se3::calc_aba(*this, data, I, update_I);
122
123
124
125
    }

    ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
    {
126
      return ::se3::integrate(*this, qs, vs);
127
128
129
130
    }

    ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
    {
131
      return ::se3::interpolate(*this, q0, q1, u);
132
133
134
135
    }

    ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
    { 
136
      return ::se3::randomConfiguration(*this, lower_pos_limit, upper_pos_limit);
137
138
139
140
    } 

    TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
    { 
141
      return ::se3::difference(*this, q0, q1);
142
143
144
145
    } 

    double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
    { 
146
      return ::se3::distance(*this, q0, q1);
147
148
    }

149
150
151
    void normalize_impl(Eigen::VectorXd & q) const
    {
      return ::se3::normalize(*this, q);
152
153
    }

154
155
156
157
158
    ConfigVector_t neutralConfiguration_impl() const
    { 
      return ::se3::neutralConfiguration(*this);
    } 

159
160
    std::string shortname() const { return ::se3::shortname(*this); }
    static std::string classname() { return "JointModel"; }
161

162
163
164
165
166
167
    template <class D>
    bool operator == (const JointModelBase<D> &) const
    {
      return false;
    }
    
168
    bool operator == (const JointModelBase<JointModel> & jmodel) const
169
170
171
172
173
174
175
    {
      return jmodel.id() == id()
          && jmodel.idx_q() == idx_q()
          && jmodel.idx_v() == idx_v();
    }


176
177
    int     nq_impl() const { return ::se3::nq(*this); }
    int     nv_impl() const { return ::se3::nv(*this); }
178

179
180
    int     idx_q()   const { return ::se3::idx_q(*this); }
    int     idx_v()   const { return ::se3::idx_v(*this); }
181

182
    JointIndex     id()      const { return ::se3::id(*this); }
183

184
185
186
187
188
    // void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
    void setIndexes(JointIndex id,int nq,int nv) 
    {
      ::se3::setIndexes(*this,id, nq, nv);
    }
189
190
  };
  
191
192
  typedef std::vector<JointData> JointDataVector;  
  typedef std::vector<JointModel> JointModelVector;
193
194
195
196

} // namespace se3


197
#endif // ifndef __se3_joint_model_hpp__