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 
231  std::shared_ptr<orca::gazebo::GazeboForceTorqueSensor> attachForceTorqueSensorToJoint(const std::string& joint_name,double update_rate = 0)
232  {
233  assertModelLoaded();
234 
235  #if GAZEBO_MAJOR_VERSION > 8
236  auto joint = model_->GetJoint(joint_name);
237  if(!joint)
238  throw std::runtime_error("Joint " + joint_name + " does not exists");
239  std::stringstream ss;
240  ss << "<sdf version='1.4'>";
241  ss << " <sensor name='force_torque' type='force_torque'>";
242  ss << " <update_rate>" << update_rate << "</update_rate>";
243  ss << " <always_on>true</always_on>";
244  ss << " <visualize>true</visualize>";
245  ss << " <force_torque>";
246  ss << " <measure_direction>child_to_parent</measure_direction>";
247  ss << " <frame>child</frame>";
248  ss << " </force_torque>";
249  ss << " </sensor>";
250  ss << "</sdf>";
251  auto sensor_base_link = joint->GetChild(); // this is related to the <frame>child</frame> setting in the SDF. Forced.
252  auto sensor = std::make_shared<orca::gazebo::GazeboForceTorqueSensor>(sensor_base_link);
253 
254  std::string forceTorqueSensorString = ss.str();
255  // Create the SDF file to configure the sensor
256  sdf::ElementPtr sdf(new sdf::Element);
257  sdf::initFile("sensor.sdf", sdf);
258  sdf::readString(forceTorqueSensorString, sdf);
259  // Create the force torque sensor
260  auto mgr = ::gazebo::sensors::SensorManager::Instance();
261  std::string sensorName = mgr->CreateSensor(sdf, "default",
262  getName() + "::" + joint_name, joint->GetId());
263 
264  // Make sure the returned sensor name is correct
265  auto excepted_name = std::string("default::" + getName() + "::" + joint_name + "::force_torque");
266  if(sensorName != excepted_name)
267  throw std::runtime_error("Returned sensor name is " + sensorName + " when it should be " + excepted_name);
268 
269  // Update the sensor manager so that it can process new sensors.
270  mgr->Update();
271 
272  // Get a pointer to the force torque sensor
273  auto gz_sensor = std::dynamic_pointer_cast<::gazebo::sensors::ForceTorqueSensor>(
274  mgr->GetSensor(sensorName));
275 
276  // Make sure the above dynamic cast worked.
277  if(!gz_sensor) throw std::runtime_error("Could not create force_torque sensor for joint " + joint_name);
278 
279  if(!gz_sensor->IsActive()) throw std::runtime_error("Sensor is not active");
280 
281  sensor->setSensor(gz_sensor);
282 
283  std::cout << "[GazeboModel \'" << getName() << "\'] " << "Force torque sensor '" << sensorName << "' successfully created" << '\n';
284  return sensor;
285 #else
286  throw std::runtime_error("Adding sensors is only supported for gz version > 8");
287 #endif
288  }
289 
290  std::shared_ptr<orca::gazebo::GazeboContactSensor> attachContactSensorToLink(const std::string& link_name, const std::string& collision_name, double update_rate = 0)
291  {
292  assertModelLoaded();
293 
294  #if GAZEBO_MAJOR_VERSION > 8
295  auto link = model_->GetLink(link_name);
296  if (!link)
297  throw std::runtime_error("Link " + link_name + " does not exists");
298  auto collision = link->GetChildCollision(collision_name);
299  if (!collision)
300  {
301  std::string error = "Collision " + collision_name + " does not exist in link " + link_name + ".\nCandidates are:\n";
302  for (const auto& coll : link->GetCollisions())
303  error += " - " + coll->GetName() + "\n";
304  throw std::runtime_error(error);
305  }
306  std::stringstream ss;
307  ss << "<sdf version='1.4'>";
308  ss << " <sensor name='contact' type='contact'>";
309  ss << " <update_rate>" << update_rate << "</update_rate>";
310  ss << " <always_on>true</always_on>";
311  ss << " <visualize>true</visualize>";
312  ss << " <contact>";
313  ss << " <collision>" << collision_name << "</collision>";
314  ss << " </contact>";
315  ss << " </sensor>";
316  ss << "</sdf>";
317  auto sensor = std::make_shared<orca::gazebo::GazeboContactSensor>(link, collision_name);
318 
319  std::string contactSensorStr = ss.str();
320  // Create the SDF file to configure the sensor
321  sdf::ElementPtr sdf(new sdf::Element);
322  sdf::initFile("sensor.sdf", sdf);
323  sdf::readString(contactSensorStr, sdf);
324  // Create the force torque sensor
325  auto mgr = ::gazebo::sensors::SensorManager::Instance();
326  std::string sensorName = mgr->CreateSensor(sdf, "default",
327  getName() + "::" + link_name, link->GetId());
328 
329  // Make sure the returned sensor name is correct
330  auto excepted_name = std::string("default::" + getName() + "::" + link_name + "::contact");
331  if(sensorName != excepted_name)
332  throw std::runtime_error("Returned sensor name is " + sensorName + " when it should be " + excepted_name);
333 
334  // Update the sensor manager so that it can process new sensors.
335  mgr->Update();
336 
337  // Get a pointer to the force torque sensor
338  auto gz_sensor = std::dynamic_pointer_cast<::gazebo::sensors::ContactSensor>(
339  mgr->GetSensor(sensorName));
340 
341  // Make sure the above dynamic cast worked.
342  if(!gz_sensor) throw std::runtime_error("Could not create contact sensor for link " + link_name);
343 
344  if(!gz_sensor->IsActive()) throw std::runtime_error("Sensor is not active");
345 
346  sensor->setSensor(gz_sensor);
347 
348  std::cout << "[GazeboModel \'" << getName() << "\'] " << "Contact sensor '" << sensorName << "' successfully created" << '\n';
349  return sensor;
350 #else
351  throw std::runtime_error("Adding sensors is only supported for gz version > 8");
352 #endif
353  }
354 
355  const Eigen::Vector3d& getGravity() const
356  {
357  assertModelLoaded();
358  return gravity_vector_;
359  }
360 
361  const std::vector<std::string>& getActuatedJointNames() const
362  {
363  assertModelLoaded();
364  return actuated_joint_names_;
365  }
366 
367  const Eigen::Matrix<double,6,1>& getBaseVelocity() const
368  {
369  assertModelLoaded();
370  return current_base_vel_;
371  }
372 
373  const Eigen::Affine3d& getWorldToBaseTransform() const
374  {
375  assertModelLoaded();
376  return current_world_to_base_;
377  }
378 
379  const Eigen::VectorXd& getJointPositions() const
380  {
381  assertModelLoaded();
382  return current_joint_positions_;
383  }
384 
385  const Eigen::VectorXd& getJointVelocities() const
386  {
387  assertModelLoaded();
388  return current_joint_velocities_;
389  }
390 
391  void setJointGravityTorques(const Eigen::VectorXd& gravity_torques)
392  {
393  assertModelLoaded();
394  if (gravity_torques.size() != joint_gravity_torques_.size())
395  throw std::runtime_error("Provided gravity torques do not match the model's dofs");
396  joint_gravity_torques_ = gravity_torques;
397  }
398 
399  const Eigen::VectorXd& getJointExternalTorques() const
400  {
401  assertModelLoaded();
402  return current_joint_external_torques_;
403  }
404 
405  const Eigen::VectorXd& getJointMeasuredTorques() const
406  {
407  assertModelLoaded();
408  return current_joint_measured_torques_;
409  }
410 
411  void setJointTorqueCommand(const Eigen::VectorXd& joint_torque_command)
412  {
413  assertModelLoaded();
414  joint_torque_command_ = joint_torque_command;
415  brakes_ = false;
416  }
417 
418  int getNDof() const
419  {
420  assertModelLoaded();
421  return ndof_;
422  }
423 
424  void printState() const
425  {
426  if(!model_)
427  {
428  std::cout << "[GazeboModel] Model is not loaded." << '\n';
429  return;
430  }
431 
432  std::cout << "[GazeboModel \'" << getName() << "\'] State :" << '\n';
433  std::cout << "- Gravity " << getGravity().transpose() << '\n';
434  std::cout << "- Base velocity\n" << getBaseVelocity().transpose() << '\n';
435  std::cout << "- Tworld->base\n" << getWorldToBaseTransform().matrix() << '\n';
436  std::cout << "- Joint positions " << getJointPositions().transpose() << '\n';
437  std::cout << "- Joint velocities " << getJointVelocities().transpose() << '\n';
438  std::cout << "- Joint external torques " << getJointExternalTorques().transpose() << '\n';
439  std::cout << "- Joint measured torques " << getJointMeasuredTorques().transpose() << '\n';
440  std::cout << "- Brakes " << (brakes_ ? "Enabled" : "Disabled") << '\n';
441  }
442 
443  void executeAfterWorldUpdate(std::function<void(uint32_t,double,double)> callback)
444  {
445  callback_ = callback;
446  }
447 
448  void setBrakes(bool enable)
449  {
450  brakes_ = enable;
451  }
452 
453  double getIterations()
454  {
455  #if GAZEBO_MAJOR_VERSION > 8
456  return world_->Iterations();
457  #else
458  return world_->GetIterations();
459  #endif
460  }
461  void print() const
462  {
463  if(!model_)
464  {
465  std::cout << "[GazeboModel] Model is not loaded." << '\n';
466  return;
467  }
468 
469  std::cerr << "[GazeboModel::" << getName() << "]" << '\n';
470 
471  std::cout << " Joints (" << joints_.size() << ")" << '\n';
472  for(unsigned int i=0; i < joints_.size() ; ++i)
473  std::cout << " Joint " << i << ": '" << joints_[i]->GetName() << "'" << '\n';
474 
475  std::cout << " Actuated joints (" << actuated_joint_names_.size() << ")" << '\n';
476  for(unsigned int i=0; i < actuated_joint_names_.size() ; i++)
477  std::cout << " Actuated joint " << i << ": '" << actuated_joint_names_[i] << "'" << '\n';
478 
479  std::cout << " Links (" << links_.size() << ")" << '\n';
480  for(unsigned int i=0; i < links_.size() ; i++)
481  std::cout << " Link " << i << ": '" << links_[i]->GetName() << "'" << '\n';
482 
483  printState();
484  }
485 protected:
487  {
488  #if GAZEBO_MAJOR_VERSION > 8
489  auto g = world_->Gravity();
490  #else
491  auto g = world_->GetPhysicsEngine()->GetGravity();
492  #endif
493  gravity_vector_[0] = g[0];
494  gravity_vector_[1] = g[1];
495  gravity_vector_[2] = g[2];
496 
497  model_->SetEnabled(!brakes_); // Enable the robot when brakes are off
498 
499  if(getIterations() > 0 && !brakes_)
500  {
501  for(int i=0 ; i < ndof_ ; ++i)
502  joints_[i]->SetForce(0,joint_torque_command_[i] + joint_gravity_torques_[i]);
503  }
504  else
505  {
506  for(int i=0 ; i < ndof_ ; ++i)
507  joints_[i]->SetVelocity(0, 0.0);
508  }
509  }
511  {
512  for(int i=0 ; i < ndof_ ; ++i)
513  {
514  auto joint = joints_[i];
515  #if GAZEBO_MAJOR_VERSION > 8
516  current_joint_positions_[i] = joint->Position(0);
517  #else
518  current_joint_positions_[i] = joint->GetAngle(0).Radian();
519  #endif
520  current_joint_velocities_[i] = joint->GetVelocity(0);
521  current_joint_external_torques_[i] = joint->GetForce(0); // WARNING: This is the external estimated force (= force applied to the joint = torque command from user)
522 
523  auto w = joint->GetForceTorque(0u);
524  #if GAZEBO_MAJOR_VERSION > 8
525  auto a = joint->LocalAxis(0u);
526  #else
527  auto a = joint->GetLocalAxis(0u);
528  #endif
529  current_joint_measured_torques_[i] = a.Dot(w.body1Torque);
530  }
531 
532  #if GAZEBO_MAJOR_VERSION > 8
533  current_world_to_base_ = convPose(model_->RelativePose());
534 
535  current_base_vel_.head(3) = convVec3(model_->RelativeLinearVel());
536  current_base_vel_.tail(3) = convVec3(model_->RelativeAngularVel());
537  #else
538  auto pose = model_->GetRelativePose();
539  current_world_to_base_.translation() = Eigen::Vector3d(pose.pos.x,pose.pos.y,pose.pos.z);
540  current_world_to_base_.linear() = Eigen::Quaterniond(pose.rot.w,pose.rot.x,pose.rot.y,pose.rot.z).toRotationMatrix();
541 
542  auto base_vel_lin = model_->GetRelativeLinearVel();
543  auto base_vel_ang = model_->GetRelativeAngularVel();
544  current_base_vel_[0] = base_vel_lin.x;
545  current_base_vel_[1] = base_vel_lin.y;
546  current_base_vel_[2] = base_vel_lin.z;
547  current_base_vel_[3] = base_vel_ang.x;
548  current_base_vel_[4] = base_vel_ang.y;
549  current_base_vel_[5] = base_vel_ang.z;
550  #endif
551  if(callback_)
552  {
553  #if GAZEBO_MAJOR_VERSION > 8
554  double sim_time = world_->SimTime().Double();
555  #else
556  double sim_time = world_->GetSimTime().Double();
557  #endif
558  #if GAZEBO_MAJOR_VERSION > 8
559  double dt = world_->Physics()->GetMaxStepSize();
560  #else
561  double dt = world_->GetPhysicsEngine()->GetMaxStepSize();
562  #endif
563  callback_(getIterations(),sim_time,dt);
564  }
565  }
566 private:
567  void assertModelLoaded() const
568  {
569  if (!model_)
570  throw std::runtime_error("[GazeboModel] Model is not loaded");
571  }
572  GazeboModel()
573  {
574  current_base_vel_.setZero();
575  current_world_to_base_.setIdentity();
576  gravity_vector_.setZero();
577  }
578  bool loadWorld()
579  {
580  auto world = ::gazebo::physics::get_world();
581  if(!world)
582  {
583  std::cerr << "[GazeboModel::" << getName() << "] " << "Could not load gazebo world" << '\n';
584  return false;
585  }
586  world_ = world;
587  world_begin_ = ::gazebo::event::Events::ConnectWorldUpdateBegin(std::bind(&GazeboModel::worldUpdateBegin,this));
588  world_end_ = ::gazebo::event::Events::ConnectWorldUpdateEnd(std::bind(&GazeboModel::worldUpdateEnd,this));
589  return true;
590  }
591 
592  ::gazebo::physics::WorldPtr world_;
593  ::gazebo::physics::ModelPtr model_;
594  ::gazebo::physics::Joint_V joints_;
595  ::gazebo::physics::Link_V links_;
596  std::vector<std::string> actuated_joint_names_;
597  Eigen::Matrix<double,6,1> current_base_vel_;
598  Eigen::Affine3d current_world_to_base_;
599  Eigen::VectorXd current_joint_positions_;
600  Eigen::VectorXd joint_gravity_torques_;
601  Eigen::VectorXd current_joint_velocities_;
602  Eigen::VectorXd current_joint_external_torques_;
603  Eigen::VectorXd current_joint_measured_torques_;
604  Eigen::VectorXd joint_torque_command_;
605  std::string name_;
606  std::string base_name_;
607  Eigen::Vector3d gravity_vector_;
608  ::gazebo::event::ConnectionPtr world_begin_;
609  ::gazebo::event::ConnectionPtr world_end_;
610  int ndof_ = 0;
611  bool brakes_ = true;
612  std::function<void(uint32_t,double,double)> callback_;
613 };
614 
615 } // namespace gazebo
616 } // namespace orca
bool load(const std::string &model_name)
Definition: GazeboModel.h:134
const Eigen::VectorXd & getJointPositions() const
Definition: GazeboModel.h:379
Definition: GazeboServer.h:59
void worldUpdateBegin()
Definition: GazeboModel.h:486
bool load(::gazebo::physics::ModelPtr model)
Definition: GazeboModel.h:152
const std::vector< std::string > & getActuatedJointNames() const
Definition: GazeboModel.h:361
const Eigen::VectorXd & getJointVelocities() const
Definition: GazeboModel.h:385
double getIterations()
Definition: GazeboModel.h:453
const Eigen::VectorXd & getJointExternalTorques() const
Definition: GazeboModel.h:399
Definition: GazeboModel.h:50
void print() const
Definition: GazeboModel.h:461
void setJointMapping(const std::vector< std::string > &joint_names)
Definition: GazeboModel.h:77
std::shared_ptr< orca::gazebo::GazeboForceTorqueSensor > attachForceTorqueSensorToJoint(const std::string &joint_name, double update_rate=0)
Definition: GazeboModel.h:231
const Eigen::Vector3d & getGravity() const
Definition: GazeboModel.h:355
void setBrakes(bool enable)
Definition: GazeboModel.h:448
void setJointTorqueCommand(const Eigen::VectorXd &joint_torque_command)
Definition: GazeboModel.h:411
void printState() const
Definition: GazeboModel.h:424
GazeboModel(const std::string &name)
Definition: GazeboModel.h:53
const Eigen::Affine3d & getWorldToBaseTransform() const
Definition: GazeboModel.h:373
const std::string & getBaseName()
Definition: GazeboModel.h:71
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:69
int getNDof() const
Definition: GazeboModel.h:418
std::shared_ptr< orca::gazebo::GazeboContactSensor > attachContactSensorToLink(const std::string &link_name, const std::string &collision_name, double update_rate=0)
Definition: GazeboModel.h:290
Definition: CartesianAccelerationPID.h:44
void setJointGravityTorques(const Eigen::VectorXd &gravity_torques)
Definition: GazeboModel.h:391
const Eigen::Matrix< double, 6, 1 > & getBaseVelocity() const
Definition: GazeboModel.h:367
bool setModelConfiguration(const std::vector< std::string > &joint_names, const std::vector< double > &joint_positions)
Definition: GazeboModel.h:105
const std::string & getName() const
Definition: GazeboModel.h:65
const Eigen::VectorXd & getJointMeasuredTorques() const
Definition: GazeboModel.h:405
void worldUpdateEnd()
Definition: GazeboModel.h:510
GazeboModel(::gazebo::physics::ModelPtr model)
Definition: GazeboModel.h:59
void executeAfterWorldUpdate(std::function< void(uint32_t, double, double)> callback)
Definition: GazeboModel.h:443
Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
Definition: Utils.h:94