sot-talos-balance  1.6.0
robot-wrapper.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
19 
20 #include <pinocchio/multibody/model.hpp>
21 #include <pinocchio/parsers/urdf.hpp>
22 #include <pinocchio/algorithm/center-of-mass.hpp>
23 #include <pinocchio/algorithm/compute-all-terms.hpp>
24 #include <pinocchio/algorithm/jacobian.hpp>
25 #include <pinocchio/algorithm/frames.hpp>
26 
27 using namespace pinocchio;
29 
30 namespace dynamicgraph
31 {
32  namespace sot
33  {
34  namespace talos_balance
35  {
36  namespace robots
37  {
38 
39  RobotWrapper::RobotWrapper(const std::string & filename,
40  const std::vector<std::string> & ,
41  bool verbose)
42  : m_verbose(verbose)
43  {
44  pinocchio::urdf::buildModel(filename, m_model, m_verbose);
45  m_model_filename = filename;
46  m_rotor_inertias.setZero(m_model.nv);
47  m_gear_ratios.setZero(m_model.nv);
48  m_Md.setZero(m_model.nv);
49  m_M.setZero(m_model.nv, m_model.nv);
50  }
51 
52  RobotWrapper::RobotWrapper(const std::string & filename,
53  const std::vector<std::string> & ,
54  const pinocchio::JointModelVariant & rootJoint,
55  bool verbose)
56  : m_verbose(verbose)
57  {
58  pinocchio::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
59  m_model_filename = filename;
60  m_rotor_inertias.setZero(m_model.nv-6);
61  m_gear_ratios.setZero(m_model.nv-6);
62  m_Md.setZero(m_model.nv-6);
63  m_M.setZero(m_model.nv, m_model.nv);
64  }
65 
66  int RobotWrapper::nq() const { return m_model.nq; }
67  int RobotWrapper::nv() const { return m_model.nv; }
68 
69  const Model & RobotWrapper::model() const { return m_model; }
71 
72  void RobotWrapper::computeAllTerms(Data & data, const Vector & q, const Vector & v) const
73  {
74  pinocchio::computeAllTerms(m_model, data, q, v);
75  data.M.triangularView<Eigen::StrictlyLower>()
76  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
77  // computeAllTerms does not compute the com acceleration, so we need to call centerOfMass
78  pinocchio::centerOfMass(m_model, data, 2, false);
79  pinocchio::framesForwardKinematics(m_model, data);
80  pinocchio::centerOfMass(m_model, data, q, v, Eigen::VectorXd::Zero(nv()));
81  }
82 
84  {
85  return m_rotor_inertias;
86  }
88  {
89  return m_gear_ratios;
90  }
91 
93  {
94  assert(rotor_inertias.size()==m_rotor_inertias.size());
96  updateMd();
97  return true;
98  }
99 
101  {
102  assert(gear_ratios.size()==m_gear_ratios.size());
104  updateMd();
105  return true;
106  }
107 
109  {
110  m_Md = m_gear_ratios.cwiseProduct(m_gear_ratios.cwiseProduct(m_rotor_inertias));
111  }
112 
113  void RobotWrapper::com(const Data & data,
114  RefVector com_pos,
116  RefVector com_acc) const
117  {
118  com_pos = data.com[0];
119  com_vel = data.vcom[0];
120  com_acc = data.acom[0];
121  }
122 
123  const Vector3 & RobotWrapper::com(const Data & data) const
124  {
125  return data.com[0];
126  }
127 
128  const Vector3 & RobotWrapper::com_vel(const Data & data) const
129  {
130  return data.vcom[0];
131  }
132 
133  const Vector3 & RobotWrapper::com_acc(const Data & data) const
134  {
135  return data.acom[0];
136  }
137 
138  const Matrix3x & RobotWrapper::Jcom(const Data & data) const
139  {
140  return data.Jcom;
141  }
142 
143  const Matrix & RobotWrapper::mass(const Data & data)
144  {
145  m_M = data.M;
146  m_M.diagonal().tail(m_model.nv-6) += m_Md;
147  return m_M;
148  }
149 
150  const Vector & RobotWrapper::nonLinearEffects(const Data & data) const
151  {
152  return data.nle;
153  }
154 
155  const SE3 & RobotWrapper::position(const Data & data,
156  const Model::JointIndex index) const
157  {
158  return data.oMi[index];
159  }
160 
161  const Motion & RobotWrapper::velocity(const Data & data,
162  const Model::JointIndex index) const
163  {
164  return data.v[index];
165  }
166 
167  const Motion & RobotWrapper::acceleration(const Data & data,
168  const Model::JointIndex index) const
169  {
170  return data.a[index];
171  }
172 
174  const Model::JointIndex index,
175  Data::Matrix6x & J) const
176  {
177  return pinocchio::getJointJacobian(m_model, data, index, pinocchio::WORLD, J);
178  }
179 
181  const Model::JointIndex index,
182  Data::Matrix6x & J) const
183  {
184  return pinocchio::getJointJacobian(m_model, data, index, pinocchio::LOCAL, J);
185  }
186 
188  const Model::FrameIndex index) const
189  {
190  const Frame & f = m_model.frames[index];
191  return data.oMi[f.parent].act(f.placement);
192  }
193 
195  const Model::FrameIndex index,
196  SE3 & framePosition) const
197  {
198  const Frame & f = m_model.frames[index];
199  framePosition = data.oMi[f.parent].act(f.placement);
200  }
201 
203  const Model::FrameIndex index) const
204  {
205  return pinocchio::getFrameVelocity(m_model,data,index);
206  }
207 
209  const Model::FrameIndex index,
210  Motion & frameVelocity) const
211  {
212  frameVelocity = pinocchio::getFrameVelocity(m_model,data,index);
213  }
214 
216  const Model::FrameIndex index) const
217  {
218  return pinocchio::getFrameAcceleration(m_model,data,index);
219  }
220 
222  const Model::FrameIndex index,
223  Motion & frameAcceleration) const
224  {
225  frameAcceleration = pinocchio::getFrameAcceleration(m_model,data,index);
226  }
227 
229  const Model::FrameIndex index) const
230  {
231  Motion a = pinocchio::getFrameAcceleration(m_model,data,index);
232  Motion v = pinocchio::getFrameVelocity(m_model,data,index);
233  a.linear() += v.angular().cross(v.linear());
234  return a;
235  }
236 
238  const Model::FrameIndex index,
239  Motion & frameAcceleration) const
240  {
241  frameAcceleration = pinocchio::getFrameAcceleration(m_model,data,index);
242  Motion v = pinocchio::getFrameVelocity(m_model,data,index);
243  frameAcceleration.linear() += v.angular().cross(v.linear());
244  }
245 
247  const Model::FrameIndex index,
248  Data::Matrix6x & J) const
249  {
250  return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::WORLD, J);
251  }
252 
254  const Model::FrameIndex index,
255  Data::Matrix6x & J) const
256  {
257  return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::LOCAL, J);
258  }
259 
260  // const Vector3 & com(Data & data,const Vector & q,
261  // const bool computeSubtreeComs = true,
262  // const bool updateKinematics = true)
263  // {
264  // return pinocchio::centerOfMass(m_model, data, q, computeSubtreeComs, updateKinematics);
265  // }
266  // const Vector3 & com(Data & data, const Vector & q, const Vector & v,
267  // const bool computeSubtreeComs = true,
268  // const bool updateKinematics = true)
269  // {
270  // return pinocchio::centerOfMass(m_model, data, q, v, computeSubtreeComs, updateKinematics);
271  // }
272 
273  } // namespace robots
274  }
275  }
276 }
void frameJacobianWorld(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
void jacobianWorld(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
const Matrix3x & Jcom(const Data &data) const
void jacobianLocal(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
void com(const Data &data, RefVector com_pos, RefVector com_vel, RefVector com_acc) const
SE3 framePosition(const Data &data, const Model::FrameIndex index) const
const Vector3 & com_acc(const Data &data) const
const Vector & nonLinearEffects(const Data &data) const
Motion frameClassicAcceleration(const Data &data, const Model::FrameIndex index) const
RobotWrapper(const std::string &filename, const std::vector< std::string > &package_dirs, bool verbose=false)
Motion frameVelocity(const Data &data, const Model::FrameIndex index) const
const Motion & velocity(const Data &data, const Model::JointIndex index) const
void computeAllTerms(Data &data, const Vector &q, const Vector &v) const
const Model & model() const
Accessor to model.
const SE3 & position(const Data &data, const Model::JointIndex index) const
Matrix m_M
diagonal part of inertia matrix due to rotor inertias
Motion frameAcceleration(const Data &data, const Model::FrameIndex index) const
const Motion & acceleration(const Data &data, const Model::JointIndex index) const
void frameJacobianLocal(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
const Vector3 & com_vel(const Data &data) const