diff --git a/gazebo_ros_muscle_interface/CMakeLists.txt b/gazebo_ros_muscle_interface/CMakeLists.txt new file mode 100644 index 0000000..c404ba2 --- /dev/null +++ b/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) diff --git a/gazebo_ros_muscle_interface/README.txt b/gazebo_ros_muscle_interface/README.txt new file mode 100644 index 0000000..adc03c4 --- /dev/null +++ b/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 /sdf test_model + +3. start gazebo with your GazeboRosPackages workspace sourced: +cd +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" + diff --git a/gazebo_ros_muscle_interface/include/gazebo_ros_muscle_interface.h b/gazebo_ros_muscle_interface/include/gazebo_ros_muscle_interface.h new file mode 100644 index 0000000..da3c2f6 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Message headers are found in the install directory. +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +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 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 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 diff --git a/gazebo_ros_muscle_interface/msg/MuscleState.msg b/gazebo_ros_muscle_interface/msg/MuscleState.msg new file mode 100644 index 0000000..54c425e --- /dev/null +++ b/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 diff --git a/gazebo_ros_muscle_interface/msg/MuscleStates.msg b/gazebo_ros_muscle_interface/msg/MuscleStates.msg new file mode 100644 index 0000000..d0cf3fc --- /dev/null +++ b/gazebo_ros_muscle_interface/msg/MuscleStates.msg @@ -0,0 +1,3 @@ +Header header +# broadcast all muscle states in world frame +MuscleState[] muscles diff --git a/gazebo_ros_muscle_interface/package.xml b/gazebo_ros_muscle_interface/package.xml new file mode 100644 index 0000000..2eb24fb --- /dev/null +++ b/gazebo_ros_muscle_interface/package.xml @@ -0,0 +1,40 @@ + + + gazebo_ros_muscle_interface + 0.0.1 + The muscle controller plugin for the opensim muscle integration in gazebo. + + + + + Michael Welter + + + + + BSD + + catkin + tf + gazebo + gazebo_ros + gazebo_msgs + roscpp + std_msgs + tf + gazebo + gazebo_ros + gazebo_msgs + roscpp + std_msgs + + + + + + + + + + + diff --git a/gazebo_ros_muscle_interface/sendactivation.py b/gazebo_ros_muscle_interface/sendactivation.py new file mode 100644 index 0000000..cc47bf8 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/gazebo_ros_muscle_interface/src/gazebo_ros_muscle_interface.cpp b/gazebo_ros_muscle_interface/src/gazebo_ros_muscle_interface.cpp new file mode 100644 index 0000000..fc428dc --- /dev/null +++ b/gazebo_ros_muscle_interface/src/gazebo_ros_muscle_interface.cpp @@ -0,0 +1,268 @@ +#include "gazebo_ros_muscle_interface.h" + +#include +#include + +#include +#include +#include + +// Useful links: +// http://wiki.ros.org/roscpp/Overview +// http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29 + +namespace gazebo +{ + +////////////////////////////////////////////////// +MuscleInterfacePlugin::MuscleInterfacePlugin() +{ + //gzdbg << __FUNCTION__ << std::endl; + rosNode = std::make_unique("gazebo_muscle_interface"); +} + +////////////////////////////////////////////////// +MuscleInterfacePlugin::~MuscleInterfacePlugin() +{ + //gzdbg << __FUNCTION__ << std::endl; + rosNode->shutdown(); +} + +////////////////////////////////////////////////// +void MuscleInterfacePlugin::Init() +{ + gazebo::ModelPlugin::Init(); + if (!m_model || !m_engine) return; + + const auto &muscles = m_model->GetMuscles(); + for (int k = 0; k < muscles.size(); ++k) + { + auto muscle = muscles[k]; + const std::string name = muscle->GetName(); + + // generate topic name using the model name as prefix + std::string topic_name = m_model->GetName() + "/" + name + "/cmd_activation"; + + // Add ROS topic for activation control + m_activation_sub_vec.push_back( + rosNode->subscribe( + topic_name, + 1, + boost::bind(&MuscleInterfacePlugin::activationCB, this, _1, k))); + + ROS_INFO("Added new topic (%s) for muscle %s (%p)", topic_name.c_str(), name.c_str(), muscle.get()); + } + + FillStateMessage(); + + const std::string topic_name = m_model->GetName() + "/muscle_states"; + m_muscle_states_pub = rosNode->advertise( topic_name, 10 ); + + m_joint_pub = rosNode->advertise("/jointState", 10); + + getMuscleActivationsService = rosNode->advertiseService< + GetMuscleActivations::Request, + GetMuscleActivations::Response>( + m_model->GetName() + "/get_activations", + boost::bind(&MuscleInterfacePlugin::getActivationsCB, this, _1, _2) + ); + ROS_INFO("ROS muscle interface: get_activations service installed!"); + + setMuscleActivationsService = rosNode->advertiseService< + SetMuscleActivations::Request, + SetMuscleActivations::Response>( + m_model->GetName() + "/set_activations", + boost::bind(&MuscleInterfacePlugin::setActivationsCB, this, _1, _2) + ); + ROS_INFO("ROS muscle interface: set_activations service installed!"); + + getMuscleStatesService = rosNode->advertiseService< + GetMuscleStates::Request, + GetMuscleStates::Response>( + m_model->GetName() + "/get_states", + boost::bind(&MuscleInterfacePlugin::getMuscleStatesCB, this, _1, _2) + ); + ROS_INFO("ROS muscle interface: get_states service installed!"); + + // Register callback that sends muscle messages with gazebo server. + m_updateConnection = event::Events::ConnectWorldUpdateEnd(boost::bind(&MuscleInterfacePlugin::OnUpdateEnd, this)); + + //gzdbg << __FUNCTION__ << " in thread " << std::this_thread::get_id() << std::endl; +} +////////////////////////////////////////////////// +void MuscleInterfacePlugin::Reset() +{ +} + +////////////////////////////////////////////////// +void MuscleInterfacePlugin::Load(physics::ModelPtr parent, sdf::ElementPtr sdf) +{ + // Store the pointer to the model + m_model = boost::dynamic_pointer_cast(parent); + if (m_model) + { + physics::WorldPtr world = m_model->GetWorld(); + if (world) + m_engine = boost::dynamic_pointer_cast(world->GetPhysicsEngine()); + } + if (m_model == nullptr || m_engine == nullptr) + gzwarn << "Muscle control plugin loaded without OpenSim physics plugin." << std::endl; +} + +////////////////////////////////////////////////// +/* Called by the world update start event. Runs in same thread as + * physics::UpdatePhysics after UpdatePhysics. Thus it returns most + * up to date information. +*/ +void MuscleInterfacePlugin::OnUpdateEnd(/*const common::UpdateInfo & _info*/) +{ + FillStateMessage(); + m_muscle_states_pub.publish ( m_muscle_states_msg ); + //physics::Joint_V joints = m_model->GetJoints(); + //for (auto joint:m_model->GetJoints()){ + //} + + sensor_msgs::JointState msg; + msg.position.push_back(m_model->GetJoint("HumerusBoneJoint")->GetAngle(0).Radian()); + msg.position.push_back(m_model->GetJoint("RadiusBoneJoint")->GetAngle(0).Radian()); + msg.velocity.push_back(m_model->GetJoint("HumerusBoneJoint")->GetVelocity(0)); + msg.velocity.push_back(m_model->GetJoint("RadiusBoneJoint")->GetVelocity(0)); + + + m_joint_pub.publish(msg); +} + +////////////////////////////////////////////////// +void MuscleInterfacePlugin::FillStateMessage() +{ + // Sync via global physics mutex. Needed because + // we use the msg member to answer the "get states" + // service. + boost::recursive_mutex::scoped_lock lock( + *m_engine->GetPhysicsUpdateMutex()); + + MuscleStates &msg = m_muscle_states_msg; + + gazebo::common::Time curTime = this->m_model->GetWorld()->GetSimTime(); + msg.header.stamp.sec = curTime.sec; + msg.header.stamp.nsec = curTime.nsec; + + std::vector muscle_path; + + const physics::Muscle_V muscles = m_model->GetMuscles(); + msg.muscles.resize(muscles.size()); + for (int muscle_idx = 0; muscle_idx < muscles.size(); ++muscle_idx) + { + MuscleState &state_msg = msg.muscles[muscle_idx]; + const physics::OpensimMuscle& muscle = *muscles[muscle_idx]; + + state_msg.name = muscle.GetName(); + state_msg.force = muscle.GetForce(); + state_msg.length = muscle.GetLength(); + state_msg.lengthening_speed = muscle.GetLengtheningSpeed(); + // TODO: Constant parameters should perhaps become available upon + // request by a service rather than being transmitted every update. + //state_msg.optimal_fiber_length = muscle.GetOptimalFiberLength(); + //state_msg.tendon_slack_length = muscle.GetTendonSlackLength(); + + muscle.GetCurrentWorldPath(muscle_path); + state_msg.path_points.resize(muscle_path.size()); + for (std::size_t i=0; iGetMuscles(); + if (index < 0 || index >= muscles.size()) + { + gzdbg << "Ignored attempt to set muscle activation by invalid index!" << std::endl; + return; + } + + // Protect setter function which is not thread safe at the moment. + // We don't want to set the activation in a thread while the physics + // update loop reads it for the next physics iteration. + boost::recursive_mutex::scoped_lock lock( + *m_engine->GetPhysicsUpdateMutex()); + + muscles[index]->SetActivationValue(msg->data); +} + +//////////////////////////////////////// ROS service callback functions ////////////////////////////////////////// +bool MuscleInterfacePlugin::getActivationsCB( + GetMuscleActivations::Request &req, + GetMuscleActivations::Response &ret) +{ + const physics::Muscle_V &muscles = m_model->GetMuscles(); + ret.activations.resize(muscles.size()); + + // Muscle members are not thread safe. This lock is + // not super critical but still we want to avoid + // setting and reading activations concurrently. + boost::recursive_mutex::scoped_lock lock( + *m_engine->GetPhysicsUpdateMutex()); + + for (std::size_t i = 0; i < muscles.size(); ++i) + ret.activations[i] = muscles[i]->GetActivationValue(); + + ret.success = true; + return true; +} + +//////////////////////////////////////// ROS service callback functions ////////////////////////////////////////// +bool MuscleInterfacePlugin::setActivationsCB( + SetMuscleActivations::Request &req, + SetMuscleActivations::Response &ret) +{ + const physics::Muscle_V &muscles = m_model->GetMuscles(); + + if (req.activations.size() != muscles.size()) + { + gzdbg << "Ignored attempt to set activations of " << muscles.size() << " muscles with an array of " << req.activations.size() << " values!" << std::endl; + ret.success = false; + return false; + } + + // See above. + boost::recursive_mutex::scoped_lock lock( + *m_engine->GetPhysicsUpdateMutex()); + + for (std::size_t i = 0; i < muscles.size(); ++i) + muscles[i]->SetActivationValue(req.activations[i]); + + ret.success = true; + return true; +} + +//////////////////////////////////////// ROS service callback functions ////////////////////////////////////////// +bool MuscleInterfacePlugin::getMuscleStatesCB( + GetMuscleStates::Request &req, + GetMuscleStates::Response &ret) +{ + boost::recursive_mutex::scoped_lock lock( + *m_engine->GetPhysicsUpdateMutex()); + + // Copy muscle state from the "cached" data to the response. + MuscleStates &m = m_muscle_states_msg; + ret.muscles.reserve(m.muscles.size()); + + for (std::size_t i = 0; i < m.muscles.size(); ++i) + ret.muscles.emplace_back(m.muscles[i]); + + return true; +} + +//////////////////////////////////////// + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(MuscleInterfacePlugin) + +} // namespace gazebo diff --git a/gazebo_ros_muscle_interface/srv/GetMuscleActivations.srv b/gazebo_ros_muscle_interface/srv/GetMuscleActivations.srv new file mode 100644 index 0000000..1c56f8b --- /dev/null +++ b/gazebo_ros_muscle_interface/srv/GetMuscleActivations.srv @@ -0,0 +1,4 @@ + +--- +float64[] activations +bool success diff --git a/gazebo_ros_muscle_interface/srv/GetMuscleStates.srv b/gazebo_ros_muscle_interface/srv/GetMuscleStates.srv new file mode 100644 index 0000000..d3e5b7f --- /dev/null +++ b/gazebo_ros_muscle_interface/srv/GetMuscleStates.srv @@ -0,0 +1,2 @@ +--- +MuscleState[] muscles diff --git a/gazebo_ros_muscle_interface/srv/SetMuscleActivations.srv b/gazebo_ros_muscle_interface/srv/SetMuscleActivations.srv new file mode 100644 index 0000000..d392ff6 --- /dev/null +++ b/gazebo_ros_muscle_interface/srv/SetMuscleActivations.srv @@ -0,0 +1,3 @@ +float64[] activations +--- +bool success diff --git a/model.sdf b/model.sdf new file mode 100644 index 0000000..da734bf --- /dev/null +++ b/model.sdf @@ -0,0 +1,152 @@ + + + + model://myoarm_nst/muscles.osim + + 0.0 0.0 0.0 1.5707963705062866 -0.0 0.0 + + 1.0 + 0.0 0.0 0.0 3.141592502593994 -0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + + + 0.0 0.506493091583252 -8.25181984964729e-08 -1.5707964897155762 -0.0 0.0 + 0.0 0.0 0.0 3.141592502593994 -0.0 0.0 + + + model://myoarm_nst/meshes/visual/VIS_table.dae + 1.0 1.0 1.0 + + + + + + 0.0 -6.40229519012e-06 1.00498998165 1.5707963705062866 -0.0 0.0 + + 20.6792 + 0.0 -0.004989981651306152 -6.402076905942522e-06 -1.5707963705062866 -0.0 0.0 + + 0.1935 + 0.0 + 0.0002 + 0.9382 + -0.0005 + 0.7965 + + + + 0.0 1.1920957376787555e-07 -1.0658141036401503e-14 -6.245003835890148e-14 -0.0 0.0 + -6.141581252450123e-05 -1.0339559316635132 1.0052673816680908 3.141592502593994 -0.0 0.0 + + + model://myoarm_nst/meshes/visual/VIS_HoldingFrame.dae + 0.0010000000474974513 0.0010000000474974513 0.0010000000474974513 + + + + + + -0.000539999979082 -0.0168567138839 1.053999874 1.5707963705062866 -0.0 0.0 + + 2.3542 + 0.000539999979082495 0.20000038146973 -0.016856778413057327 -1.5707963705062866 -0.0 0.0 + + 0.4991 + -0.316 + 0.041 + 0.2367 + 0.0264 + 0.7157 + + + + -5.54544385522604e-07 1.1994612236776447e-07 6.082260028961173e-09 -2.265664136302803e-07 3.1310214865243324e-08 0.5759587287902832 + 0.9980682134628296 -0.8001264333724976 1.0325278043746948 3.141592502593994 0.12217305600643158 0.698131799697876 + + + model://myoarm_nst/meshes/visual/VIS_HumerusBone.dae + 0.0010000000474974513 0.0010000000474974513 0.0010000000474974513 + + + + + + -0.000539999979082 0.0186109776662 1.4839998827 1.5707963705062866 -0.0 0.0 + + 1.5068 + 0.0005399999208748341 0.20001277923584 0.018610898405313492 -1.5707963705062866 -0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + + + 1.607113517820835e-07 1.183957749617548e-07 7.775454946568061e-09 -2.709087539187749e-06 -1.9988910082702205e-07 -0.9948384165763855 + 1.770756483078003 -0.3644533157348633 0.20604144036769867 1.7902381159728975e-06 1.4486231803894043 -2.4434592723846436 + + + model://myoarm_nst/meshes/visual/VIS_RadiusBone.dae + 0.0009999999310821295 0.0010000000474974513 0.0010000000474974513 + + + + + + table + HoldingFrame + + 0.0 1.0 0.0 + true + + + + + HoldingFrame + HumerusBone + + 0.0 1.0 0.0 + true + + -0.8 + 0.8 + + + 0.7 + + + + + + HumerusBone + RadiusBone + + 0.0 1.0 0.0 + true + + -0.7 + 0.7 + + + 0.7 + + + + + world + table + + + + + diff --git a/myoarm_nst_rl/Myo_NST.bibi b/myoarm_nst_rl/Myo_NST.bibi new file mode 100644 index 0000000..d1087d0 --- /dev/null +++ b/myoarm_nst_rl/Myo_NST.bibi @@ -0,0 +1,17 @@ + + + + brain_model/idle_brain.py + + + + + + + myoarm_nst/model.sdf + + + + + + diff --git a/myoarm_nst_rl/Myo_NST.exc b/myoarm_nst_rl/Myo_NST.exc new file mode 100644 index 0000000..f074520 --- /dev/null +++ b/myoarm_nst_rl/Myo_NST.exc @@ -0,0 +1,30 @@ + + + 2 DOF NST Myorobotics Arm for Deep RL Experiment + Myo_NST.jpg + Loads the 2 DOF Myorobotics Arm into the Holodeck. Muscles can be controlled via ROS messages. + 1000000000 + + + + production + + + + + + + + + + + + opensim + + 1.e-12 + 1.e-12 + 0.02 + + diff --git a/myoarm_nst_rl/Myo_NST.jpg b/myoarm_nst_rl/Myo_NST.jpg new file mode 100644 index 0000000..14c7fd8 Binary files /dev/null and b/myoarm_nst_rl/Myo_NST.jpg differ diff --git a/myoarm_nst_rl/controller.py b/myoarm_nst_rl/controller.py new file mode 100644 index 0000000..fe81012 --- /dev/null +++ b/myoarm_nst_rl/controller.py @@ -0,0 +1,64 @@ +""" +This module contains the transfer function which is responsible for determining the muscle movements of the myoarm +""" +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64 +@nrp.MapVariable("agent", initial_value=None, scope=nrp.GLOBAL) + +#muscles +@nrp.MapRobotPublisher('lbm', Topic('/gazebo_muscle_interface/robot/left_bottom/cmd_activation', Float64)) +@nrp.MapRobotPublisher('ltm', Topic('/gazebo_muscle_interface/robot/left_top/cmd_activation', Float64)) +@nrp.MapRobotPublisher('rbm', Topic('/gazebo_muscle_interface/robot/right_bottom/cmd_activation', Float64)) +@nrp.MapRobotPublisher('rtm', Topic('/gazebo_muscle_interface/robot/right_top/cmd_activation', Float64)) + +@nrp.MapRobotSubscriber('joints', Topic('/jointState', JointState)) + +@nrp.Neuron2Robot() +def controller(t, agent, lbm, ltm, rbm, rtm, joints): + if agent.value is not None: + clientLogger.info("FORWARD PASS") + import math + import numpy as np + + angle_lower = joints.value.position[0] + angle_vel_lower = joints.value.velocity[0] + angle_upper = joints.value.position[1] + angle_vel_upper = joints.value.velocity[1] + + clientLogger.info('humerus_angle ', angle_lower) + clientLogger.info('humerus_ang_vel ', angle_vel_lower) + clientLogger.info('radius_angle ', angle_upper) + clientLogger.info('radius_ang_vel ', angle_vel_lower) + + observation = np.array([math.cos(angle_lower), math.sin(angle_lower), + angle_vel_lower, math.cos(angle_upper), + math.sin(angle_upper), angle_vel_upper]) + + # get movement action from agent and publish to robot + action = agent.value.forward(observation) + + clientLogger.info('lbm ', action[0]) + clientLogger.info('ltm ', action[1]) + clientLogger.info('rbm ', action[2]) + clientLogger.info('rtm ', action[3]) + lbm.send_message(std_msgs.msg.Float64(action[0])) + ltm.send_message(std_msgs.msg.Float64(action[1])) + rbm.send_message(std_msgs.msg.Float64(action[2])) + rtm.send_message(std_msgs.msg.Float64(action[3])) + + + reward = -(angle_lower**2 + 0.1*angle_vel_lower**2 + + angle_upper**2 + 0.1*angle_vel_upper**2 + + 0.001*np.sum(np.power(action, 2))) + + #learn from the reward + agent.value.backward(reward) + agent.value.step = agent.value.step + 1 + + clientLogger.info('BACKWARD PASS, step ', agent.value.step) + clientLogger.info('Amount of reward ', reward) + + if agent.value.step%20 == 0: + clientLogger.info('saving weights') + PATH = '/home/akshay/Documents/NRP/Experiments/myoarm_nst_rl/ddpg_weights.h5' + agent.value.save_weights(PATH, overwrite=True) diff --git a/myoarm_nst_rl/ddpg_weights_actor.h5 b/myoarm_nst_rl/ddpg_weights_actor.h5 new file mode 100644 index 0000000..7b045f5 Binary files /dev/null and b/myoarm_nst_rl/ddpg_weights_actor.h5 differ diff --git a/myoarm_nst_rl/ddpg_weights_critic.h5 b/myoarm_nst_rl/ddpg_weights_critic.h5 new file mode 100644 index 0000000..ee2a529 Binary files /dev/null and b/myoarm_nst_rl/ddpg_weights_critic.h5 differ diff --git a/myoarm_nst_rl/holodeck.3ds b/myoarm_nst_rl/holodeck.3ds new file mode 100644 index 0000000..8cab4d7 --- /dev/null +++ b/myoarm_nst_rl/holodeck.3ds @@ -0,0 +1,98 @@ +{ + "dynamicEnvMap": true, + "shadows": true, + "antiAliasing": true, + "ssao": false, + "ssaoDisplay": false, + "ssaoClamp": "0.94", + "ssaoLumInfluence": "1", + "rgbCurve": { + "red": [ + [ + 0, + 0 + ], + [ + 0.410400390625, + 0.4481201171875 + ], + [ + 0.761962890625, + 0.7957763671875 + ], + [ + 1, + 1 + ] + ], + "green": [ + [ + 0, + 0 + ], + [ + 0.261962890625, + 0.2801513671875 + ], + [ + 0.718994140625, + 0.7371826171875 + ], + [ + 1, + 1 + ] + ], + "blue": [ + [ + 0, + 0 + ], + [ + 0.265869140625, + 0.2606201171875 + ], + [ + 0.840087890625, + 0.7645263671875 + ], + [ + 1, + 1 + ] + ] + }, + "levelsInBlack": "0.09", + "levelsInGamma": 0.52, + "levelsInWhite": "0.8", + "levelsOutBlack": 0, + "levelsOutWhite": 1, + "skyBox": "forest", + "sun": "", + "bloom": false, + "bloomStrength": "0.83", + "bloomRadius": "0.29", + "bloomThreshold": "0.91", + "fog": true, + "fogDensity": "0.02", + "fogColor": "#97a2af", + "pbrMaterial": true, + "defaultCameraMode":"lookatrobot", + "nearClippingDistance": "0.01", + "shadowSettings": [ + { + "lightName": "sun", + "mapSize": 4096, + "cameraBottom": -10, + "cameraLeft": -10, + "cameraRight": 10, + "cameraTop": 15, + "bias": 0.0001, + "radius": 1.2 + } + ], + "cameraSensitivity": { + "translation": 0.1, + "rotation": 1.0 + } +} diff --git a/myoarm_nst_rl/holodeck.uis b/myoarm_nst_rl/holodeck.uis new file mode 100644 index 0000000..cca085f --- /dev/null +++ b/myoarm_nst_rl/holodeck.uis @@ -0,0 +1,9 @@ +{ + "camera": { + "defaultMode": "free-camera", + "sensitivity": { + "translation": 1.0, + "rotation": 1.0 + } + } +} diff --git a/myoarm_nst_rl/init_DRLagent.py b/myoarm_nst_rl/init_DRLagent.py new file mode 100644 index 0000000..30a3758 --- /dev/null +++ b/myoarm_nst_rl/init_DRLagent.py @@ -0,0 +1,70 @@ +# internal keras-rl agent to persist +@nrp.MapVariable("agent", initial_value=None, scope=nrp.GLOBAL) +@nrp.Robot2Neuron() +def init_DRLagent(t, agent): + # initialize the keras-rl agent + if agent.value is None: + # import keras-rl in NRP through virtual env + import site, os + site.addsitedir(os.path.expanduser('~/.opt/tensorflow_venv/lib/python2.7/site-packages')) + from keras.models import Model, Sequential + from keras.layers import Dense, Activation, Flatten, Input, concatenate + from keras.optimizers import Adam, RMSprop + from rl.agents import DDPGAgent + from rl.memory import SequentialMemory + from rl.random import OrnsteinUhlenbeckProcess + + from keras import backend as K + + K.clear_session() + + obs_shape = (6,) + + nb_actions = 4 + + clientLogger.info('INIT AGENT') + + clientLogger.info('obs_shape', obs_shape) + + # create the nets for rl agent + # actor net + actor = Sequential() + actor.add(Flatten(input_shape=(1,) + obs_shape)) + actor.add(Dense(32)) + actor.add(Activation('relu')) + actor.add(Dense(32)) + actor.add(Activation('relu')) + actor.add(Dense(32)) + actor.add(Activation('relu')) + actor.add(Dense(nb_actions)) + actor.add(Activation('sigmoid')) + + # critic net + action_input = Input(shape=(nb_actions,), name='action_input') + observation_input = Input(shape=(1,) + obs_shape, name='observation_input') + flattened_observation = Flatten()(observation_input) + x = concatenate([action_input, flattened_observation]) + x = Dense(64)(x) + x = Activation('relu')(x) + x = Dense(64)(x) + x = Activation('relu')(x) + x = Dense(64)(x) + x = Activation('relu')(x) + x = Dense(1)(x) + x = Activation('linear')(x) + critic = Model(inputs=[action_input, observation_input], outputs=x) + + + # instanstiate rl agent + memory = SequentialMemory(limit=1000, window_length=1) + random_process = OrnsteinUhlenbeckProcess(theta=.15, mu=0., sigma=.2, size=nb_actions) + agent.value = DDPGAgent(nb_actions=nb_actions, actor=actor, critic=critic, critic_action_input=action_input, memory=memory, nb_steps_warmup_critic=10, nb_steps_warmup_actor=10, random_process=random_process, gamma=.99, batch_size=5, target_model_update=1e-3, delta_clip=1.) + agent.value.training = True + + PATH = '/home/akshay/Documents/NRP/Experiments/myoarm_nst_rl/ddpg_weights.h5' + if os.path.isfile(PATH): + print('loading weights') + agent.value.load_weights(PATH) + + agent.value.compile(Adam(lr=.001, clipnorm=1.), metrics=['mae']) + diff --git a/myoarm_nst_rl/rqt_plot.py b/myoarm_nst_rl/rqt_plot.py new file mode 100644 index 0000000..5f6ed9a --- /dev/null +++ b/myoarm_nst_rl/rqt_plot.py @@ -0,0 +1,12 @@ +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64 +from gazebo_msgs.msg import LinkStates +@nrp.MapRobotSubscriber('joints', Topic('/jointState', JointState)) + +@nrp.MapRobotPublisher('x', Topic('/x', Float64)) +@nrp.Robot2Neuron() +def transferfunction_0(t, joints, x): + # log the first timestep (20ms), each couple of seconds + if t % 2 < 0.02: + clientLogger.info('Time: ', t) + x.send_message(std_msgs.msg.Float64(joints.value.position[0]))