[Documentation] [TitleIndex] [WordIndex

(!) 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

Introduction

Articulation models describe the kinematic trajectories of articulated objects, like doors, drawers, and other mechanisms. Such models can be learned online and in real-time, while a robot is pulling a door open. The articulation stack in the ALUFR-ROS-PKG 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 ALUFR-ROS-PKG repository at Google Code.

cd ~/ros
svn co https://alufr-ros-pkg.googlecode.com/svn/trunk alufr-ros-pkg

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.

roscreate-pkg 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 pose-vector 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 stand-alone node, it is easier to inspect visually what is going on. To that purpose, RViz with the ModelMsg-plugin 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>

alt text

The C++ node presented in this tutorial samples positions of an object rotating around the z-axis 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


2019-11-16 12:39