model.hxx 11.8 KB
Newer Older
1
//
2
// Copyright (c) 2015-2018 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// 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/>.

#ifndef __se3_model_hxx__
#define __se3_model_hxx__

#include "pinocchio/spatial/fwd.hpp"
jcarpent's avatar
jcarpent committed
23
#include "pinocchio/utils/string-generator.hpp"
24
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
25

26
#include <boost/bind.hpp>
27
#include <boost/utility.hpp>
28

jcarpent's avatar
jcarpent committed
29
30
/// @cond DEV

31
32
namespace se3
{
33
34
  namespace details
  {
35
36
37
    struct FilterFrame
    {
      const std::string & name;
38
      const FrameType & typeMask;
39
40
41
42
43
44
45
      
      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
46
      { return (typeMask & frame.type) && (name == frame.name); }
47
      
48
49
    };
  }
50
51
52
53
  
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  const typename ModelTpl<Scalar,Options,JointCollectionTpl>::
  Vector3 ModelTpl<Scalar,Options,JointCollectionTpl>::gravity981(0,0,-9.81);
54

55
56
57
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream& operator<<(std::ostream & os,
                                  const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
58
  {
59
    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::Index Index;
60
    
61
    os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
62
    for(Index i=0;i<(Index)(model.njoints);++i)
jcarpent's avatar
jcarpent committed
63
64
65
66
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
67
68
    return os;
  }
69
  
70
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
jcarpent's avatar
jcarpent committed
71
  template<typename JointModelDerived>
72
73
74
75
76
77
78
79
80
81
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
                                                     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
                                                     )
82
  {
83
84
    assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
           &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
jcarpent's avatar
jcarpent committed
85
    assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
86

jcarpent's avatar
jcarpent committed
87
88
89
90
    assert(max_effort.size() == joint_model.nv()
           && max_velocity.size() == joint_model.nv()
           && min_config.size() == joint_model.nq()
           && max_config.size() == joint_model.nq());
91
    
92
    JointIndex idx = (JointIndex)(njoints++);
93
    
jcarpent's avatar
jcarpent committed
94
    joints         .push_back(JointModel(joint_model.derived()));
95
96
    JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
    jmodel.setIndexes(idx,nq,nv);
jcarpent's avatar
jcarpent committed
97
    
98
    inertias       .push_back(Inertia::Zero());
99
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
100
    jointPlacements.push_back(joint_placement);
101
    names          .push_back(joint_name);
jcarpent's avatar
jcarpent committed
102
103
    nq += joint_model.nq();
    nv += joint_model.nv();
104

105
106
    // Optimal efficiency here would be using the static-dim bottomRows, while specifying the dimension in argument in the case where D::NV is Eigen::Dynamic.
    // However, this option is not compiling in Travis (why?).
107
    // As efficiency of ModelTpl::addJoint is not critical, the dynamic bottomRows is used here.
108
    effortLimit.conservativeResize(nv);
109
    jmodel.jointVelocitySelector(effortLimit) = max_effort;
110
    velocityLimit.conservativeResize(nv);
111
    jmodel.jointVelocitySelector(velocityLimit) = max_velocity;
112
113
114
115
    lowerPositionLimit.conservativeResize(nq);
    jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
    upperPositionLimit.conservativeResize(nq);
    jmodel.jointConfigSelector(upperPositionLimit) = max_config;
116
    
117
    neutralConfiguration.conservativeResize(nq);
118
119
    typedef NeutralStep<LieGroupMap,ConfigVectorType> NeutralVisitor;
    NeutralStepAlgo<NeutralVisitor,JointModelDerived>::run(jmodel,neutralConfiguration);
120

121
    rotorInertia.conservativeResize(nv);
122
    jmodel.jointVelocitySelector(rotorInertia).setZero();
123
    rotorGearRatio.conservativeResize(nv);
124
    jmodel.jointVelocitySelector(rotorGearRatio).setZero();
jcarpent's avatar
jcarpent committed
125
    
jcarpent's avatar
jcarpent committed
126
127
128
129
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
130
131
132
    return idx;
  }

133
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
134
  template<typename JointModelDerived>
135
136
137
138
139
  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
                                                     const JointModelBase<JointModelDerived> & joint_model,
                                                     const SE3 & joint_placement,
                                                     const std::string & joint_name)
140
  {
141
    VectorXs max_effort, max_velocity, min_config, max_config;
142

143
144
145
146
    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());
147

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

166
167
168
169
170
  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)
171
  {
jcarpent's avatar
jcarpent committed
172
173
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
174
    nbodies++;
175
  }
176

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

208
209
210
211
  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
212
  {
jcarpent's avatar
jcarpent committed
213
214
    typedef std::vector<std::string>::iterator::difference_type it_diff_t;
    it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
jcarpent's avatar
jcarpent committed
215
    assert((res<INT_MAX) && "Id superior to int range. Should never happen.");
216
    return ModelTpl::JointIndex(res);
217
218
  }
  
219
220
221
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  existJointName(const std::string & name) const
222
223
224
225
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

226
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
227
  inline const std::string &
228
229
  ModelTpl<Scalar,Options,JointCollectionTpl>::
  getJointName(const JointIndex index) const
230
  {
231
    assert( index < (ModelTpl::JointIndex)joints.size() );
232
233
234
    return names[index];
  }

235
236
237
238
  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
239
  {
240
241
242
243
    typename container::aligned_vector<Frame>::const_iterator it
    = std::find_if(frames.begin()
                   ,frames.end()
                   ,details::FilterFrame(name, type));
jcarpent's avatar
jcarpent committed
244
245
    assert(it != frames.end() && "Frame not found");
    assert((std::find_if( boost::next(it), frames.end(), details::FilterFrame(name, type)) == frames.end())
246
        && "Several frames match the filter");
247
    return FrameIndex(it - frames.begin());
248
  }
249
  
250
251
252
  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
253
  {
254
255
    return std::find_if(frames.begin(), frames.end(),
                        details::FilterFrame(name, type)) != frames.end();
256
257
  }

258
259
260
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline int ModelTpl<Scalar,Options,JointCollectionTpl>::
  addFrame(const Frame & frame)
261
  {
262
    if(!existFrame(frame.name, frame.type))
263
    {
264
      frames.push_back(frame);
265
266
      nframes++;
      return nframes - 1;
267
268
269
    }
    else
    {
270
      return -1;
271
    }
272
  }
jcarpent's avatar
jcarpent committed
273
  
274
275
276
  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
277
278
279
280
  {
    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
      subtrees[parent].push_back(joint_id);
  }
281

282
283
} // namespace se3

jcarpent's avatar
jcarpent committed
284
285
/// @endcond

286
#endif // ifndef __se3_model_hxx__