model.hxx 9.35 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/tools/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
35
36
37
  namespace details
  {
    struct FilterFrame {
      const std::string& name;
      const FrameType & typeMask;
jcarpent's avatar
jcarpent committed
38
39
40
      FilterFrame(const std::string& name, const FrameType& typeMask)
        : name(name), typeMask(typeMask) {}
      bool operator()(const Frame& frame) const
41
42
43
44
      { return (typeMask & frame.type) && (name == frame.name); }
    };
  }

jcarpent's avatar
jcarpent committed
45
  inline std::ostream& operator<<(std::ostream & os, const Model & model)
46
  {
47
48
    os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
    for(Model::Index i=0;i<(Model::Index)(model.njoints);++i)
jcarpent's avatar
jcarpent committed
49
50
51
52
    {
      os << "  Joint "<< model.names[i] << ": parent=" << model.parents[i]  << std::endl;
    }
    
53
54
    return os;
  }
55
  
jcarpent's avatar
jcarpent committed
56
57
58
59
  template<typename JointModelDerived>
  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                    const JointModelBase<JointModelDerived> & joint_model,
                                    const SE3 & joint_placement,
60
                                    const std::string & joint_name,
jcarpent's avatar
jcarpent committed
61
62
63
                                    const Eigen::VectorXd & max_effort,
                                    const Eigen::VectorXd & max_velocity,
                                    const Eigen::VectorXd & min_config,
64
                                    const Eigen::VectorXd & max_config
jcarpent's avatar
jcarpent committed
65
                                    )
66
  {
67
68
    assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
           &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
jcarpent's avatar
jcarpent committed
69
    assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
70

jcarpent's avatar
jcarpent committed
71
72
73
74
    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());
75
76
77
    
    Model::JointIndex idx = (Model::JointIndex) (njoints++);
    
jcarpent's avatar
jcarpent committed
78
    joints         .push_back(JointModel(joint_model.derived()));
79
80
    JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
    jmodel.setIndexes(idx,nq,nv);
jcarpent's avatar
jcarpent committed
81
    
82
    inertias       .push_back(Inertia::Zero());
83
    parents        .push_back(parent);
jcarpent's avatar
jcarpent committed
84
    jointPlacements.push_back(joint_placement);
85
    names          .push_back(joint_name);
jcarpent's avatar
jcarpent committed
86
87
    nq += joint_model.nq();
    nv += joint_model.nv();
88

89
90
91
    // 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?).
    // As efficiency of Model::addJoint is not critical, the dynamic bottomRows is used here.
92
    effortLimit.conservativeResize(nv);
93
    jmodel.jointVelocitySelector(effortLimit) = max_effort;
94
    velocityLimit.conservativeResize(nv);
95
    jmodel.jointVelocitySelector(velocityLimit) = max_velocity;
96
97
98
99
    lowerPositionLimit.conservativeResize(nq);
    jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
    upperPositionLimit.conservativeResize(nq);
    jmodel.jointConfigSelector(upperPositionLimit) = max_config;
100
    
101
    neutralConfiguration.conservativeResize(nq);
102
    NeutralStepAlgo<LieGroupTpl,JointModelDerived>::run(jmodel,neutralConfiguration);
103

104
    rotorInertia.conservativeResize(nv);
105
    jmodel.jointVelocitySelector(rotorInertia).setZero();
106
    rotorGearRatio.conservativeResize(nv);
107
    jmodel.jointVelocitySelector(rotorGearRatio).setZero();
jcarpent's avatar
jcarpent committed
108
    
jcarpent's avatar
jcarpent committed
109
110
111
112
    // Init and add joint index to its parent subtrees.
    subtrees.push_back(IndexVector(1));
    subtrees[idx][0] = idx;
    addJointIndexToParentSubtrees(idx);
113
114
115
    return idx;
  }

116
117
118
119
120
121
122
123
124
  template<typename JointModelDerived>
  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                    const JointModelBase<JointModelDerived> & joint_model,
                                    const SE3 & joint_placement,
                                    const std::string & joint_name
                                    )
  {
    Eigen::VectorXd max_effort, max_velocity, min_config, max_config;

125
126
    max_effort = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
    max_velocity = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
127
    min_config = Eigen::VectorXd::Constant(joint_model.nq(), -std::numeric_limits<double>::max());
128
129
    max_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());

130
    return addJoint(parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config, max_config);
131
132
  }
  
jcarpent's avatar
jcarpent committed
133
  inline int Model::addJointFrame(const JointIndex& jidx,
134
                                         int         fidx)
135
  {
jcarpent's avatar
jcarpent committed
136
    if(fidx < 0) {
137
138
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
139
      fidx = (int)getFrameId(names[parents[jidx]], (FrameType)(JOINT | FIXED_JOINT));
140
    }
141
    assert((size_t)fidx < frames.size() && "Frame index out of bound");
142
    // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
143
    return addFrame(Frame(names[jidx],jidx,(FrameIndex)fidx,SE3::Identity(),JOINT));
144
145
  }

146

jcarpent's avatar
jcarpent committed
147
148
  inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                       const Inertia & Y,
149
                                       const SE3 & body_placement)
150
  {
jcarpent's avatar
jcarpent committed
151
152
    const Inertia & iYf = Y.se3Action(body_placement);
    inertias[joint_index] += iYf;
153
    nbodies++;
154
  }
155

jcarpent's avatar
jcarpent committed
156
  inline int Model::addBodyFrame(const std::string & body_name,
157
158
159
                                  const JointIndex  & parentJoint,
                                  const SE3         & body_placement,
                                        int           previousFrame)
160
  {
jcarpent's avatar
jcarpent committed
161
    if(previousFrame < 0) {
162
163
      // FIXED_JOINT is required because the parent can be the universe and its
      // type is FIXED_JOINT
164
      previousFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
165
    }
166
167
    assert((size_t)previousFrame < frames.size() && "Frame index out of bound");
    return addFrame(Frame(body_name, parentJoint, (FrameIndex)previousFrame, body_placement, BODY));
168
  }
jcarpent's avatar
jcarpent committed
169
  
jcarpent's avatar
jcarpent committed
170
  inline Model::JointIndex Model::getBodyId(const std::string & name) const
171
  {
172
    return getFrameId(name, BODY);
173
174
  }
  
jcarpent's avatar
jcarpent committed
175
  inline bool Model::existBodyName(const std::string & name) const
176
  {
177
    return existFrame(name, BODY);
178
179
180
  }


jcarpent's avatar
jcarpent committed
181
  inline Model::JointIndex Model::getJointId(const std::string & name) const
182
  {
jcarpent's avatar
jcarpent committed
183
184
    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
185
    assert((res<INT_MAX) && "Id superior to int range. Should never happen.");
186
    return Model::JointIndex(res);
187
188
  }
  
jcarpent's avatar
jcarpent committed
189
  inline bool Model::existJointName(const std::string & name) const
190
191
192
193
  {
    return (names.end() != std::find(names.begin(),names.end(),name));
  }

jcarpent's avatar
jcarpent committed
194
  inline const std::string& Model::getJointName(const JointIndex index) const
195
  {
196
    assert( index < (Model::JointIndex)joints.size() );
197
198
199
    return names[index];
  }

jcarpent's avatar
jcarpent committed
200
  inline Model::FrameIndex Model::getFrameId( const std::string & name, const FrameType & type ) const
201
  {
202
    container::aligned_vector<Frame>::const_iterator it = std::find_if( frames.begin()
203
                                                        , frames.end()
jcarpent's avatar
jcarpent committed
204
                                                        , details::FilterFrame(name, type)
205
                                                        );
jcarpent's avatar
jcarpent committed
206
207
    assert(it != frames.end() && "Frame not found");
    assert((std::find_if( boost::next(it), frames.end(), details::FilterFrame(name, type)) == frames.end())
208
        && "Several frames match the filter");
209
    return Model::FrameIndex(it - frames.begin());
210
211
  }

jcarpent's avatar
jcarpent committed
212
  inline bool Model::existFrame( const std::string & name, const FrameType & type) const
213
  {
214
    return std::find_if( frames.begin(), frames.end(),
jcarpent's avatar
jcarpent committed
215
        details::FilterFrame(name, type)) != frames.end();
216
217
218
  }


jcarpent's avatar
jcarpent committed
219
  inline int Model::addFrame( const Frame & frame )
220
  {
221
    if( !existFrame(frame.name, frame.type) )
222
    {
223
      frames.push_back(frame);
224
225
      nframes++;
      return nframes - 1;
226
227
228
    }
    else
    {
229
      return -1;
230
    }
231
  }
jcarpent's avatar
jcarpent committed
232
233
234
235
236
237
  
  inline void Model::addJointIndexToParentSubtrees(const JointIndex joint_id)
  {
    for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent])
      subtrees[parent].push_back(joint_id);
  }
238

239
240
} // namespace se3

jcarpent's avatar
jcarpent committed
241
242
/// @endcond

243
#endif // ifndef __se3_model_hxx__