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

Description: In this tutorial, you will create a simple python script that calls ROS services for model fitting and selection. The script will output the estimated model class (like rotational, prismatic, etc.) and the estimated model parameters (like radius of rotation, etc.).

Keywords: articulation models, kinematic trajectory, model selection, model fitting, rospy, Python

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. In this tutorial, you will create a simple python script that calls ROS services for model fitting and selection. The script will output the estimated model class (like rotational, prismatic, etc.) and the estimated model parameters (like radius of rotation, etc.).

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

First, let's 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 node defined in articulation_models.

roscreate-pkg test_articulation articulation_rviz_plugin

Create a Python client

Copy and paste the following code to a file named model_selection_client.py. This Python node basically contains two functions: sample_track and main. The first function generates an artificial trajectory of an articulated object of a given type (either rotational or prismatic), and returns it in form of a articulation_msgs/TrackMsg message. The main function creates a service proxy for the model_select service, and repeatedly samples trajectories and passes them on to this service. Finally, it prints the result of model selection to the screen, and publishes a model message that can be visualized for example in rviz.

   1 #!/usr/bin/env python
   2 
   3 import roslib; roslib.load_manifest('test_articulation')
   4 import rospy
   5 import numpy
   6 
   7 from articulation_msgs.msg import *
   8 from articulation_msgs.srv import *
   9 from geometry_msgs.msg import Pose, Point, Quaternion
  10 from sensor_msgs.msg import ChannelFloat32
  11 
  12 
  13 PRISMATIC = 0
  14 ROTATIONAL = 1
  15 MODELS={PRISMATIC:'prismatic',ROTATIONAL:'rotational'}
  16 
  17 def sample_track(model = PRISMATIC, n = 100, sigma_position = 0.02):
  18         msg = TrackMsg()
  19         msg.header.stamp = rospy.get_rostime()
  20         msg.header.frame_id = "/"
  21         msg.id = model
  22         for i in range(0,n):
  23                 q = i / float(n)
  24                 if model == PRISMATIC:
  25                         pose = Pose(Point(q, 0, 0), Quaternion(0, 0, 0, 1))
  26                 elif model == ROTATIONAL:
  27                         pose = Pose(Point(numpy.sin(q), numpy.cos(q) - 1.0, 0), Quaternion(0, 0, 0, 1))
  28                 else:
  29                         raise NameError("unknown model, cannot generate trajectory!")
  30                 pose.position.x += numpy.random.rand()*sigma_position
  31                 pose.position.y += numpy.random.rand()*sigma_position
  32                 pose.position.z += numpy.random.rand()*sigma_position
  33                 msg.pose.append( pose )
  34         return msg
  35 
  36 def main():
  37         rospy.init_node('test_fitting')
  38 
  39         model_select = rospy.ServiceProxy('model_select', TrackModelSrv)
  40         model_pub = rospy.Publisher('model', ModelMsg)
  41         print
  42   
  43         while True:
  44                 for model_type,model_name in MODELS.items():
  45                         request = TrackModelSrvRequest()
  46                         print "generating track of type '%s'" % model_name
  47                         request.model.track = sample_track( model_type )
  48                  
  49                         try:            
  50                                 response = model_select(request)
  51                                 print "selected model: '%s' (n = %d, log LH = %f)" % (
  52                                         response.model.name,
  53                                         len(response.model.track.pose),
  54                                         [entry.value for entry in response.model.params if entry.name=='loglikelihood'][0]
  55                                 )
  56                                 model_pub.publish(response.model)
  57                         except rospy.ServiceException: 
  58                                 print "model selection failed"
  59                                 pass
  60                         if rospy.is_shutdown():
  61                                 exit(0)
  62                         print
  63                         rospy.sleep(0.5)
  64 
  65 
  66 
  67 if __name__ == '__main__':
  68   main()

Create a launch file

Copy and paste the following into a file called model_selection.launch in your current directory.

<launch>

        <node pkg="test_articulation" type="model_selection_client.py" name="test_fitting" output="screen" />

        <node pkg="articulation_models" type="model_learner_srv" name="model_learner_srv" output="screen"/>

        <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find test_articulation)/fit_models.vcg" />

</launch>

This will bring up the model selection service (offered by the model_learner_srv node from articulation_models). Further, this will start rviz in a configuration that will show the grid, the coordinate axes, the generated trajectory and the model fitted to it. For making this work, download this configuration file fit_models.vcg for RVIZ and save it in the test_articulation folder.

Run the example

Now simply run:

rosmake test_articulation
roslaunch model_selection.launch

The result should look similar to the following figure.

alt text

The Python node presented in this tutorial samples trajectories alternately prismatic and rotational models, and runs them through model selection. The track and the fitted model can then both be visualized in RVIZ.

Wiki: articulation_tutorials/Tutorials/Using the Articulation Models (Python) (last edited 2012-04-17 13:50:23 by Felix Endres)