40 #include <gazebo/gazebo.hh> 41 #include <gazebo/physics/physics.hh> 42 #include <gazebo/common/common.hh> 81 if(joint_names.size() != actuated_joint_names_.size())
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." 91 for(
auto joint_name : joint_names)
93 auto joint = model_->GetJoint(joint_name);
97 throw std::runtime_error(
"Joint " + joint_name +
" does not exists in model");
99 joints_.push_back(joint);
102 this->actuated_joint_names_ = joint_names;
109 if (joint_names.size() != joint_positions.size())
111 std::cerr <<
"[GazeboModel \'" <<
getName() <<
"\'] " <<
"joint_names lenght should be the same as joint_positions : " << joint_names.size() <<
" vs " << joint_positions.size() <<
'\n';
115 auto world = ::gazebo::physics::get_world();
117 bool is_paused = world->IsPaused();
118 if (!is_paused) world->SetPaused(
true);
120 std::map<std::string, double> joint_position_map;
121 for (
unsigned int i = 0; i < joint_names.size(); i++)
123 joint_position_map[joint_names[i]] = joint_positions[i];
126 model_->SetJointPositions(joint_position_map);
129 world->SetPaused(is_paused);
134 bool load(
const std::string& model_name)
139 #if GAZEBO_MAJOR_VERSION > 8 140 auto model = world_->ModelByName( model_name );
142 auto model = world_->GetModel( model_name );
146 std::cerr <<
"[GazeboModel \'" <<
getName() <<
"\'] " <<
"Could not get gazebo model " << model_name <<
". Make sure it is loaded in the server" <<
'\n';
152 bool load(::gazebo::physics::ModelPtr model)
156 std::cerr <<
"Model is invalid" <<
'\n';
163 actuated_joint_names_.clear();
164 for(
auto joint : model->GetJoints() )
167 joint->SetProvideFeedback(
true);
171 #
if GAZEBO_MAJOR_VERSION >= 7
172 && joint->GetType() != ::gazebo::physics::Joint::FIXED_JOINT
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))
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())
187 joints_.push_back(joint);
188 actuated_joint_names_.push_back(joint->GetName());
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)
199 <<
" lower limit " << joint->GetLowerLimit(0u)
200 <<
" upper limit " << joint->GetUpperLimit(0u)
205 if(actuated_joint_names_.size() == 0)
207 std::cerr <<
"[GazeboModel \'" << model->GetName() <<
"\'] " <<
"Could not get any actuated joint for model " << model->GetName() <<
'\n';
214 links_ = model->GetLinks();
215 name_ = model->GetName();
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_);
234 #if GAZEBO_MAJOR_VERSION > 8 235 auto joint = model_->GetJoint(joint_name);
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>";
246 std::string forceTorqueSensorString = ss.str();
248 sdf::ElementPtr sdf(
new sdf::Element);
249 sdf::initFile(
"sensor.sdf", sdf);
250 sdf::readString(forceTorqueSensorString, sdf);
252 auto mgr = ::gazebo::sensors::SensorManager::Instance();
253 std::string sensorName = mgr->CreateSensor(sdf,
"default",
254 getName() +
"::" + joint_name, joint->GetId());
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);
265 auto sensor = std::dynamic_pointer_cast<::gazebo::sensors::ForceTorqueSensor>(
266 mgr->GetSensor(sensorName));
269 if(!sensor)
throw std::runtime_error(
"Could not create force_torque sensor for joint " + joint_name);
271 if(!sensor->IsActive())
throw std::runtime_error(
"Sensor is not active");
273 std::cout <<
"[GazeboModel \'" <<
getName() <<
"\'] " <<
"Force torque sensor '" << sensorName <<
"' successfully created" <<
'\n';
276 throw std::runtime_error(
"Adding sensors is only supported for gz version > 8");
284 #if GAZEBO_MAJOR_VERSION > 8 285 auto link = model_->GetLink(link_name);
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>";
296 std::string contactSensorStr = ss.str();
298 sdf::ElementPtr sdf(
new sdf::Element);
299 sdf::initFile(
"sensor.sdf", sdf);
300 sdf::readString(contactSensorStr, sdf);
302 auto mgr = ::gazebo::sensors::SensorManager::Instance();
303 std::string sensorName = mgr->CreateSensor(sdf,
"default",
304 getName() +
"::" + link_name, link->GetId());
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);
315 auto sensor = std::dynamic_pointer_cast<::gazebo::sensors::ContactSensor>(
316 mgr->GetSensor(sensorName));
319 if(!sensor)
throw std::runtime_error(
"Could not create contact sensor for link " + link_name);
321 if(!sensor->IsActive())
throw std::runtime_error(
"Sensor is not active");
323 std::cout <<
"[GazeboModel \'" <<
getName() <<
"\'] " <<
"Contact sensor '" << sensorName <<
"' successfully created" <<
'\n';
326 throw std::runtime_error(
"Adding sensors is only supported for gz version > 8");
333 return gravity_vector_;
339 return actuated_joint_names_;
345 return current_base_vel_;
351 return current_world_to_base_;
357 return current_joint_positions_;
363 return current_joint_velocities_;
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;
377 return current_joint_external_torques_;
383 return current_joint_measured_torques_;
389 joint_torque_command_ = joint_torque_command;
403 std::cout <<
"[GazeboModel] Model is not loaded." <<
'\n';
407 std::cout <<
"[GazeboModel \'" <<
getName() <<
"\'] State :" <<
'\n';
408 std::cout <<
"- Gravity " <<
getGravity().transpose() <<
'\n';
409 std::cout <<
"- Base velocity\n" <<
getBaseVelocity().transpose() <<
'\n';
415 std::cout <<
"- Brakes " << (brakes_ ?
"Enabled" :
"Disabled") <<
'\n';
420 callback_ = callback;
430 #if GAZEBO_MAJOR_VERSION > 8 431 return world_->Iterations();
433 return world_->GetIterations();
440 std::cout <<
"[GazeboModel] Model is not loaded." <<
'\n';
444 std::cerr <<
"[GazeboModel::" <<
getName() <<
"]" <<
'\n';
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';
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';
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';
463 #if GAZEBO_MAJOR_VERSION > 8 464 auto g = world_->Gravity();
466 auto g = world_->GetPhysicsEngine()->GetGravity();
468 gravity_vector_[0] = g[0];
469 gravity_vector_[1] = g[1];
470 gravity_vector_[2] = g[2];
472 model_->SetEnabled(!brakes_);
476 for(
int i=0 ; i < ndof_ ; ++i)
477 joints_[i]->SetForce(0,joint_torque_command_[i] + joint_gravity_torques_[i]);
481 for(
int i=0 ; i < ndof_ ; ++i)
482 joints_[i]->SetVelocity(0, 0.0);
487 for(
int i=0 ; i < ndof_ ; ++i)
489 auto joint = joints_[i];
490 #if GAZEBO_MAJOR_VERSION > 8 491 current_joint_positions_[i] = joint->Position(0);
493 current_joint_positions_[i] = joint->GetAngle(0).Radian();
495 current_joint_velocities_[i] = joint->GetVelocity(0);
496 current_joint_external_torques_[i] = joint->GetForce(0);
498 auto w = joint->GetForceTorque(0u);
499 #if GAZEBO_MAJOR_VERSION > 8 500 auto a = joint->LocalAxis(0u);
502 auto a = joint->GetLocalAxis(0u);
504 current_joint_measured_torques_[i] = a.Dot(w.body1Torque);
507 #if GAZEBO_MAJOR_VERSION > 8 508 current_world_to_base_ =
convPose(model_->RelativePose());
510 current_base_vel_.head(3) =
convVec3(model_->RelativeLinearVel());
511 current_base_vel_.tail(3) =
convVec3(model_->RelativeAngularVel());
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();
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;
528 #if GAZEBO_MAJOR_VERSION > 8 529 double sim_time = world_->SimTime().Double();
531 double sim_time = world_->GetSimTime().Double();
533 #if GAZEBO_MAJOR_VERSION > 8 534 double dt = world_->Physics()->GetMaxStepSize();
536 double dt = world_->GetPhysicsEngine()->GetMaxStepSize();
542 void assertModelLoaded()
const 545 throw std::runtime_error(
"[GazeboModel] Model is not loaded");
549 current_base_vel_.setZero();
550 current_world_to_base_.setIdentity();
551 gravity_vector_.setZero();
555 auto world = ::gazebo::physics::get_world();
558 std::cerr <<
"[GazeboModel::" <<
getName() <<
"] " <<
"Could not load gazebo world" <<
'\n';
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_;
581 std::string base_name_;
582 Eigen::Vector3d gravity_vector_;
583 ::gazebo::event::ConnectionPtr world_begin_;
584 ::gazebo::event::ConnectionPtr world_end_;
587 std::function<void(uint32_t,double,double)> callback_;
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