From e8a8794b63c26071c832c362ef6e4c119171ee39 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 10 Nov 2019 08:47:28 +0800
Subject: [PATCH] Generalize Spring.
---
src/SpringPlugin.cc | 16 +++++++++-------
src/SpringPlugin.hh | 43 +++++++++++++++++++++++++------------------
2 files changed, 34 insertions(+), 25 deletions(-)
diff --git a/src/SpringPlugin.cc b/src/SpringPlugin.cc
index 487dd25..13f6d38 100644
--- a/src/SpringPlugin.cc
+++ b/src/SpringPlugin.cc
@@ -15,9 +15,8 @@
*
*/
-#include "gazebo/physics/physics.hh"
#include "SpringPlugin.hh"
-#include <ros/ros.h>
+
using namespace gazebo;
@@ -44,11 +43,14 @@ void SpringPlugin::Load(physics::ModelPtr lmodel,
this->kdExplicit = lsdf->Get<double>("kd");
+ this->axisExplicit = lsdf->Get<int>("axis");
+
ROS_INFO_NAMED("SpringPlugin",
- "Loading joint : %s kp: %f kd: %f",
+ "Loading joint : %s kp: %f kd: %f alongs %d axis",
this->jointExplicitName.c_str(),
this->kpExplicit,
- this->kdExplicit);
+ this->kdExplicit,
+ this->axisExplicit);
}
/////////////////////////////////////////////////
@@ -77,9 +79,9 @@ void SpringPlugin::ExplicitUpdate()
common::Time stepTime = currTime - this->prevUpdateTime;
this->prevUpdateTime = currTime;
- double pos = this->jointExplicit->GetAngle(0).Radian();
- double vel = this->jointExplicit->GetVelocity(0);
+ double pos = this->jointExplicit->GetAngle(axisExplicit).Radian();
+ double vel = this->jointExplicit->GetVelocity(axisExplicit);
double force = -this->kpExplicit * pos
-this->kdExplicit * vel;
- this->jointExplicit->SetForce(0, force);
+ this->jointExplicit->SetForce(axisExplicit, force);
}
diff --git a/src/SpringPlugin.hh b/src/SpringPlugin.hh
index 586fa21..2e8655e 100644
--- a/src/SpringPlugin.hh
+++ b/src/SpringPlugin.hh
@@ -13,41 +13,48 @@
* 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__
#include <string>
+#pragma GCC diagnostic push
+#pragma GCC system_header
#include "gazebo/common/Plugin.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/util/system.hh"
+#include <ros/ros.h>
+
+#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();
-
- private: void ExplicitUpdate();
+class GAZEBO_VISIBLE SpringPlugin : public ModelPlugin
+{
+public: SpringPlugin();
+public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+public: virtual void Init();
- private: event::ConnectionPtr updateConnection;
+private: void ExplicitUpdate();
- private: physics::ModelPtr model;
+private: event::ConnectionPtr updateConnection;
- private: common::Time prevUpdateTime;
+private: physics::ModelPtr model;
- private: physics::JointPtr jointExplicit;
- private: std::string jointExplicitName;
+private: common::Time prevUpdateTime;
- /// \brief simulate spring/damper with ExplicitUpdate function
- private: double kpExplicit;
+private: physics::JointPtr jointExplicit;
+private: std::string jointExplicitName;
- /// \brief simulate spring/damper with ExplicitUpdate function
- private: double kdExplicit;
+ /// \brief simulate spring/damper with ExplicitUpdate function
+private: double kpExplicit;
- };
+ /// \brief simulate spring/damper with ExplicitUpdate function
+private: double kdExplicit;
+
+ /// \brief Specify on which axis the spring is applied.
+private: int axisExplicit;
+};
}
#endif
--
GitLab