Geometric Relations Semantics tutorial

The goal of the geometric relations semantics is to provide semantic checking for calculations with geometric relations between rigid bodies on top of existing geometric libraries, which are only working on specific coordinate representations. Since there are already a lot of libraries with good support for geometric calculations on specific coordinate representations (The Orocos Kinematics and Dynamics library, the ROS geometry library, boost, …) we do not want to design yet another library but rather will extend these existing geometric libraries with semantic support. The effort to extend an existing geometric library with semantic support is very limited: it boils down to the implementation of about six function template specializations.

In this tutorial we will show how the geometric relations semantics can be used to do

• semantic checking of geometric semantics calculations (within a c++ main file, a ros topic, and an orocos component)
• avoid system integration errors (this case by semantic checking of the communicated geometric relations between orocos components and a ros node)

The full information on the geometric relations semantics is available on the geometric-relations-semantics wiki. If you want to take a head-start I advice to read the theoretical article and the software article both accepted as a tutorial for IEEE Robotics and Automation Magazine on the 4th June 2012 and 10th September 2012, respectively.

• First, install the orocos_toolchain (replace groovy with your ros version) and rtt_ros_integration

 sudo aptitude install ros-groovy-orocos-toolchain rtt_ros_integration 

• Add the following lines to your .bashrc file:

    export orocos_toolchain_ros_PATH=`rosstack find orocos_toolchain`;
    source $orocos_toolchain_ros_PATH/env.sh;
    export RTTLUA_MODULES=`rospack find ocl`/lua/modules/?.lua
    if [ "x$LUA_PATH" == "x" ]; then
            LUA_PATH=";;"
    fi
    export LUA_PATH="$LUA_PATH;$RTTLUA_MODULES" 
    

     
* Next, get the sources from git using:

  git clone http://git.mech.kuleuven.be/robotics/geometric_relations_semantics.git
  git clone https://github.com/tdelaet/geometric_relations_semantics_tutorial.git -b assignment
  

• Go into the geometric_relations_semantics directory using:

   cd geometric_relations_semantics 

• Add this directory to your ROS_PACKAGE_PATH environment variable using:

export ROS_PACKAGE_PATH=$PWD:$ROS_PACKAGE_PATH

• Go into the geometric_relations_semantics_tutorial directory using:

cd ../geometric_relations_semantics_tutorial

• Add this directory to your ROS_PACKAGE_PATH environment variable using:

export ROS_PACKAGE_PATH=$PWD:$ROS_PACKAGE_PATH

• Install the dependencies and build the library using:

rosdep install geometric_relations_semantics_tutorial
rosmake geometric_relations_semantics_tutorial

Everything should compile out of the box, and you are now ready to start using geometric relation semantics tutorial.

To get familiar with the basics we will follow the tutorials as proposed on the http://www.orocos.org/wiki/geometric-relations-semantics-wiki/tutorials geometric relations semantics homepage. The goal is to find how the geometric semantics can help to avoid commonly made errors and to get familiar with the current available software support.

In the second tutorial we will investigate how the geometric semantics can be used for avoiding errors in geometric relations calculations and during system integration.

two_robot_image-crop.pdf

In this tutorial we will show how the semantics for geometric relations between rigid bodies can be used in robotics. To this end we use the example illustrated in the figure above, in which two robots cooperate for spray painting a cylindrical object. The first robot holds the cylindrical object, while the other robot holds the spray gun. To complete the painting task, the robot programmer has to determine the joint angles of the second robot holding the spray gun such that a predefined pose between the spray gun and cylindrical object is obtained (the joint angles of the first robot holding the cylindrical object are given).

In a first step, the rigid bodies and the frames attached to them are identified:

• {b1} attached to the base B1 of the first robot,
• {e1} attached to the end-effector E1 of the first robot,
• {o1} attached to cylindrical object O1 ,
• {b2} attached to the base B2 of the second robot,
• {e2} attached to the end-effector E2 of the second robot, and
• {o2} attached to the spray gun O2 .

In our example the following poses are available:

• PoseCoord ({e1}|E1 , {b1}|B1 , [b1]) determined by the forward position kinematics of the first robot FK(robot1 ),
• PoseCoord ({b2}|B2 , {b1}|B1 , [b1 ]) determined by the mounting of the robots,
• PoseCoord ({o1}|O1 , {e1}|E1 , [e1]) determined by the mounting of the cylindrical object on the first robot end-effector,
• PoseCoord ({o2}|O2 , {e2}|E2 , [e2]) determined by the mounting of the spray gun on the second robot end-effector,
• PoseCoord ({o2}|O2 , {o1}|O1 , [o1]) determined by the desired spray-painting pose.

In order to find the joint angles of the second robot the robot programmer has to find PoseCoord({e2}|E2 , {b2}|B2 , [b2]), and subsequently use the inverse kinematics of the second robot. Your task is to find PoseCoord({e2}|E2 , {b2}|B2 , [b2]) such that we can use an inverse kinematics algorithm (for instance from orocos-kdl) to determine the joint angles.

Since this tutorial also wants to show how the geometric relation semantics can be used during system integration the tutorial integrates orocos components and ros nodes. The tuturial has the two orocos components:

• publisher of type Geometric_semantics_component_tutorial_pusblisher, which publishes PoseCoord ({e1}|E1 , {b1}|B1 , [b1]) on an orocos port and on a ros topic with name /geometric_semantics_tutorial/pose. This component runs periodically.
• subscriber of type Geometric_semantics_component_tutorial_subscriber, which listens for PoseCoordinates on the ros topic /geometric_semantics_tutorial/pose_result and checks every time it receives a pose, if the received pose has the expected semantics of

PoseCoordSemantics({e2}|E2 , {b2}|B2 , [b2]). This component runs aperiodically by waking up upon receival of a pose.

and one ros node:

• node, which listens to the ros topic with name /geometric_semantics_tutorial/pose to receive PoseCoord ({e1}|E1 , {b1}|B1 , [b1]) and performs the necessary calculations to publish PoseCoordSemantics({e2}|E2 , {b2}|B2 , [b2]).

Your assignment is to fill in the necessary calculations in node.

The two orocos components are available in the geometric_relations_semantics_tutorial_orocos_components package.

The template of the ros node is available in the geometric_relations_semantics_tutorial_ros_nodes package.

• In a first terminal, run a roscore

roscore 

* In a second terminal, go to the geometric_relations_semantics_tutorial_orocos_components package and start the orocos components:

 
 roscd geometric_relations_semantics_tutorial_orocos_components
 rosrun ocl rttlua-gnulinux -i deploy_tutorial.lua 
 

If you get the error ”/opt/ros/groovy/stacks/orocos_toolchain/ocl/bin/rttlua-gnulinux: deploy_tutorial.lua:6: TaskContext.getPeer: no peer Deployer”, replace Deployer by deployer on line 6 of deploy_tutorial.lua (will appear on older orocos_toolchain releases by a backwared incompatible change in rttlua).

In this terminal you can see the output of the orocos components. If the semantics of the poses received by the subscriber does not comply with what is expected (defined in the props_subscriber.cpf) it will raise an Error.

• In a third terminal, go to the geometric_relations_semantics_tutorial_ros_nodes package and start the ros node:

 
 roscd geometric_relations_semantics_tutorial_ros_nodes
 bin/node
 

At first you will see some output telling you that you still have to fill in the calculations. If you add geometric relations semantics calculations mesages concerning the semantic (in)correctness of them will be shown.

* Go to the geometric_relations_semantics_tutorial_ros_nodes package

 
 roscd geometric_relations_semantics_tutorial_ros_nodes
 

* Open the node.cpp file with your favourite editor

 
 vim node.cpp
 

* The file already contains the entire skeleton and code for receiving a pose and publishing the result. Furthermore, the available poses (see above) have been predefined and some intermediate poses (that you can use for your calculations) have been allocated.

* Fill in the doGeometricSemanticsCalculations function with the necessary geometric relations calculations.

geometric_semantics.pdf