model.hxx 11.8 KB
Newer Older
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
4
5
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

6
7
#ifndef __pinocchio_model_hxx__
#define __pinocchio_model_hxx__
8
9

#include "pinocchio/spatial/fwd.hpp"
jcarpent's avatar
jcarpent committed
10
#include "pinocchio/utils/string-generator.hpp"
11
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
12

13
#include <boost/bind.hpp>
14
#include <boost/utility.hpp>
15

jcarpent's avatar
jcarpent committed
16
17
/// @cond DEV

18
namespace pinocchio
19
{
20
21
  namespace details
  {
22
23
24
    struct FilterFrame
    {
      const std::string & name;
25
      const FrameType & typeMask;
26
27
28
29
30
31
32
      
      FilterFrame(const std::string& name, const FrameType & typeMask)
      : name(name), typeMask(typeMask)
      {}
      
      template<typename Scalar, int Options>
      bool operator()(const FrameTpl<Scalar,Options> & frame) const
33
      { return (typeMask & frame.type) && (name == frame.name); }
34
      
35
36
    };
  }
37
38
39
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  const typename ModelTpl<Scalar,Options,JointCollectionTpl>::
40
  Vector3 ModelTpl<Scalar,Options,JointCollectionTpl>::gravity981((Scalar)0,(Scalar)0,(Scalar)-9.81);
41

42
43
44
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream& operator<<(std::ostream & os,
                                  const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
45
  {
46
    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::Index Index;
47
    
48
    os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
49
    for(Index i=0;i<(Index)(model.njoints);++i)
jcarpent's avatar
jcarpent committed
50
    {
51
      os << "  Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i]  << std::endl;
jcarpent's avatar
jcarpent committed
52
53
    }
    
54
55
    return os;
  }
56
  
57
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
jcarpent's avatar
jcarpent committed
58
  template<typename JointModelDerived>
59
60
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
61
62
63
64
65
66
67
68
                                                        const JointModelBase<JointModelDerived> & 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
                                                        )
69
  {
70
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
71
           &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
72
73
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME((joint_model.nq()>=0) && (joint_model.nv()>=0));
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(joint_model.nq() >= joint_model.nv());
74

75
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(max_effort.size() == joint_model.nv()
jcarpent's avatar
jcarpent committed
76
77
78
           && max_velocity.size() == joint_model.nv()
           && min_config.size() == joint_model.nq()
           && max_config.size() == joint_model.nq());
79
    
80
    JointIndex idx = (JointIndex)(njoints++);
81
    
jcarpent's avatar
jcarpent committed
82
    joints         .push_back(JointModel(joint_model.derived()));
83
84
    JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
    jmodel.setIndexes(idx,nq,nv);
jcarpent's avatar
jcarpent committed
85
    
86
87
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(jmodel.idx_q() >= 0);
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(jmodel.idx_v() >= 0);
Justin Carpentier's avatar
Justin Carpentier committed
88

89
    inertias       .push_back(Inertia::Zero());
90
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
91
    jointPlacements.push_back(joint_placement);
92
    names          .push_back(joint_name);
jcarpent's avatar
jcarpent committed
93
94
    nq += joint_model.nq();
    nv += joint_model.nv();
95

96
97
98
99
100
101
102
103
104
105
106
107
    if(joint_model.nq() > 0 && joint_model.nv() > 0)
    {
      effortLimit.conservativeResize(nv);
      jmodel.jointVelocitySelector(effortLimit) = max_effort;
      velocityLimit.conservativeResize(nv);
      jmodel.jointVelocitySelector(velocityLimit) = max_velocity;
      lowerPositionLimit.conservativeResize(nq);
      jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
      upperPositionLimit.conservativeResize(nq);
      jmodel.jointConfigSelector(upperPositionLimit) = max_config;
      
      /// TODO: remove this pragma when neutralConfiguration will be removed
108
109
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
110
111
112
      neutralConfiguration.conservativeResize(nq);
      typedef NeutralStep<LieGroupMap,ConfigVectorType> NeutralVisitor;
      NeutralStepAlgo<NeutralVisitor,JointModelDerived>::run(jmodel,neutralConfiguration);
113
#pragma GCC diagnostic pop
114
115
116
117
118
119
      
      rotorInertia.conservativeResize(nv);
      jmodel.jointVelocitySelector(rotorInertia).setZero();
      rotorGearRatio.conservativeResize(nv);
      jmodel.jointVelocitySelector(rotorGearRatio).setZero();
    }
jcarpent's avatar
jcarpent committed
120
    
jcarpent's avatar
jcarpent committed
121
122
123
124
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
125
126
127
    return idx;
  }

128
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
129
  template<typename JointModelDerived>
130
131
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
132
133
134
                                                        const JointModelBase<JointModelDerived> & joint_model,
                                                        const SE3 & joint_placement,
                                                        const std::string & joint_name)
135
  {
136
    VectorXs max_effort, max_velocity, min_config, max_config;
137

138
139
140
141
    max_effort = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
    max_velocity = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
    min_config = VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
    max_config = VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());
142

143
    return addJoint(parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config, max_config);
144
145
  }
  
146
147
148
149
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline int ModelTpl<Scalar,Options,JointCollectionTpl>::
  addJointFrame(const JointIndex & jidx,
                int fidx)
150
  {
jcarpent's avatar
jcarpent committed
151
    if(fidx < 0) {
152
153
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
154
      fidx = (int)getFrameId(names[parents[jidx]], (FrameType)(JOINT | FIXED_JOINT));
155
    }
156
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE((size_t)fidx < frames.size(), "Frame index out of bound");
157
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
158
    return addFrame(Frame(names[jidx],jidx,(FrameIndex)fidx,SE3::Identity(),JOINT));
159
160
  }

161
162
163
164
165
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void ModelTpl<Scalar,Options,JointCollectionTpl>::
  appendBodyToJoint(const ModelTpl::JointIndex joint_index,
                    const Inertia & Y,
                    const SE3 & body_placement)
166
  {
jcarpent's avatar
jcarpent committed
167
168
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
169
    nbodies++;
170
  }
171

172
173
174
175
176
177
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline int ModelTpl<Scalar,Options,JointCollectionTpl>::
  addBodyFrame(const std::string & body_name,
               const JointIndex  & parentJoint,
               const SE3         & body_placement,
               int           previousFrame)
178
  {
jcarpent's avatar
jcarpent committed
179
    if(previousFrame < 0) {
180
181
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
182
      previousFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
183
    }
184
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE((size_t)previousFrame < frames.size(), "Frame index out of bound");
185
    return addFrame(Frame(body_name, parentJoint, (FrameIndex)previousFrame, body_placement, BODY));
186
  }
jcarpent's avatar
jcarpent committed
187
  
188
189
190
191
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getBodyId(const std::string & name) const
192
  {
193
    return getFrameId(name, BODY);
194
195
  }
  
196
197
198
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existBodyName(const std::string & name) const
199
  {
200
    return existFrame(name, BODY);
201
202
  }

203
204
205
206
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename  ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getJointId(const std::string & name) const
207
  {
jcarpent's avatar
jcarpent committed
208
209
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
210
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE((res<INT_MAX), "Id superior to int range. Should never happen.");
211
    return ModelTpl::JointIndex(res);
212
213
  }
  
214
215
216
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existJointName(const std::string & name) const
217
218
219
220
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

221
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
222
  inline const std::string &
223
224
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getJointName(const JointIndex index) const
225
  {
226
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( index < (ModelTpl::JointIndex)joints.size() );
227
228
229
    return names[index];
  }

230
231
232
233
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getFrameId(const std::string & name, const FrameType & type) const
234
  {
235
236
237
238
    typename container::aligned_vector<Frame>::const_iterator it
    = std::find_if(frames.begin()
                   ,frames.end()
                   ,details::FilterFrame(name, type));
239
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(it != frames.end(), "Frame not found");
240
    PINOCCHIO_ASSERT_THROW_AT_RUNTIME((std::find_if( boost::next(it), frames.end(), details::FilterFrame(name, type)) == frames.end())
241
        && "Several frames match the filter");
242
    return FrameIndex(it - frames.begin());
243
  }
244
  
245
246
247
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existFrame(const std::string & name, const FrameType & type) const
248
  {
249
250
    return std::find_if(frames.begin(), frames.end(),
                        details::FilterFrame(name, type)) != frames.end();
251
252
  }

253
254
255
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline int ModelTpl<Scalar,Options,JointCollectionTpl>::
  addFrame(const Frame & frame)
256
  {
257
    if(!existFrame(frame.name, frame.type))
258
    {
259
      frames.push_back(frame);
260
261
      nframes++;
      return nframes - 1;
262
263
264
    }
    else
    {
265
      return -1;
266
    }
267
  }
jcarpent's avatar
jcarpent committed
268
  
269
270
271
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void ModelTpl<Scalar,Options,JointCollectionTpl>::
  addJointIndexToParentSubtrees(const JointIndex joint_id)
jcarpent's avatar
jcarpent committed
272
273
274
  {
    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
      subtrees[parent].push_back(joint_id);
275
276
277
    
    // Also add joint_id to the universe
    subtrees[0].push_back(joint_id);
jcarpent's avatar
jcarpent committed
278
  }
279

280
} // namespace pinocchio
281

jcarpent's avatar
jcarpent committed
282
283
/// @endcond

284
#endif // ifndef __pinocchio_model_hxx__