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_);
235 #if GAZEBO_MAJOR_VERSION > 8 236 auto joint = model_->GetJoint(joint_name);
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>";
251 auto sensor_base_link = joint->GetChild();
252 auto sensor = std::make_shared<orca::gazebo::GazeboForceTorqueSensor>(sensor_base_link);
254 std::string forceTorqueSensorString = ss.str();
256 sdf::ElementPtr sdf(
new sdf::Element);
257 sdf::initFile(
"sensor.sdf", sdf);
258 sdf::readString(forceTorqueSensorString, sdf);
260 auto mgr = ::gazebo::sensors::SensorManager::Instance();
261 std::string sensorName = mgr->CreateSensor(sdf,
"default",
262 getName() +
"::" + joint_name, joint->GetId());
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);
273 auto gz_sensor = std::dynamic_pointer_cast<::gazebo::sensors::ForceTorqueSensor>(
274 mgr->GetSensor(sensorName));
277 if(!gz_sensor)
throw std::runtime_error(
"Could not create force_torque sensor for joint " + joint_name);
279 if(!gz_sensor->IsActive())
throw std::runtime_error(
"Sensor is not active");
281 sensor->setSensor(gz_sensor);
283 std::cout <<
"[GazeboModel \'" <<
getName() <<
"\'] " <<
"Force torque sensor '" << sensorName <<
"' successfully created" <<
'\n';
286 throw std::runtime_error(
"Adding sensors is only supported for gz version > 8");
290 std::shared_ptr<orca::gazebo::GazeboContactSensor>
attachContactSensorToLink(
const std::string& link_name,
const std::string& collision_name,
double update_rate = 0)
294 #if GAZEBO_MAJOR_VERSION > 8 295 auto link = model_->GetLink(link_name);
297 throw std::runtime_error(
"Link " + link_name +
" does not exists");
298 auto collision = link->GetChildCollision(collision_name);
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);
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>";
313 ss <<
" <collision>" << collision_name <<
"</collision>";
317 auto sensor = std::make_shared<orca::gazebo::GazeboContactSensor>(link, collision_name);
319 std::string contactSensorStr = ss.str();
321 sdf::ElementPtr sdf(
new sdf::Element);
322 sdf::initFile(
"sensor.sdf", sdf);
323 sdf::readString(contactSensorStr, sdf);
325 auto mgr = ::gazebo::sensors::SensorManager::Instance();
326 std::string sensorName = mgr->CreateSensor(sdf,
"default",
327 getName() +
"::" + link_name, link->GetId());
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);
338 auto gz_sensor = std::dynamic_pointer_cast<::gazebo::sensors::ContactSensor>(
339 mgr->GetSensor(sensorName));
342 if(!gz_sensor)
throw std::runtime_error(
"Could not create contact sensor for link " + link_name);
344 if(!gz_sensor->IsActive())
throw std::runtime_error(
"Sensor is not active");
346 sensor->setSensor(gz_sensor);
348 std::cout <<
"[GazeboModel \'" <<
getName() <<
"\'] " <<
"Contact sensor '" << sensorName <<
"' successfully created" <<
'\n';
351 throw std::runtime_error(
"Adding sensors is only supported for gz version > 8");
358 return gravity_vector_;
364 return actuated_joint_names_;
370 return current_base_vel_;
376 return current_world_to_base_;
382 return current_joint_positions_;
388 return current_joint_velocities_;
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;
402 return current_joint_external_torques_;
408 return current_joint_measured_torques_;
414 joint_torque_command_ = joint_torque_command;
428 std::cout <<
"[GazeboModel] Model is not loaded." <<
'\n';
432 std::cout <<
"[GazeboModel \'" <<
getName() <<
"\'] State :" <<
'\n';
433 std::cout <<
"- Gravity " <<
getGravity().transpose() <<
'\n';
434 std::cout <<
"- Base velocity\n" <<
getBaseVelocity().transpose() <<
'\n';
440 std::cout <<
"- Brakes " << (brakes_ ?
"Enabled" :
"Disabled") <<
'\n';
445 callback_ = callback;
455 #if GAZEBO_MAJOR_VERSION > 8 456 return world_->Iterations();
458 return world_->GetIterations();
465 std::cout <<
"[GazeboModel] Model is not loaded." <<
'\n';
469 std::cerr <<
"[GazeboModel::" <<
getName() <<
"]" <<
'\n';
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';
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';
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';
488 #if GAZEBO_MAJOR_VERSION > 8 489 auto g = world_->Gravity();
491 auto g = world_->GetPhysicsEngine()->GetGravity();
493 gravity_vector_[0] = g[0];
494 gravity_vector_[1] = g[1];
495 gravity_vector_[2] = g[2];
497 model_->SetEnabled(!brakes_);
501 for(
int i=0 ; i < ndof_ ; ++i)
502 joints_[i]->SetForce(0,joint_torque_command_[i] + joint_gravity_torques_[i]);
506 for(
int i=0 ; i < ndof_ ; ++i)
507 joints_[i]->SetVelocity(0, 0.0);
512 for(
int i=0 ; i < ndof_ ; ++i)
514 auto joint = joints_[i];
515 #if GAZEBO_MAJOR_VERSION > 8 516 current_joint_positions_[i] = joint->Position(0);
518 current_joint_positions_[i] = joint->GetAngle(0).Radian();
520 current_joint_velocities_[i] = joint->GetVelocity(0);
521 current_joint_external_torques_[i] = joint->GetForce(0);
523 auto w = joint->GetForceTorque(0u);
524 #if GAZEBO_MAJOR_VERSION > 8 525 auto a = joint->LocalAxis(0u);
527 auto a = joint->GetLocalAxis(0u);
529 current_joint_measured_torques_[i] = a.Dot(w.body1Torque);
532 #if GAZEBO_MAJOR_VERSION > 8 533 current_world_to_base_ =
convPose(model_->RelativePose());
535 current_base_vel_.head(3) =
convVec3(model_->RelativeLinearVel());
536 current_base_vel_.tail(3) =
convVec3(model_->RelativeAngularVel());
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();
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;
553 #if GAZEBO_MAJOR_VERSION > 8 554 double sim_time = world_->SimTime().Double();
556 double sim_time = world_->GetSimTime().Double();
558 #if GAZEBO_MAJOR_VERSION > 8 559 double dt = world_->Physics()->GetMaxStepSize();
561 double dt = world_->GetPhysicsEngine()->GetMaxStepSize();
567 void assertModelLoaded()
const 570 throw std::runtime_error(
"[GazeboModel] Model is not loaded");
574 current_base_vel_.setZero();
575 current_world_to_base_.setIdentity();
576 gravity_vector_.setZero();
580 auto world = ::gazebo::physics::get_world();
583 std::cerr <<
"[GazeboModel::" <<
getName() <<
"] " <<
"Could not load gazebo world" <<
'\n';
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_;
606 std::string base_name_;
607 Eigen::Vector3d gravity_vector_;
608 ::gazebo::event::ConnectionPtr world_begin_;
609 ::gazebo::event::ConnectionPtr world_end_;
612 std::function<void(uint32_t,double,double)> callback_;
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