-
Notifications
You must be signed in to change notification settings - Fork 1
Developing a Controller
Introduction to Creating a Package
This tutorial will describe developing Reveal scenario interfaced with Gazebo that simulates the dynamics of a simple pendulum acting solely under the influence of gravity. The pendulum has a frictionless pivot, a massless rod and a massive bob. The pendulum is started at an angle where the rod is orthogonal to the axis drawn between its equilibrium positions, i.e. if the pendulum was a clock the bob would start at 3 o'clock. As the scenario evolves the pendulum should oscillate back and forth subject only to the force of gravity.
A controller is not strictly necessary for this scenario, but the simple pendulum does provide enough of a basis to fully describe the requirements and process necessary to create a Reveal scenario. In the case of the controller, the control code will still read state and compute null controls to demonstrate data exchange across the Gazebo simulator-Reveal controller interface.
For a complete reference to the following code refer to the Pendulum Scenario in the Reveal Packages repository.
Following the prescription provided in the Introduction, the pendulum package requires the following file structure:
<root-package-path>/
|-pendulum/
|--scenario/
|---CMakeLists.txt
|---controller.h
|---models/
|----sdf/
|-----reveal_pendulum/
|------model.config
|------model.sdf
|---gazebo/
|----CMakeLists.txt
|----gazebo.world
|----reveal.world
|----plugin.cpp
|----manifest.xml
|--analyzers/
After cloning the Reveal Packages Repositiory, navigate to the location to which it was cloned. There you will see a directory titled pendulum which is the <package-directory> and the file structure will correspond with the above skeleton.
The first step is to develop the scenario definition and models. The following is an example sdf model definition for a frictionless pendulum. This file is named model.sdf and is stored in the models\sdf\reveal_pendulum directory along with its associated model.config file.
<?xml version="1.0" ?>
<sdf version='1.5'>
<model name='pendulum'>
<link name='base_link'>
<visual name='base_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</visual>
</link>
<link name='arm_link'>
<pose>0 0 0 0 0 0</pose>
<visual name='arm_link_visual'>
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>1.0</length>
</cylinder>
</geometry>
</visual>
</link>
<joint name='pivot_joint' type='revolute'>
<child>arm_link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-6.28319</lower>
<upper>6.28319</upper>
</limit>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='bob_link'>
<pose>0 0 -1.0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.0004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0004</iyy>
<iyz>0</iyz>
<izz>0.0004</izz>
</inertia>
</inertial>
<visual name='bob_link_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.1</radius>
</sphere>
</geometry>
</visual>
</link>
<joint name='arm_bob_weld' type='revolute'>
<child>bob_link</child>
<parent>arm_link</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name='world_base_weld' type='revolute'>
<child>base_link</child>
<parent>world</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
</axis>
</joint>
</model>
</sdf>
For detailed information on the sdf format, refer to sdformat.org and gazebosim.org
In addition to model definitions, a world configuration needs to be defined. It is advised to create two world configuration files, one necessary to interface with Reveal and the other for scenario testing and possibly data generation. The following is the example gazebo.world configuration file for testing with Gazebo only.
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="pendulum-scenario">
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<max_contacts>20</max_contacts>
<simbody>
<min_step_size>0.0001</min_step_size>
<accuracy>0.001</accuracy>
<max_transient_velocity>0.01</max_transient_velocity>
<contact>
<stiffness>1e8</stiffness>
<dissipation>100</dissipation>
<plastic_coef_restitution>0.5</plastic_coef_restitution>
<plastic_impact_velocity>0.5</plastic_impact_velocity>
<static_friction>100</static_friction>
<dynamic_friction>100</dynamic_friction>
<viscous_friction>0</viscous_friction>
<override_impact_capture_velocity>0.001</override_impact_capture_velocity>
<override_stiction_transition_velocity>0.001</override_stiction_transition_velocity>
</contact>
</simbody>
<bullet>
<solver>
<type>sequential_impulse</type>
<min_step_size>0.0001</min_step_size>
<iters>50</iters>
<sor>1.3</sor>
</solver>
<constraints>
<cfm>0.0</cfm>
<erp>0.2</erp>
<contact_surface_layer>0.001</contact_surface_layer>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
</constraints>
</bullet>
<ode>
<solver>
<type>quick</type>
<min_step_size>0.0001</min_step_size>
<iters>500</iters>
<sor>1.0</sor>
</solver>
<constraints>
<cfm>0.0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
</physics>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://reveal_pendulum</uri>
<pose>0 0 0 1.57079632679 0 0</pose>
<plugin name='controller' filename='libgz-plugin.so'/>
</include>
<gui>
<camera name='user_camera'>
<pose>4 0 0 0 0 3.1416</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
</world>
</sdf>
The world configuration includes references to model definitions and specific settings for the available dynamics engines.
To make a Reveal compatible world-definition file, copy the gazebo.world file to another file named reveal.world and add an additional reference to the Reveal monitor plugin within the world element following the <gui> element. The Reveal monitor should be provided with the models in a comma separated list that will be manipulated and recorded by Reveal. It is not necessary to understand how the monitor works for scenario development beyond providing the relevant models. For Gazebo, the reveal.world file drives the simulation, so it must be present for a scenario to operate properly in Reveal.
...
</gui>
<plugin name="world-plugin" filename="libreveal-gz-monitor.so">
<models>pendulum</models>
</plugin>
The controller itself is defined to be simulator independent, so that as new simulators are interfaced with the scenario the controller code can be easily reused. This helps prevent introducing non-standard approaches by developers adding in support for new simulators.
The controller implementation is simulator agnostic and so has no references to any simulator. Data is exchanged between the controller and the simulator-scenario interface through maps. The map keys coincide with the link and joint names defined in the model definitions.
#include <string>
#include <map>
//-----------------------------------------------------------------------------
void get_initial_velocity( std::map<std::string,double>& velocity ) {
velocity.insert( std::pair<std::string,double>("pivot_joint", 0.0) );
}
//-----------------------------------------------------------------------------
void get_control( double t, std::map<std::string,double> position, std::map<std::string,double> velocity, std::map<std::string, double>& force ) {
double q = position.find( "pivot_joint" )->second;
double dq = velocity.find( "pivot_joint" )->second;
force.insert( std::pair<std::string,double>( "pivot_joint", 0.0 ) );
}
The get_initial_velocity(...) function returns the starting velocities of the system joints through the velocity map parameter. Joints in the system are mapped using the joint name defined in the model configuration.
The get_control(...) function reads any relevant states from the position and velocity maps, computes a control, assigns the control to the force map given the current virtual time t. The simulator-scenario implementation is responsible for reading state from the simulator, assigning states to the correct map, calling get_control(...), and reading any computed force from the returned map.
A cmake build script must be defined in the scenario subdirectory. This build script will primarily copy necessary configuration files and redirect building of the necessary simulator-scenario interface to the appropriate subdirectory.
cmake_minimum_required(VERSION 2.8.7)
#------------------------------------------------------------------------------
option( GAZEBO "compile package with Gazebo support" ON )
#------------------------------------------------------------------------------
SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR})
SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR})
file( COPY ${CMAKE_CURRENT_SOURCE_DIR}/models DESTINATION ${CMAKE_BINARY_DIR} )
if( GAZEBO )
add_subdirectory( "gazebo" )
endif()
In Gazebo, controllers interact with the simulator through the ModelPlugin interface. Referring back to the earlier gazebo.world definition, the model plugin is referenced in the <plugin> element of the reveal_pendulum reference:
<include>
<uri>model://reveal_pendulum</uri>
<pose>0 0 0 1.57079632679 0 0</pose>
<plugin name='controller' filename='libgz-plugin.so'/>
</include>
Therefore, for this world file to load the correct model, a valid model definition for reveal_pendulum needs to be found in the path specified by the GAZEBO_MODEL_PATH environment variable and a compatible, compiled library named libgz-plugin.so needs to be found in the path specified by the GAZEBO_PLUGIN_PATH environment variable. In this example, libgz-plugin.so is implemented in plugin.cpp which is located in the gazebo subdirectory.
The simulator-scenario+controller interface is bridged to Gazebo using gazebo::ModelPlugin interface. The majority of code in the plugin will read state from the simulator, map state into the controller, map forces out of the controller, and set forces in the simulator.
Below is the plugin.cpp implementation.
#include <gazebo/gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <controller.h>
//-----------------------------------------------------------------------------
namespace gazebo {
//-----------------------------------------------------------------------------
class plugin_c : public ModelPlugin {
private:
event::ConnectionPtr _preupdateConnection; //< controls applied before update
physics::WorldPtr _world; //< to query world state
physics::ModelPtr _model; //< to query model state
physics::JointPtr _pivot; //< to get state and set controls of pivot joint
public:
//---------------------------------------------------------------------------
/// Default constructor
plugin_c( void ) { }
//---------------------------------------------------------------------------
/// Destructor
virtual ~plugin_c( void ) {
// unregister the update callback
event::Events::DisconnectWorldUpdateBegin( _preupdateConnection );
}
//---------------------------------------------------------------------------
/// validates that the scenario loaded matches the references expected and
/// sets any link and joint references for ease of use later
/// @return true if validation succeeded OR false if validation failed
bool validate( void ) {
// locate the pivot joint in the model
_pivot = _model->GetJoint( "pivot_joint" );
// if any of the pointers are nothing then validation fails
if( !( _world && _model && _pivot ) ) return false;
// otherwise the configuration is valid
return true;
}
//---------------------------------------------------------------------------
/// Initializes the controller. Fulfills the gazebo ModelPlugin interface
virtual void Load( physics::ModelPtr model, sdf::ElementPtr sdf ) {
// get model and world references
_model = model;
_world = model->GetWorld();
// validate the configuration
if( !validate( ) ) {
std::cerr << "Unable to validate model in plugin" << std::endl;
std::cerr << "ERROR: Plugin failed to load" << std::endl;
return;
}
// get the starting velocity for the joints from the controller
std::map<std::string, double> dq;
get_initial_velocity( dq );
// set the starting velocity for the joints from the controller data
_pivot->SetVelocity( 0, dq.find("pivot_joint")->second );
// register the gazebo callback
_preupdateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind( &plugin_c::Preupdate, this ) );
// -- FIN --
std::cerr << "controller has initialized" << std::endl;
}
//---------------------------------------------------------------------------
/// Registered as the Gazebo WorldUpdateBegin function for the ModelPlugin.
/// Gets current state from the updated model, computes controls by calling
/// the controller, and applies those controls to corresponding joints.
virtual void Preupdate( ) {
double t = _world->GetSimTime().Double(); // get the current virtual time
std::map<std::string, double> q; // map for position state
std::map<std::string, double> dq; // map for velocity state
std::map<std::string, double> u; // map for controls
// map in the positions relevant to control code
q.insert( std::pair<std::string,double>("pivot_joint", _pivot->GetAngle(0).Radian() ) );
// map in the velocities relevant to control code
dq.insert( std::pair<std::string,double>("pivot_joint", _pivot->GetVelocity(0) ) );
// let controller compute controls
get_control( t, q, dq, u );
// apply the forces
_pivot->SetForce( 0, u.find("pivot_joint")->second );
}
};
GZ_REGISTER_MODEL_PLUGIN( plugin_c )
//-----------------------------------------------------------------------------
} // namespace gazebo
//-----------------------------------------------------------------------------
The manifest.xml file enables the Reveal build system to identify all build products required for a properly functioning scenario. The following is the manifest for the pendulum scenario:
<?xml version="1.0" ?>
<Reveal>
<Manifest>
<Gazebo>
<World file="reveal.world" />
<Product file="libgz-plugin.so" type="controller" />
</Gazebo>
</Manifest>
</Reveal>
If multiple libraries are required to execute the scenario, each should be listed individually in its own Product element and must include the file tag.
A cmake build script must be provided in the gazebo subdirectory to handle building the plugin. If you note in the scenario level directory CMakeLists.txt directs the build process into this directory. The following build script handles finding all the requirements to build plugin.cpp into libgz-plugin.so which can be used with both standalone Gazebo and Reveal wrapped around Gazebo:
cmake_minimum_required(VERSION 2.8.7)
set( COMPONENT_NAME "controller" )
project( "reveal-package-pendulum-gazebo-${COMPONENT_NAME}" )
#------------------------------------------------------------------------------
include_directories( .. )
include( FindPkgConfig )
if( PKG_CONFIG_FOUND )
pkg_check_modules( GAZEBO REQUIRED gazebo )
endif()
include_directories( ${GAZEBO_INCLUDE_DIRS} )
link_directories( ${GAZEBO_LIBRARY_DIRS} )
#------------------------------------------------------------------------------
set( LIBS
${GAZEBO_LIBRARIES}
)
#------------------------------------------------------------------------------
add_library( gz-plugin SHARED plugin.cpp )
target_link_libraries( gz-plugin ${LIBS} )
#------------------------------------------------------------------------------
# Copy data files
file( COPY ${CMAKE_CURRENT_SOURCE_DIR}/manifest.xml DESTINATION ${CMAKE_CURRENT_BINARY_DIR} )
file( COPY ${CMAKE_CURRENT_SOURCE_DIR}/gazebo.world DESTINATION ${CMAKE_CURRENT_BINARY_DIR} )
file( COPY ${CMAKE_CURRENT_SOURCE_DIR}/reveal.world DESTINATION ${CMAKE_CURRENT_BINARY_DIR} )
Once all of the above requirements are met, create a build directory in the scenario directory then execute the following commands:
cd build
cmake ..
make
If libgz-plugin.so is successfully built, execute the following commands from the build directory and the pendulum scenario should run in standalone Gazebo:
export GAZEBO_MODEL_PATH=$PWD/models/sdf:$GAZEBO_MODEL_PATH
export GAZEBO_PLUGIN_PATH=$PWD:$GAZEBO_PLUGIN_PATH
gazebo gazebo/gazebo.world
Continue with Developing the Analyzer.