Ok, here is what i found:
You can set the position and orientation for every Gazebo model with the service "setModelState", but if you really want to set the joints' angles of a model, you have to write a Gazebo Plugin as F.Brosseau has stated in his answer.
My solution is based on the following three webpages and some experimenting. It is probably a quite bad solution, but the only one I came up with in the last few hours. Feel free to use and improve it. I won't take any responsibility for it.
http://gazebosim.org/tutorials/?tut=plugins_hello_world
http://gazebosim.org/tutorials?tut=plugins_model&cat=write_plugin
http://gazebosim.org/tutorials?cat=guided_i&tut=guided_i6
The plugin will subscribe to a special named topic for every "revolute" joint it can find in the model. You can find the topic names with "rostopic list" after you started a model with this plugin. The topics consist only of a Float32 value, which correspond to the angle in [rad]. If you publish a topic with the joint name and an angle, the joint will be forced to this angle and hold in place.
CMakeLists.txt:
cmake_minimum_required(VERSION 2.8.3)
project(force_joint)
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)
find_package(gazebo REQUIRED)
include_directories(SYSTEM ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} src)
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS} -std=c++11")
add_library(force_joint SHARED src/force_joint.cpp)
target_link_libraries(force_joint ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES})
force_joint.cpp
#include <ros/ros.h>
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <gazebo/gazebo.hh>
#include <boost/bind.hpp>
#include <boost/algorithm/string.hpp>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <thread>
#include <memory>
#include <map>
namespace gazebo
{
class force_joint : public ModelPlugin
{
public:
force_joint() : ModelPlugin(){}
//read every joint of model and initialize ROS topics
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
this->model = _parent;
this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&force_joint::OnUpdate, this, _1));
physics::Joint_V jointVector = this->model->GetJoints();
// Initialize ros, if it has not already bee initialized.
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "gazebo_client",
ros::init_options::NoSigintHandler);
}
// Create our ROS node. This acts in a similar manner to the Gazebo node
this->rosNode.reset(new ros::NodeHandle("force_joints"));
// Create a named topic, and subscribe to it for every joint
for(physics::Joint_V::iterator jit=jointVector.begin(); jit!=jointVector.end(); ++jit)
{
//test if revolute joint ... i think ...
//if not, ignore joint
if((*jit)->GetType() != 576)
continue;
std::string modelName = this->model->GetName();
boost::algorithm::replace_all(modelName, " ", "_");
std::string jointName = (*jit)->GetName();
boost::algorithm::replace_all(jointName, " ", "_");
std::string subPath = modelName + "/" + jointName;
ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>(subPath,
1,
boost::bind(&force_joint::OnRosMsg, this, _1, (*jit)->GetName()),
ros::VoidPtr(), &this->rosQueue);
this->rosSubList.push_back(this->rosNode->subscribe(so));
joints.push_back((*jit)->GetName());
jointAngles[(*jit)->GetName()] = (*jit)->GetAngle(0).Radian();
}
// Spin up ...
(more)
