ORCA: Optimization-based framework for Robotic Control Applications
GazeboModel.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 #pragma once
38 
39 // Gazebo headers
40 #include <gazebo/gazebo.hh>
41 #include <gazebo/physics/physics.hh>
42 #include <gazebo/common/common.hh>
43 #include "orca/gazebo/Utils.h"
44 
45 namespace orca
46 {
47 namespace gazebo
48 {
49 
51 {
52 public:
53  GazeboModel(const std::string& name)
54  : GazeboModel()
55  {
56  load(name);
57  }
58 
59  GazeboModel(::gazebo::physics::ModelPtr model)
60  : GazeboModel()
61  {
62  load(model);
63  }
64 
65  const std::string& getName() const
66  {
67  assertModelLoaded();
68  return name_;
69  }
70 
71  const std::string& getBaseName()
72  {
73  assertModelLoaded();
74  return base_name_;
75  }
76 
77  void setJointMapping(const std::vector<std::string>& joint_names)
78  {
79  assertModelLoaded();
80 
81  if(joint_names.size() != actuated_joint_names_.size())
82  {
83  std::cerr << "[GazeboModel] Size of provided joint names does not match actual configuration"
84  << "\nCurrent config has " << actuated_joint_names_.size() << " actuated joints."
85  << "\nProvided config has " << joint_names.size() << " actuated joints."
86  << '\n';
87  return;
88  }
89 
90  joints_.clear();
91  for(auto joint_name : joint_names)
92  {
93  auto joint = model_->GetJoint(joint_name);
94  if(!joint)
95  {
96  //print();
97  throw std::runtime_error("Joint " + joint_name + " does not exists in model");
98  }
99  joints_.push_back(joint);
100  }
101 
102  this->actuated_joint_names_ = joint_names;
103  }
104 
105  bool setModelConfiguration(const std::vector<std::string>& joint_names,const std::vector<double>& joint_positions)
106  {
107  assertModelLoaded();
108 
109  if (joint_names.size() != joint_positions.size())
110  {
111  std::cerr << "[GazeboModel \'" << getName() << "\'] " << "joint_names lenght should be the same as joint_positions : " << joint_names.size() << " vs " << joint_positions.size() << '\n';
112  return false;
113  }
114 
115  auto world = ::gazebo::physics::get_world();
116  // make the service call to pause gazebo
117  bool is_paused = world->IsPaused();
118  if (!is_paused) world->SetPaused(true);
119 
120  std::map<std::string, double> joint_position_map;
121  for (unsigned int i = 0; i < joint_names.size(); i++)
122  {
123  joint_position_map[joint_names[i]] = joint_positions[i];
124  }
125 
126  model_->SetJointPositions(joint_position_map);
127 
128  // resume paused state before this call
129  world->SetPaused(is_paused);
130 
131  return true;
132  }
133 
134  bool load(const std::string& model_name)
135  {
136  if(!loadWorld())
137  return false;
138 
139  #if GAZEBO_MAJOR_VERSION > 8
140  auto model = world_->ModelByName( model_name );
141  #else
142  auto model = world_->GetModel( model_name );
143  #endif
144  if(! model)
145  {
146  std::cerr << "[GazeboModel \'" << getName() << "\'] " << "Could not get gazebo model " << model_name << ". Make sure it is loaded in the server" << '\n';
147  return false;
148  }
149  return load(model);
150  }
151 
152  bool load(::gazebo::physics::ModelPtr model)
153  {
154  if(!model)
155  {
156  std::cerr << "Model is invalid" << '\n';
157  return false;
158  }
159  if(!loadWorld())
160  return false;
161 
162  joints_.clear();
163  actuated_joint_names_.clear();
164  for(auto joint : model->GetJoints() )
165  {
166  // Provide feedback to get the internal torque
167  joint->SetProvideFeedback(true);
168  // Not adding fixed joints
169  bool added = false;
170  if( true
171  #if GAZEBO_MAJOR_VERSION >= 7
172  && joint->GetType() != ::gazebo::physics::Joint::FIXED_JOINT
173  #endif
174  #if GAZEBO_MAJOR_VERSION > 8
175  && joint->LowerLimit(0u) != joint->UpperLimit(0u)
176  && joint->LowerLimit(0u) != 0
177  && !std::isnan(joint->LowerLimit(0u))
178  && !std::isnan(joint->UpperLimit(0u))
179  #else
180  && joint->GetLowerLimit(0u) != joint->GetUpperLimit(0u)
181  && joint->GetLowerLimit(0u).Radian() != 0
182  && !std::isnan(joint->GetLowerLimit(0u).Radian())
183  && !std::isnan(joint->GetUpperLimit(0u).Radian())
184  #endif
185  )
186  {
187  joints_.push_back(joint);
188  actuated_joint_names_.push_back(joint->GetName());
189  added = true;
190  }
191 
192  std::cout << "[GazeboModel \'" << model->GetName() << "\'] " << (added ? "Adding":"Not adding")
193  << (added ? "" : " fixed/invalid") << " joint " << joint->GetName()
194  << " type (" << joint->GetType() << ")"
195  #if GAZEBO_MAJOR_VERSION > 8
196  << " lower limit " << joint->LowerLimit(0u)
197  << " upper limit " << joint->UpperLimit(0u)
198  #else
199  << " lower limit " << joint->GetLowerLimit(0u)
200  << " upper limit " << joint->GetUpperLimit(0u)
201  #endif
202  << '\n';
203  }
204 
205  if(actuated_joint_names_.size() == 0)
206  {
207  std::cerr << "[GazeboModel \'" << model->GetName() << "\'] " << "Could not get any actuated joint for model " << model->GetName() << '\n';
208  return false;
209  }
210 
211 
212  model_ = model;
213 
214  links_ = model->GetLinks();
215  name_ = model->GetName();
216 
217  ndof_ = actuated_joint_names_.size();
218  current_joint_positions_.setZero(ndof_);
219  joint_gravity_torques_.setZero(ndof_);
220  current_joint_velocities_.setZero(ndof_);
221  current_joint_external_torques_.setZero(ndof_);
222  joint_torque_command_.setZero(ndof_);
223  current_joint_measured_torques_.setZero(ndof_);
224 
225  print();
226 
227  return true;
228  }
229 
230  ::gazebo::sensors::ForceTorqueSensorPtr attachForceTorqueSensorToJoint(const std::string& joint_name,double update_rate = 0)
231  {
232  assertModelLoaded();
233 
234  #if GAZEBO_MAJOR_VERSION > 8
235  auto joint = model_->GetJoint(joint_name);
236  if(!joint)
237  throw std::runtime_error("Joint " + joint_name + " does not exists");
238  std::stringstream ss;
239  ss << "<sdf version='1.4'>";
240  ss << " <sensor name='force_torque' type='force_torque'>";
241  ss << " <update_rate>" << update_rate << "</update_rate>";
242  ss << " <always_on>true</always_on>";
243  ss << " <visualize>true</visualize>";
244  ss << " </sensor>";
245  ss << "</sdf>";
246  std::string forceTorqueSensorString = ss.str();
247  // Create the SDF file to configure the sensor
248  sdf::ElementPtr sdf(new sdf::Element);
249  sdf::initFile("sensor.sdf", sdf);
250  sdf::readString(forceTorqueSensorString, sdf);
251  // Create the force torque sensor
252  auto mgr = ::gazebo::sensors::SensorManager::Instance();
253  std::string sensorName = mgr->CreateSensor(sdf, "default",
254  getName() + "::" + joint_name, joint->GetId());
255 
256  // Make sure the returned sensor name is correct
257  auto excepted_name = std::string("default::" + getName() + "::" + joint_name + "::force_torque");
258  if(sensorName != excepted_name)
259  throw std::runtime_error("Returned sensor name is " + sensorName + " when it should be " + excepted_name);
260 
261  // Update the sensor manager so that it can process new sensors.
262  mgr->Update();
263 
264  // Get a pointer to the force torque sensor
265  auto sensor = std::dynamic_pointer_cast<::gazebo::sensors::ForceTorqueSensor>(
266  mgr->GetSensor(sensorName));
267 
268  // Make sure the above dynamic cast worked.
269  if(!sensor) throw std::runtime_error("Could not create force_torque sensor for joint " + joint_name);
270 
271  if(!sensor->IsActive()) throw std::runtime_error("Sensor is not active");
272 
273  std::cout << "[GazeboModel \'" << getName() << "\'] " << "Force torque sensor '" << sensorName << "' successfully created" << '\n';
274  return sensor;
275 #else
276  throw std::runtime_error("Adding sensors is only supported for gz version > 8");
277 #endif
278  }
279 
280  ::gazebo::sensors::ContactSensorPtr attachContactSensorToLink(const std::string& link_name,double update_rate = 0)
281  {
282  assertModelLoaded();
283 
284  #if GAZEBO_MAJOR_VERSION > 8
285  auto link = model_->GetLink(link_name);
286  if(!link)
287  throw std::runtime_error("Link " + link_name + " does not exists");
288  std::stringstream ss;
289  ss << "<sdf version='1.4'>";
290  ss << " <sensor name='contact' type='contact'>";
291  ss << " <update_rate>" << update_rate << "</update_rate>";
292  ss << " <always_on>true</always_on>";
293  ss << " <visualize>true</visualize>";
294  ss << " </sensor>";
295  ss << "</sdf>";
296  std::string contactSensorStr = ss.str();
297  // Create the SDF file to configure the sensor
298  sdf::ElementPtr sdf(new sdf::Element);
299  sdf::initFile("sensor.sdf", sdf);
300  sdf::readString(contactSensorStr, sdf);
301  // Create the force torque sensor
302  auto mgr = ::gazebo::sensors::SensorManager::Instance();
303  std::string sensorName = mgr->CreateSensor(sdf, "default",
304  getName() + "::" + link_name, link->GetId());
305 
306  // Make sure the returned sensor name is correct
307  auto excepted_name = std::string("default::" + getName() + "::" + link_name + "::contact");
308  if(sensorName != excepted_name)
309  throw std::runtime_error("Returned sensor name is " + sensorName + " when it should be " + excepted_name);
310 
311  // Update the sensor manager so that it can process new sensors.
312  mgr->Update();
313 
314  // Get a pointer to the force torque sensor
315  auto sensor = std::dynamic_pointer_cast<::gazebo::sensors::ContactSensor>(
316  mgr->GetSensor(sensorName));
317 
318  // Make sure the above dynamic cast worked.
319  if(!sensor) throw std::runtime_error("Could not create contact sensor for link " + link_name);
320 
321  if(!sensor->IsActive()) throw std::runtime_error("Sensor is not active");
322 
323  std::cout << "[GazeboModel \'" << getName() << "\'] " << "Contact sensor '" << sensorName << "' successfully created" << '\n';
324  return sensor;
325 #else
326  throw std::runtime_error("Adding sensors is only supported for gz version > 8");
327 #endif
328  }
329 
330  const Eigen::Vector3d& getGravity() const
331  {
332  assertModelLoaded();
333  return gravity_vector_;
334  }
335 
336  const std::vector<std::string>& getActuatedJointNames() const
337  {
338  assertModelLoaded();
339  return actuated_joint_names_;
340  }
341 
342  const Eigen::Matrix<double,6,1>& getBaseVelocity() const
343  {
344  assertModelLoaded();
345  return current_base_vel_;
346  }
347 
348  const Eigen::Affine3d& getWorldToBaseTransform() const
349  {
350  assertModelLoaded();
351  return current_world_to_base_;
352  }
353 
354  const Eigen::VectorXd& getJointPositions() const
355  {
356  assertModelLoaded();
357  return current_joint_positions_;
358  }
359 
360  const Eigen::VectorXd& getJointVelocities() const
361  {
362  assertModelLoaded();
363  return current_joint_velocities_;
364  }
365 
366  void setJointGravityTorques(const Eigen::VectorXd& gravity_torques)
367  {
368  assertModelLoaded();
369  if (gravity_torques.size() != joint_gravity_torques_.size())
370  throw std::runtime_error("Provided gravity torques do not match the model's dofs");
371  joint_gravity_torques_ = gravity_torques;
372  }
373 
374  const Eigen::VectorXd& getJointExternalTorques() const
375  {
376  assertModelLoaded();
377  return current_joint_external_torques_;
378  }
379 
380  const Eigen::VectorXd& getJointMeasuredTorques() const
381  {
382  assertModelLoaded();
383  return current_joint_measured_torques_;
384  }
385 
386  void setJointTorqueCommand(const Eigen::VectorXd& joint_torque_command)
387  {
388  assertModelLoaded();
389  joint_torque_command_ = joint_torque_command;
390  brakes_ = false;
391  }
392 
393  int getNDof() const
394  {
395  assertModelLoaded();
396  return ndof_;
397  }
398 
399  void printState() const
400  {
401  if(!model_)
402  {
403  std::cout << "[GazeboModel] Model is not loaded." << '\n';
404  return;
405  }
406 
407  std::cout << "[GazeboModel \'" << getName() << "\'] State :" << '\n';
408  std::cout << "- Gravity " << getGravity().transpose() << '\n';
409  std::cout << "- Base velocity\n" << getBaseVelocity().transpose() << '\n';
410  std::cout << "- Tworld->base\n" << getWorldToBaseTransform().matrix() << '\n';
411  std::cout << "- Joint positions " << getJointPositions().transpose() << '\n';
412  std::cout << "- Joint velocities " << getJointVelocities().transpose() << '\n';
413  std::cout << "- Joint external torques " << getJointExternalTorques().transpose() << '\n';
414  std::cout << "- Joint measured torques " << getJointMeasuredTorques().transpose() << '\n';
415  std::cout << "- Brakes " << (brakes_ ? "Enabled" : "Disabled") << '\n';
416  }
417 
418  void executeAfterWorldUpdate(std::function<void(uint32_t,double,double)> callback)
419  {
420  callback_ = callback;
421  }
422 
423  void setBrakes(bool enable)
424  {
425  brakes_ = enable;
426  }
427 
428  double getIterations()
429  {
430  #if GAZEBO_MAJOR_VERSION > 8
431  return world_->Iterations();
432  #else
433  return world_->GetIterations();
434  #endif
435  }
436  void print() const
437  {
438  if(!model_)
439  {
440  std::cout << "[GazeboModel] Model is not loaded." << '\n';
441  return;
442  }
443 
444  std::cerr << "[GazeboModel::" << getName() << "]" << '\n';
445 
446  std::cout << " Joints (" << joints_.size() << ")" << '\n';
447  for(unsigned int i=0; i < joints_.size() ; ++i)
448  std::cout << " Joint " << i << ": '" << joints_[i]->GetName() << "'" << '\n';
449 
450  std::cout << " Actuated joints (" << actuated_joint_names_.size() << ")" << '\n';
451  for(unsigned int i=0; i < actuated_joint_names_.size() ; i++)
452  std::cout << " Actuated joint " << i << ": '" << actuated_joint_names_[i] << "'" << '\n';
453 
454  std::cout << " Links (" << links_.size() << ")" << '\n';
455  for(unsigned int i=0; i < links_.size() ; i++)
456  std::cout << " Link " << i << ": '" << links_[i]->GetName() << "'" << '\n';
457 
458  printState();
459  }
460 protected:
462  {
463  #if GAZEBO_MAJOR_VERSION > 8
464  auto g = world_->Gravity();
465  #else
466  auto g = world_->GetPhysicsEngine()->GetGravity();
467  #endif
468  gravity_vector_[0] = g[0];
469  gravity_vector_[1] = g[1];
470  gravity_vector_[2] = g[2];
471 
472  model_->SetEnabled(!brakes_); // Enable the robot when brakes are off
473 
474  if(getIterations() > 0 && !brakes_)
475  {
476  for(int i=0 ; i < ndof_ ; ++i)
477  joints_[i]->SetForce(0,joint_torque_command_[i] + joint_gravity_torques_[i]);
478  }
479  else
480  {
481  for(int i=0 ; i < ndof_ ; ++i)
482  joints_[i]->SetVelocity(0, 0.0);
483  }
484  }
486  {
487  for(int i=0 ; i < ndof_ ; ++i)
488  {
489  auto joint = joints_[i];
490  #if GAZEBO_MAJOR_VERSION > 8
491  current_joint_positions_[i] = joint->Position(0);
492  #else
493  current_joint_positions_[i] = joint->GetAngle(0).Radian();
494  #endif
495  current_joint_velocities_[i] = joint->GetVelocity(0);
496  current_joint_external_torques_[i] = joint->GetForce(0); // WARNING: This is the external estimated force (= force applied to the joint = torque command from user)
497 
498  auto w = joint->GetForceTorque(0u);
499  #if GAZEBO_MAJOR_VERSION > 8
500  auto a = joint->LocalAxis(0u);
501  #else
502  auto a = joint->GetLocalAxis(0u);
503  #endif
504  current_joint_measured_torques_[i] = a.Dot(w.body1Torque);
505  }
506 
507  #if GAZEBO_MAJOR_VERSION > 8
508  current_world_to_base_ = convPose(model_->RelativePose());
509 
510  current_base_vel_.head(3) = convVec3(model_->RelativeLinearVel());
511  current_base_vel_.tail(3) = convVec3(model_->RelativeAngularVel());
512  #else
513  auto pose = model_->GetRelativePose();
514  current_world_to_base_.translation() = Eigen::Vector3d(pose.pos.x,pose.pos.y,pose.pos.z);
515  current_world_to_base_.linear() = Eigen::Quaterniond(pose.rot.w,pose.rot.x,pose.rot.y,pose.rot.z).toRotationMatrix();
516 
517  auto base_vel_lin = model_->GetRelativeLinearVel();
518  auto base_vel_ang = model_->GetRelativeAngularVel();
519  current_base_vel_[0] = base_vel_lin.x;
520  current_base_vel_[1] = base_vel_lin.y;
521  current_base_vel_[2] = base_vel_lin.z;
522  current_base_vel_[3] = base_vel_ang.x;
523  current_base_vel_[4] = base_vel_ang.y;
524  current_base_vel_[5] = base_vel_ang.z;
525  #endif
526  if(callback_)
527  {
528  #if GAZEBO_MAJOR_VERSION > 8
529  double sim_time = world_->SimTime().Double();
530  #else
531  double sim_time = world_->GetSimTime().Double();
532  #endif
533  #if GAZEBO_MAJOR_VERSION > 8
534  double dt = world_->Physics()->GetMaxStepSize();
535  #else
536  double dt = world_->GetPhysicsEngine()->GetMaxStepSize();
537  #endif
538  callback_(getIterations(),sim_time,dt);
539  }
540  }
541 private:
542  void assertModelLoaded() const
543  {
544  if (!model_)
545  throw std::runtime_error("[GazeboModel] Model is not loaded");
546  }
547  GazeboModel()
548  {
549  current_base_vel_.setZero();
550  current_world_to_base_.setIdentity();
551  gravity_vector_.setZero();
552  }
553  bool loadWorld()
554  {
555  auto world = ::gazebo::physics::get_world();
556  if(!world)
557  {
558  std::cerr << "[GazeboModel::" << getName() << "] " << "Could not load gazebo world" << '\n';
559  return false;
560  }
561  world_ = world;
562  world_begin_ = ::gazebo::event::Events::ConnectWorldUpdateBegin(std::bind(&GazeboModel::worldUpdateBegin,this));
563  world_end_ = ::gazebo::event::Events::ConnectWorldUpdateEnd(std::bind(&GazeboModel::worldUpdateEnd,this));
564  return true;
565  }
566 
567  ::gazebo::physics::WorldPtr world_;
568  ::gazebo::physics::ModelPtr model_;
569  ::gazebo::physics::Joint_V joints_;
570  ::gazebo::physics::Link_V links_;
571  std::vector<std::string> actuated_joint_names_;
572  Eigen::Matrix<double,6,1> current_base_vel_;
573  Eigen::Affine3d current_world_to_base_;
574  Eigen::VectorXd current_joint_positions_;
575  Eigen::VectorXd joint_gravity_torques_;
576  Eigen::VectorXd current_joint_velocities_;
577  Eigen::VectorXd current_joint_external_torques_;
578  Eigen::VectorXd current_joint_measured_torques_;
579  Eigen::VectorXd joint_torque_command_;
580  std::string name_;
581  std::string base_name_;
582  Eigen::Vector3d gravity_vector_;
583  ::gazebo::event::ConnectionPtr world_begin_;
584  ::gazebo::event::ConnectionPtr world_end_;
585  int ndof_ = 0;
586  bool brakes_ = true;
587  std::function<void(uint32_t,double,double)> callback_;
588 };
589 
590 } // namespace gazebo
591 } // namespace orca
bool load(const std::string &model_name)
Definition: GazeboModel.h:134
const Eigen::Vector3d & getGravity() const
Definition: GazeboModel.h:330
::gazebo::sensors::ContactSensorPtr attachContactSensorToLink(const std::string &link_name, double update_rate=0)
Definition: GazeboModel.h:280
Definition: GazeboServer.h:59
void worldUpdateBegin()
Definition: GazeboModel.h:461
const Eigen::VectorXd & getJointPositions() const
Definition: GazeboModel.h:354
bool load(::gazebo::physics::ModelPtr model)
Definition: GazeboModel.h:152
const Eigen::Matrix< double, 6, 1 > & getBaseVelocity() const
Definition: GazeboModel.h:342
void print() const
Definition: GazeboModel.h:436
double getIterations()
Definition: GazeboModel.h:428
const std::string & getName() const
Definition: GazeboModel.h:65
const Eigen::VectorXd & getJointVelocities() const
Definition: GazeboModel.h:360
Definition: GazeboModel.h:50
void setJointMapping(const std::vector< std::string > &joint_names)
Definition: GazeboModel.h:77
void printState() const
Definition: GazeboModel.h:399
void setBrakes(bool enable)
Definition: GazeboModel.h:423
void setJointTorqueCommand(const Eigen::VectorXd &joint_torque_command)
Definition: GazeboModel.h:386
int getNDof() const
Definition: GazeboModel.h:393
GazeboModel(const std::string &name)
Definition: GazeboModel.h:53
const Eigen::VectorXd & getJointMeasuredTorques() const
Definition: GazeboModel.h:380
const std::string & getBaseName()
Definition: GazeboModel.h:71
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:64
::gazebo::sensors::ForceTorqueSensorPtr attachForceTorqueSensorToJoint(const std::string &joint_name, double update_rate=0)
Definition: GazeboModel.h:230
Definition: CartesianAccelerationPID.h:44
void setJointGravityTorques(const Eigen::VectorXd &gravity_torques)
Definition: GazeboModel.h:366
bool setModelConfiguration(const std::vector< std::string > &joint_names, const std::vector< double > &joint_positions)
Definition: GazeboModel.h:105
const Eigen::Affine3d & getWorldToBaseTransform() const
Definition: GazeboModel.h:348
void worldUpdateEnd()
Definition: GazeboModel.h:485
GazeboModel(::gazebo::physics::ModelPtr model)
Definition: GazeboModel.h:59
void executeAfterWorldUpdate(std::function< void(uint32_t, double, double)> callback)
Definition: GazeboModel.h:418
const Eigen::VectorXd & getJointExternalTorques() const
Definition: GazeboModel.h:374
Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
Definition: Utils.h:84
const std::vector< std::string > & getActuatedJointNames() const
Definition: GazeboModel.h:336