Add files via upload

gazebo_ros_muscle_interface/CMakeLists.txt

@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_ros_muscle_interface)
+
+# check c++11 / c++0x
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+    message(STATUS "Compiler supports cxx11")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+    message(STATUS "Compiler supports c++0x")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+endif()
+
+# https://gcc.gnu.org/wiki/Visibility
+# Because reasons ...
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fvisibility=hidden")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_ros
+  gazebo_msgs
+  roscpp
+  std_msgs
+  geometry_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(gazebo REQUIRED)
+find_package(OGRE)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  MuscleState.msg
+  MuscleStates.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetMuscleActivations.srv
+  SetMuscleActivations.srv
+  GetMuscleStates.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES gazebo_ros_muscle_interface
+  CATKIN_DEPENDS gazebo_ros gazebo_msgs roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+  ${TBB_INCLUDE_DIR}
+)
+
+## Declare a cpp library
+ add_library(gazebo_ros_muscle_interface
+   src/gazebo_ros_muscle_interface.cpp
+ )
+
+link_directories(
+  ${OGRE_LIBRARY_DIRS}
+)
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(gazebo_ros_muscle_interface
+   ${catkin_LIBRARIES}
+   ${GAZEBO_LIBRARIES}
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executables and/or libraries for installation
+install(TARGETS gazebo_ros_muscle_interface
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_husky_gazebo_plugins.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

gazebo_ros_muscle_interface/README.txt

@@ -0,0 +1,24 @@
+Short description for testing the generic controller plugin.
+
+1. Compile the GazeboRosPackages ROS workspace with catkin_make
+
+2. Create a symlink for the test model in your ~/.gazebo/models folder:
+cd ~/.gazebo/models
+ln -s <package_path>/sdf test_model
+
+3. start gazebo with your GazeboRosPackages workspace sourced:
+cd <GazeboRosPackages>
+source devel/setup.bash
+rosrun gazebo_ros gazebo  (--verbose for debug output)
+
+4. Insert the test model from the models list. It will show a robot with 4 links and 3 joints (see model.sdf in sdf folder).
+
+5. Use ROS topic publisher to control the joints:
+rostopic list    (list available topics)
+
+example for setting velocity of joint2 to 0.2 meter per second:
+rostopic pub /test_robot/my_joint2/cmd_vel std_msgs/Float64 "data: 0.2"
+
+example for setting position of joint1 to 1.5 rad (joint angle):
+rostopic pub /test_robot/my_joint1/cmd_pos std_msgs/Float64 "data: 1.5"
+

gazebo_ros_muscle_interface/include/gazebo_ros_muscle_interface.h

@@ -0,0 +1,106 @@
+#ifndef GENERIC_CONTROLLER_PLUGIN_H
+#define GENERIC_CONTROLLER_PLUGIN_H
+
+#include <vector>
+#include <string>
+#include <gazebo/common/Events.hh>
+#include <gazebo/physics/opensim/OpensimModel.hh>
+#include <gazebo/physics/opensim/OpensimMuscle.hh>
+#include <gazebo/physics/opensim/OpensimJoint.hh>
+#include <gazebo/physics/opensim/OpensimLink.hh>
+#include <gazebo/gazebo.hh>
+
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/transport/Node.hh>
+#include <gazebo/transport/Publisher.hh>
+
+// Message headers are found in the install directory.
+#include <std_msgs/Float64.h>
+#include <geometry_msgs/Vector3.h>
+#include <sensor_msgs/JointState.h>
+#include <gazebo_ros_muscle_interface/MuscleStates.h>
+#include <gazebo_ros_muscle_interface/GetMuscleActivations.h>
+#include <gazebo_ros_muscle_interface/SetMuscleActivations.h>
+#include <gazebo_ros_muscle_interface/GetMuscleStates.h>
+
+#include <ros/ros.h>
+
+#include <mutex>
+
+namespace gazebo
+{
+
+using namespace gazebo_ros_muscle_interface;
+
+using physics::OpensimMusclePtr;
+using physics::OpensimModelPtr;
+
+
+class MuscleInterfacePlugin : public ModelPlugin
+{
+public:
+    MuscleInterfacePlugin();
+    ~MuscleInterfacePlugin();
+
+  // Load the plugin and initilize all controllers
+  void Load(physics::ModelPtr parent, sdf::ElementPtr sdf) override;
+
+  void Init() override;
+
+  void Reset() override;
+
+  // Simulation update callback function
+  void OnUpdateEnd(/*const common::UpdateInfo &/*_info*/);
+
+private:
+  // Generic position command callback function (ROS topic)
+  void activationCB(const std_msgs::Float64::ConstPtr &msg, int index);
+
+  bool getActivationsCB(GetMuscleActivations::Request &req,
+                        GetMuscleActivations::Response &ret);
+
+  bool setActivationsCB(SetMuscleActivations::Request &req,
+                        SetMuscleActivations::Response &ret);
+
+  bool getMuscleStatesCB(GetMuscleStates::Request &req,
+                         GetMuscleStates::Response &ret);
+
+  // ROS node handle
+  std::unique_ptr<ros::NodeHandle> rosNode;
+
+  // Pointer to the model
+  physics::OpensimModelPtr m_model;
+
+  /// \brief Pointer to the physics instance
+  physics::OpensimPhysicsPtr m_engine;
+
+  // Pointer to the update event connection
+  event::ConnectionPtr m_updateConnection;
+
+  // ROS subscriber for individual muscle control topics
+  std::vector<ros::Subscriber> m_activation_sub_vec;
+
+  // ROS muscle state publisher
+  private: ros::Publisher m_muscle_states_pub, m_joint_pub;
+
+  // ROS muscle state message cache.
+  // Used to answer service callbacks and in transmission of topic messages.
+  private: MuscleStates m_muscle_states_msg;
+
+  // ROS Accessor services
+  private: ros::ServiceServer getMuscleActivationsService;
+
+  // ROS Accessor services
+  private: ros::ServiceServer setMuscleActivationsService;
+
+  // ROS Accessor services
+  private: ros::ServiceServer getMuscleStatesService;
+
+  // Get data from gazebo and fill m_msucle_states_msg with it.
+  private: void FillStateMessage();
+};
+
+} // namespace gazebo
+
+#endif

gazebo_ros_muscle_interface/msg/MuscleState.msg

@@ -0,0 +1,6 @@
+# muscle state
+string name
+float32 force
+float32 length
+float32 lengthening_speed
+geometry_msgs/Vector3[] path_points

gazebo_ros_muscle_interface/msg/MuscleStates.msg

@@ -0,0 +1,3 @@
+Header header
+# broadcast all muscle states in world frame
+MuscleState[] muscles

gazebo_ros_muscle_interface/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package>
+  <name>gazebo_ros_muscle_interface</name>
+  <version>0.0.1</version>
+  <description>The muscle controller plugin for the opensim muscle integration in gazebo.</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="welter@truphysics.com">Michael Welter</maintainer>
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>gazebo</build_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>gazebo_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>gazebo</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>gazebo_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

gazebo_ros_muscle_interface/sendactivation.py

@@ -0,0 +1,18 @@
+#! /usr/bin/env python2.7
+
+import sys
+import rospy
+import threading
+import time
+
+from std_msgs.msg import Float64
+
+rospy.init_node('sendactivation', anonymous=True)
+
+topic = sys.argv[1]
+activation = float(sys.argv[2])
+
+pub = rospy.Publisher('/gazebo_muscle_interface/'+topic+'/cmd_activation', Float64, queue_size=1, latch = True)
+pub.publish(Float64(data=activation))
+
+time.sleep(0.5)