diff --git a/src/SpringPlugin.cc b/src/SpringPlugin.cc index 487dd25a0f965cdeb3048da3125e6fae053afc56..957c8b5ee6facd92b6a1c8ac823a40bbab9470db 100644 --- a/src/SpringPlugin.cc +++ b/src/SpringPlugin.cc @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ #include "gazebo/physics/physics.hh" #include "SpringPlugin.hh" @@ -24,19 +24,15 @@ using namespace gazebo; GZ_REGISTER_MODEL_PLUGIN(SpringPlugin) ///////////////////////////////////////////////// -SpringPlugin::SpringPlugin() -{ -} +SpringPlugin::SpringPlugin() {} ///////////////////////////////////////////////// -void SpringPlugin::Load(physics::ModelPtr lmodel, - sdf::ElementPtr lsdf) -{ +void SpringPlugin::Load(physics::ModelPtr lmodel, sdf::ElementPtr lsdf) { this->model = lmodel; // hardcoded params for this test if (!lsdf->HasElement("joint_spring")) - ROS_ERROR_NAMED("SpringPlugin","No field joint_spring for SpringPlugin"); + ROS_ERROR_NAMED("SpringPlugin", "No field joint_spring for SpringPlugin"); else this->jointExplicitName = lsdf->Get<std::string>("joint_spring"); @@ -44,30 +40,22 @@ void SpringPlugin::Load(physics::ModelPtr lmodel, this->kdExplicit = lsdf->Get<double>("kd"); - ROS_INFO_NAMED("SpringPlugin", - "Loading joint : %s kp: %f kd: %f", - this->jointExplicitName.c_str(), - this->kpExplicit, + ROS_INFO_NAMED("SpringPlugin", "Loading joint : %s kp: %f kd: %f", this->jointExplicitName.c_str(), this->kpExplicit, this->kdExplicit); } ///////////////////////////////////////////////// -void SpringPlugin::Init() -{ +void SpringPlugin::Init() { this->jointExplicit = this->model->GetJoint(this->jointExplicitName); /* this->jointImplicit->SetStiffnessDamping(0, this->kpImplicit, this->kdImplicit); */ - this->updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&SpringPlugin::ExplicitUpdate, this)); + this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&SpringPlugin::ExplicitUpdate, this)); } - - ///////////////////////////////////////////////// -void SpringPlugin::ExplicitUpdate() -{ +void SpringPlugin::ExplicitUpdate() { #if GAZEBO_MAJOR_VERSION < 9 common::Time currTime = this->model->GetWorld()->GetSimTime(); #else @@ -77,9 +65,12 @@ void SpringPlugin::ExplicitUpdate() common::Time stepTime = currTime - this->prevUpdateTime; this->prevUpdateTime = currTime; +#if GAZEBO_MAJOR_VERSION < 9 double pos = this->jointExplicit->GetAngle(0).Radian(); +#else + double pos = this->jointExplicit->Position(0); +#endif double vel = this->jointExplicit->GetVelocity(0); - double force = -this->kpExplicit * pos - -this->kdExplicit * vel; + double force = -this->kpExplicit * pos - this->kdExplicit * vel; this->jointExplicit->SetForce(0, force); } diff --git a/src/SpringPlugin.hh b/src/SpringPlugin.hh index 586fa211a269d0d350550935ab237e4d472380fa..19d5bbd774ffa12958637f960e6486bfb0c3c4f2 100644 --- a/src/SpringPlugin.hh +++ b/src/SpringPlugin.hh @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ #ifndef __GAZEBO_SPRING_TEST_PLUGIN_HH__ #define __GAZEBO_SPRING_TEST_PLUGIN_HH__ @@ -23,31 +23,42 @@ #include "gazebo/physics/physics.hh" #include "gazebo/util/system.hh" -namespace gazebo -{ - class GAZEBO_VISIBLE SpringPlugin : public ModelPlugin - { - public: SpringPlugin(); - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - public: virtual void Init(); +namespace gazebo { +class GAZEBO_VISIBLE SpringPlugin : public ModelPlugin { + public: + SpringPlugin(); - private: void ExplicitUpdate(); + public: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - private: event::ConnectionPtr updateConnection; + public: + virtual void Init(); - private: physics::ModelPtr model; + private: + void ExplicitUpdate(); - private: common::Time prevUpdateTime; + private: + event::ConnectionPtr updateConnection; - private: physics::JointPtr jointExplicit; - private: std::string jointExplicitName; + private: + physics::ModelPtr model; - /// \brief simulate spring/damper with ExplicitUpdate function - private: double kpExplicit; + private: + common::Time prevUpdateTime; - /// \brief simulate spring/damper with ExplicitUpdate function - private: double kdExplicit; + private: + physics::JointPtr jointExplicit; - }; -} + private: + std::string jointExplicitName; + + /// \brief simulate spring/damper with ExplicitUpdate function + private: + double kpExplicit; + + /// \brief simulate spring/damper with ExplicitUpdate function + private: + double kdExplicit; +}; +} // namespace gazebo #endif diff --git a/srdf/pyrene.srdf b/srdf/pyrene.srdf new file mode 120000 index 0000000000000000000000000000000000000000..70c974e500bbb5f6251ff8c8f060ddc6d57d1c4b --- /dev/null +++ b/srdf/pyrene.srdf @@ -0,0 +1 @@ +talos.srdf \ No newline at end of file diff --git a/urdf/pyrene.urdf b/urdf/pyrene.urdf new file mode 120000 index 0000000000000000000000000000000000000000..23aaac66293d24dad83b551ae06e699c03b097df --- /dev/null +++ b/urdf/pyrene.urdf @@ -0,0 +1 @@ +talos_full_v2.urdf \ No newline at end of file