Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. 
Using the Articulation Model Library (C++)
Description: This tutorial demonstrates how to use the articulation model library directly in your programs. This is more efficient than sending ROS messages or ROS services. In this tutorial, a short program is presented that creates an artificial trajectory of an object rotating around a hinge, and then uses the model fitting library to recover the rotational center and radius. Further, the sampled trajectory and the fitted model are publishes as a ROS message for visualization in RVIZ.Keywords: articulation models, kinematic trajectory, model selection, model fitting, roscpp, cpp
Tutorial Level: INTERMEDIATE
Contents
Introduction
Articulation models describe the kinematic trajectories of articulated objects, like doors, drawers, and other mechanisms. Such models can be learned online and in realtime, while a robot is pulling a door open. The articulation stack in the ALUFRROSPKG repository provides several functions for fitting, selecting, saving and loading articulation models. This tutorial demonstrates how to use the articulation model library directly in your programs. This is more efficient than sending ROS messages or ROS services. In this tutorial, a short program is presented that creates an artificial trajectory of an object rotating around a hinge, and then uses the model fitting library to recover the rotational center and radius. Further, the sampled trajectory and the fitted model are publishes as a ROS message for visualization in RVIZ.
Download and Compile
All we need for this tutorial is the articulation_rviz_plugin and its dependency. We assume that you have already installed the basic components of ROS, checked out the ALUFRROSPKG repository at Google Code.
cd ~/ros svn co https://alufrrospkg.googlecode.com/svn/trunk alufrrospkg
For compilation, type
rosmake articulation_rviz_plugin
which should build (next to many other things) the RVIZ , the articulation_models and the articulation_rviz_plugin package.
Create a new package
Create a new package that depends on articulation_rviz_plugin. This also provides us with the messages defined in articulation_msgs, and the model selection library from articulation_models.
roscreatepkg test_articulation articulation_rviz_plugin
Create a C++ ROS node for model fitting
Copy and paste the following code to a file named model_fitting_demo.cpp. This program simulates the position of an object rotating around a hinge. In each iteration of the main loop, a new pose is sampled, and added to the posevector of the articulation_msgs/ModelMsg message. Calling factory.restoreModel turns this struct subsequently into a C++ object. This object is of type articulation_models::GenericModel, and offers methods for fitting (fitModel), evaluating (evaluateModel) and setting and getting parameters (getParam and setParam). For this demonstration, a few of the fitted parameters, like center of rotation and radius, are printed to the screen. Also, the model object can return an updated model message (using the getModel()method). This message is then published to the /model topic.
1 /*
2 * model_fitting_demo.cpp
3 *
4 * Created on: Jun 8, 2010
5 * Author: sturm
6 */
7
8 #include <ros/ros.h>
9
10 #include "articulation_models/models/factory.h"
11
12 #include "articulation_msgs/ModelMsg.h"
13 #include "articulation_msgs/TrackMsg.h"
14 #include "articulation_msgs/ParamMsg.h"
15
16 using namespace std;
17 using namespace articulation_models;
18 using namespace articulation_msgs;
19
20 #include <boost/random.hpp>
21 #include <boost/random/normal_distribution.hpp>
22
23 int main(int argc, char** argv) {
24 ros::init(argc, argv, "model_fitting");
25 ros::NodeHandle n;
26 ros::Publisher model_pub = n.advertise<ModelMsg> ("model", 5);
27 ros::Rate loop_rate(5);
28 int count = 0;
29
30 boost::normal_distribution<> nd(0.0, 0.02);
31 boost::mt19937 rng;
32 boost::variate_generator<boost::mt19937&, boost::normal_distribution<> >
33 var_nor(rng, nd);
34
35 MultiModelFactory factory;
36
37 while (ros::ok()) {
38 ModelMsg model_msg;
39 model_msg.name = "rotational";
40 ParamMsg sigma_param;
41 sigma_param.name = "sigma_position";
42 sigma_param.value = 0.02;
43 sigma_param.type = ParamMsg::PRIOR;
44 model_msg.params.push_back(sigma_param);
45
46 model_msg.track.header.stamp = ros::Time();
47 model_msg.track.header.frame_id = "/";
48 model_msg.track.track_type = TrackMsg::TRACK_POSITION_ONLY;
49
50 for (int i = 0; i < 100; i++) {
51 geometry_msgs::Pose pose;
52 pose.position.x = cos(i / 100.0 + count / 10.0)+var_nor();
53 pose.position.y = sin(i / 100.0 + count / 10.0)+var_nor();
54 pose.position.z = var_nor();
55 pose.orientation.x = 0;
56 pose.orientation.y = 0;
57 pose.orientation.z = 0;
58 pose.orientation.w = 1;
59 model_msg.track.pose.push_back(pose);
60 }
61
62 GenericModelPtr model_instance = factory.restoreModel(model_msg);
63 model_instance>fitModel();
64 model_instance>evaluateModel();
65
66 cout << "model class = "<< model_instance>getModelName() << endl;
67 cout << " radius = "<<model_instance>getParam("rot_radius")<< endl;
68 cout << " center.x = "<<model_instance>getParam("rot_center.x")<< endl;
69 cout << " center.y = "<<model_instance>getParam("rot_center.y")<< endl;
70 cout << " center.z = "<<model_instance>getParam("rot_center.z")<< endl;
71 cout << " log LH = " << model_instance>getLikelihood() << endl;
72
73 ModelMsg fitted_model_msg = model_instance>getModel();
74 model_pub.publish(fitted_model_msg);
75
76 ros::spinOnce();
77 loop_rate.sleep();
78 ++count;
79 }
80 }
Creating a launch file
Although the demo program from above can be run as a standalone node, it is easier to inspect visually what is going on. To that purpose, RViz with the ModelMsgplugin can be used.
<launch> <node pkg="articulation_tutorials" type="model_fitting_demo" name="test_fitting" output="screen" /> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="d $(find articulation_tutorials)/cpp_model_fitting/display_grid_axes_model.vcg" /> </launch>

The C++ node presented in this tutorial samples positions of an object rotating around the zaxis with a radius of 1m. The RVIZ model visualization plugin shows both the trajectory, colored green/red according to the individual pose likelihoods, and the fitted model in gray 
Demo output (Screen)
This is some example output of the demonstration program presented in this tutorial. Note that the rotational model needs at least 4 poses before it can compute the model coefficients. Also note, that the initial estimate of the model coefficients is very poor when only the first few centimeters (with sigma=2cm noise) are observed. The estimates quickly improve after a longer trajectory has been observed.
model class = rotational radius = 1 center.x = 0 center.y = 0 center.z = 0 log LH = nan [ INFO] [1276083891.231585290]: Loading general config from [/home/lollypop/sturm/.rviz/config] [ INFO] [1276083891.232050690]: Loading display config from [/home/lollypop/sturm/.rviz/display_config] model class = rotational radius = 1 center.x = 0 center.y = 0 center.z = 0 log LH = nan points:3 RotationalModel: fitPlane failed model class = rotational radius = 1 center.x = 0 center.y = 0 center.z = 0 log LH = nan points:4 model class = rotational radius = 0.180308 center.x = 0.960925 center.y = 0.0613411 center.z = 0.188755 log LH = 0.290972 points:5 model class = rotational radius = 1117.88 center.x = 362.016 center.y = 469.342 center.z = 947.452 log LH = 0.651335 points:6 model class = rotational radius = 555.041 center.x = 176.694 center.y = 69.0765 center.z = 521.298 log LH = 0.927887 points:7 model class = rotational radius = 0.135209 center.x = 1.03131 center.y = 0.0515943 center.z = 0.100709 log LH = 2.17121 points:8 model class = rotational radius = 0.271709 center.x = 0.977617 center.y = 0.200966 center.z = 0.25174 log LH = 1.18562 points:9 model class = rotational radius = 1.34235 center.x = 1.27822 center.y = 0.0666502 center.z = 1.28642 log LH = 1.12099 points:10 model class = rotational radius = 662.294 center.x = 264.891 center.y = 55.9791 center.z = 604.851 log LH = 0.976538 points:11 model class = rotational radius = 0.362923 center.x = 0.820574 center.y = 0.160068 center.z = 0.288078 log LH = 1.01913 points:12 model class = rotational radius = 0.456756 center.x = 0.734882 center.y = 0.185757 center.z = 0.360662 log LH = 0.760279 points:13 model class = rotational radius = 0.429472 center.x = 0.791086 center.y = 0.188593 center.z = 0.362659 log LH = 0.79271 points:14 model class = rotational radius = 0.380097 center.x = 0.754191 center.y = 0.19568 center.z = 0.27918 log LH = 0.890839 points:15 model class = rotational radius = 0.510351 center.x = 0.700314 center.y = 0.183675 center.z = 0.404699 log LH = 0.806198 points:16 model class = rotational radius = 0.592625 center.x = 0.452138 center.y = 0.125068 center.z = 0.225673 log LH = 0.805371 points:17 model class = rotational radius = 0.681178 center.x = 0.57213 center.y = 0.175353 center.z = 0.527507 log LH = 0.81509 points:18 model class = rotational radius = 0.611191 center.x = 0.60594 center.y = 0.208415 center.z = 0.470494 log LH = 1.06384 points:19 model class = rotational radius = 0.684332 center.x = 0.354202 center.y = 0.105043 center.z = 0.228971 log LH = 0.686269 points:20 model class = rotational radius = 0.723053 center.x = 0.368001 center.y = 0.116914 center.z = 0.354212 log LH = 0.6778 points:21 model class = rotational radius = 0.68478 center.x = 0.352511 center.y = 0.10448 center.z = 0.225485 log LH = 0.634019 points:22 model class = rotational radius = 0.711196 center.x = 0.379455 center.y = 0.118657 center.z = 0.350076 log LH = 0.638756 points:23 model class = rotational radius = 0.751677 center.x = 0.348053 center.y = 0.110153 center.z = 0.376941 log LH = 0.670791 points:24 model class = rotational radius = 0.761347 center.x = 0.420902 center.y = 0.137444 center.z = 0.496227 log LH = 1.10818 points:25 model class = rotational radius = 0.868042 center.x = 0.327393 center.y = 0.108216 center.z = 0.551141 log LH = 1.1696 points:26 model class = rotational radius = 1.01176 center.x = 0.0587054 center.y = 0.0105799 center.z = 0.358562 log LH = 1.05315 points:27 model class = rotational radius = 1.61691 center.x = 0.425125 center.y = 0.236657 center.z = 0.640292 log LH = 1.16074 points:28 model class = rotational radius = 1.0924 center.x = 0.0450039 center.y = 0.0218906 center.z = 0.291469 log LH = 1.02786 points:29 model class = rotational radius = 0.961882 center.x = 0.0745566 center.y = 0.0108829 center.z = 0.242933 log LH = 1.03783 points:30 model class = rotational radius = 1.05601 center.x = 0.0477931 center.y = 0.0255933 center.z = 0.0205304 log LH = 1.00237 points:31 model class = rotational radius = 1.11501 center.x = 0.0912058 center.y = 0.049268 center.z = 0.175049 log LH = 7.41091 points:32 model class = rotational radius = 0.960844 center.x = 0.0406013 center.y = 0.0287704 center.z = 0.141344 log LH = 5.95775 points:33 model class = rotational radius = 1.0852 center.x = 0.0725985 center.y = 0.0393999 center.z = 0.0886639 log LH = 5.60011 points:34 model class = rotational radius = 1.04272 center.x = 0.0103253 center.y = 0.0334244 center.z = 0.247143 log LH = 1.21064 points:35 model class = rotational radius = 0.885302 center.x = 0.104408 center.y = 0.0641665 center.z = 0.046224 log LH = 0.977554 points:36 model class = rotational radius = 1.04713 center.x = 0.0170094 center.y = 0.0445478 center.z = 0.117126 log LH = 0.999225 points:37 model class = rotational radius = 0.995899 center.x = 0.0121307 center.y = 0.00314552 center.z = 0.0674619 log LH = 1.00695 points:38 model class = rotational radius = 1.02188 center.x = 0.013386 center.y = 0.0171478 center.z = 0.0320113 log LH = 0.967128 points:39 model class = rotational radius = 1.04887 center.x = 0.0366193 center.y = 0.0304123 center.z = 0.0628515 log LH = 1.0118 points:40 model class = rotational radius = 1.03742 center.x = 0.0273324 center.y = 0.0250118 center.z = 0.0393251 log LH = 0.986585