From 72fbd71580eea86d5afc243d3f647779c48c741c Mon Sep 17 00:00:00 2001 From: fmessmer Date: Mon, 5 Aug 2019 14:20:21 +0200 Subject: [PATCH] query physics engine type --- .../src/hwi_switch_robot_hw_sim.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/cob_gazebo_ros_control/src/hwi_switch_robot_hw_sim.cpp b/cob_gazebo_ros_control/src/hwi_switch_robot_hw_sim.cpp index 003f6cb..d3c6120 100644 --- a/cob_gazebo_ros_control/src/hwi_switch_robot_hw_sim.cpp +++ b/cob_gazebo_ros_control/src/hwi_switch_robot_hw_sim.cpp @@ -231,6 +231,19 @@ bool HWISwitchRobotHWSim::initSim( sim_joints_.push_back(joint); + // get physics engine type +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); +#else + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine(); +#endif + physics_type_ = physics->GetType(); + if (physics_type_.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "No physics type found."); + } + + // ToDo: Can a joint (gazebo::physics::JointPtr) be used for EFFORT if joint->SetMaxForce has been called before? if (joint_control_methods_[index] == VELOCITY || joint_control_methods_[index] == POSITION) {