v1.0.0

.gitmodules

+[submodule "cmake"]
+	path = cmake
+	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git

CMakeLists.txt

+# Copyright 2020 CNRS
+cmake_minimum_required(VERSION 3.1)
+
+# Project properties
+set(PROJECT_ORG gepetto)
+set(PROJECT_NAME gazebo-spring)
+set(PROJECT_DESCRIPTION "A spring plugin for gazebo")
+set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
+
+# Project options
+OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
+
+# Project configuration
+SET(PROJECT_USE_CMAKE_EXPORT TRUE)
+
+# JRL-cmakemodule setup
+include(cmake/base.cmake)
+
+# Project definition
+COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
+#CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
+
+# Project dependencies
+find_package(gazebo REQUIRED)
+find_package(catkin REQUIRED COMPONENTS roscpp)
+
+if(NOT ${gazebo_VERSION} VERSION_LESS 11)
+  CHECK_MINIMAL_CXX_STANDARD(17 ENFORCE)
+endif()
+
+# Main Library
+SET(${PROJECT_NAME}_HEADERS
+  )
+
+SET(${PROJECT_NAME}_SOURCES
+  src/SpringPlugin.cc
+  )
+
+ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
+TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PRIVATE ${GAZEBO_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+
+IF(SUFFIX_SO_VERSION)
+  SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
+ENDIF(SUFFIX_SO_VERSION)
+
+INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)

README.md

+# Gazebo spring plugin
+
+imported from talos-data

cmake @ 000a9019

+Subproject commit 000a90190d1dca8b028f437095f2fddff0f9839b

package.xml

+<?xml version="1.0"?>
+<package format="3">
+    <name>gazebo-spring</name>
+    <version>1.0.0</version>
+    <description>A spring plugin for gazebo</description>
+    <maintainer email="guilhem.saurel@laas.fr">Guilhem Saurel</maintainer>
+    <license>BSD</license>
+    <exec_depend condition="$ROS_VERSION == 1">catkin</exec_depend>
+    <exec_depend condition="$ROS_VERSION == 2">ament_cmake</exec_depend>
+    <buildtool_depend>cmake</buildtool_depend>
+    <export>
+        <build_type>cmake</build_type>
+    </export>
+</package>

src/SpringPlugin.cc

+/*
+ * Copyright 2011 Nate Koenig & Andrew Howard
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "SpringPlugin.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_MODEL_PLUGIN(SpringPlugin)
+
+/////////////////////////////////////////////////
+SpringPlugin::SpringPlugin() {}
+
+/////////////////////////////////////////////////
+void SpringPlugin::Load(physics::ModelPtr lmodel, sdf::ElementPtr lsdf) {
+  model_ = lmodel;
+
+  // hardcoded params for this test
+  if (!lsdf->HasElement("joint_spring"))
+    ROS_ERROR_NAMED("SpringPlugin", "No field joint_spring for SpringPlugin");
+  else
+    jointExplicitName_ = lsdf->Get<std::string>("joint_spring");
+
+  kpExplicit_ = lsdf->Get<double>("kp");
+
+  kdExplicit_ = lsdf->Get<double>("kd");
+
+  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_);
+}
+
+/////////////////////////////////////////////////
+void SpringPlugin::Init() {
+  jointExplicit_ = model_->GetJoint(jointExplicitName_);
+
+  /*  jointImplicit->SetStiffnessDamping(0, kpImplicit,
+      kdImplicit); */
+
+  updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&SpringPlugin::ExplicitUpdate, this));
+}
+
+/////////////////////////////////////////////////
+void SpringPlugin::ExplicitUpdate() {
+#if GAZEBO_MAJOR_VERSION < 9
+  common::Time currTime = model_->GetWorld()->GetSimTime();
+#else
+  common::Time currTime = model_->GetWorld()->SimTime();
+#endif
+
+  common::Time stepTime = currTime - prevUpdateTime_;
+  prevUpdateTime_ = currTime;
+
+#if GAZEBO_MAJOR_VERSION < 9
+  double pos = jointExplicit_->GetAngle(axisExplicit_).Radian();
+#else
+  double pos = jointExplicit_->Position(axisExplicit_);
+#endif
+  double vel = jointExplicit_->GetVelocity(axisExplicit_);
+  double force = -kpExplicit_ * pos - kdExplicit_ * vel;
+  jointExplicit_->SetForce(axisExplicit_, force);
+}

src/SpringPlugin.hh

+/*
+ * Copyright (C) 2012 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * 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();
+
+ private:
+  event::ConnectionPtr updateConnection_;
+
+ 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_;
+
+  /// \brief simulate spring/damper with ExplicitUpdate function
+ private:
+  double kdExplicit_;
+
+  /// \brief Specify on which axis the spring is applied.
+ private:
+  int axisExplicit_;
+};
+}  // namespace gazebo
+#endif