diff --git a/src/SpringPlugin.cc b/src/SpringPlugin.cc index fe442ab276ac594eee1a190be71d51909ecb248b..ff4ef366febcc9c8adbec0de835866ff77c9a32d 100644 --- a/src/SpringPlugin.cc +++ b/src/SpringPlugin.cc @@ -17,7 +17,6 @@ #include "SpringPlugin.hh" - using namespace gazebo; GZ_REGISTER_MODEL_PLUGIN(SpringPlugin) @@ -26,9 +25,7 @@ GZ_REGISTER_MODEL_PLUGIN(SpringPlugin) SpringPlugin::SpringPlugin() {} ///////////////////////////////////////////////// -void SpringPlugin::Load(physics::ModelPtr lmodel, - sdf::ElementPtr lsdf) -{ +void SpringPlugin::Load(physics::ModelPtr lmodel, sdf::ElementPtr lsdf) { model_ = lmodel; // hardcoded params for this test @@ -43,24 +40,18 @@ void SpringPlugin::Load(physics::ModelPtr lmodel, axisExplicit_ = lsdf->Get<int>("axis"); - ROS_INFO_NAMED("SpringPlugin", - "Loading joint : %s kp: %f kd: %f alongs %d axis", - jointExplicitName_.c_str(), - kpExplicit_, - kdExplicit_, - axisExplicit_); + ROS_INFO_NAMED("SpringPlugin", "Loading joint : %s kp: %f kd: %f alongs %d axis", jointExplicitName_.c_str(), + kpExplicit_, kdExplicit_, axisExplicit_); } ///////////////////////////////////////////////// -void SpringPlugin::Init() -{ +void SpringPlugin::Init() { jointExplicit_ = model_->GetJoint(jointExplicitName_); /* jointImplicit->SetStiffnessDamping(0, kpImplicit, kdImplicit); */ - updateConnection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&SpringPlugin::ExplicitUpdate, this)); + updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&SpringPlugin::ExplicitUpdate, this)); } ///////////////////////////////////////////////// @@ -80,7 +71,6 @@ void SpringPlugin::ExplicitUpdate() { double pos = jointExplicit_->Position(axisExplicit_); #endif double vel = jointExplicit_->GetVelocity(axisExplicit_); - double force = -kpExplicit_ * pos - -kdExplicit_ * vel; + double force = -kpExplicit_ * pos - kdExplicit_ * vel; jointExplicit_->SetForce(axisExplicit_, force); } diff --git a/src/SpringPlugin.hh b/src/SpringPlugin.hh index 3ded1d20dfdcb8b6a17b87e8bb6b6771b4e6122c..5fcb23733dc3f4827b0633e03e61a5be3f0a6a6c 100644 --- a/src/SpringPlugin.hh +++ b/src/SpringPlugin.hh @@ -28,33 +28,46 @@ #pragma GCC diagnostic pop -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_; + + private: + common::Time prevUpdateTime_; + + private: + physics::JointPtr jointExplicit_; + + private: + std::string jointExplicitName_; /// \brief simulate spring/damper with ExplicitUpdate function -private: double kpExplicit_; + private: + double kpExplicit_; /// \brief simulate spring/damper with ExplicitUpdate function -private: double kdExplicit_; - + private: + double kdExplicit_; + /// \brief Specify on which axis the spring is applied. -private: int axisExplicit_; + private: + int axisExplicit_; }; -} +} // namespace gazebo #endif