How to create a new task?

Please read the iTaSC_Manual first, to get acquainted with the iTaSC terminology and structure. A task is the combination of a virtual_kinematic_chain and a constraint/controller. In the iTaSC software, it is a (ROS-)package that contains:

• src/ containing the source code of the components (C++)
  • VKC_taskname.hpp/cpp
  • CC_taskname.hpp/cpp
• cpf/ containing the property files for the components and FSM's
• scripts/ containing the FSM's and components in a LUA implementation
  • taskname_supervisor.lua: the supervisor orocos-component, containing the Orocos specific code, eg. ports to recieve/send events on...
  • taskname_fsm.lua: the finite-state machine containing the actual Skill
  • running_taskname_coordination.lua: the sub-finite-state machine of the running state of the task, containing the coordination part of the task, it determines what the task part of the iTaSC algorithm should do and in which order
  • running_taskname_fsm.lua: the sub-finite-state machine machine of the running state of the task, determining the actual actions of the task (when to enable the task, when to change weights...)
• launch/ containing eventual ROS launch files

The running_taskname_coordination.lua and running_taskname_fsm.lua, are sub-FSM's of the running state of the task (defined in taskname_fsm.lua). They are executed sequentially, first the coordination part, then the FSM part.

Getting started

Virtual Kinematic Chain (VKC)

Important are the expected reference frames and points for the data on following ports. (o1=object 1, o2= object 2)

• Inputs
  • $T_{o1}^{o2}$ = RelPose(o2|o2,o1|o1,o1) = pose of o2 on body o2 wrt. o1 on body o1 expressed in o1
  • $_{o1}^{o1}t_{o2}^{o1}$ = RelTwist(o1|o1,o1,o2) = twist with ref.point o1 on object o1 expressed in o1
• Outputs
  • $T_{o1}^{o2}$ = RelPose(o2|o2,o1|o1,o1) = pose of o2 on body o2 wrt. o1 on body o1 expressed in o1
  • $J_{u}\chi_{u}$ =
  • $_{o1}^{o2}J_{f o1}^{o1}$ = Jacobian(o1|o2,o1,o1) = ref.point o1 on object o2 expressed in o1

The expected references are also mentioned as comments in the files

Constraint/Controller (CC)

task_layout

A full template will be made available soon... At the moment, start from an example... Have a look at the keep_distance task-package (in the itasc_comanipulation_demo stack) as a good example of a task. Special cases are:

• cartesian_motion: This task, defines a virtual kinematic chain with in stead of feature coordinates x,y,z,roll, pitch, yaw, the full pose (KDL::Frame), to enhance efficiency of the code (and prevent singularity problems).A separate port, the ChifT port transfers this data from the VKC to the CC, which should be connected manually at the moment!. ChifT = RelPose(o2|o2,o1|o1,o1) = pose of o2 on body o2 wrt. o1 on body o1 expressed in w
• joint_motion: This task has no virtual kinematic chain, because its output equation is y=q. It constrains only the joint coordinates.

Conventions

• the components and scripts have the task name in their names, eg. for cartesian_motion
  • package name: cartesian_motion
  • component names: VKC_cartesian_motion.hpp
  • script names: cartesian_motion_supervisor.lua
• For conformity it is advised to use lower case names with underscores to separate words

How to create a new robot or object?

Should inherit from SubRobot.hpp, which can be found in the itasc_core. This file is a template for a robot or object component. See itasc_robots_objects stack for examples.

As can be seen in the examples, a robot component contains always a KDL::Tree, even if the robot is just a chain. This is to be able to use the KDL::Tree functionality, which is regrettable, not perfectly similar as the KDL::Chain functionality. E.g. tree.getSegment(string name) has a string as input, chain.getSegment(number) has a number as input, but not a string...

How to create a new solver?

Coming soon, have a look at itasc_solvers for examples.

Youbot lissajous tracing tutorial (Cartesian VKC)

Summary

Installation

Dependencies

• itasc and it's dependencies
• trajectory_generators
• youbot drivers
• ROS Electric

The easiest way to install all needed dependencies: (How to find the debian packages on ros.org)

• ROS Electric
• Orocos toolchain
  • sudo apt-get install ros-electric-rtt-common-msgs
  • sudo apt-get install ros-electric-rtt-ros-comm
  • sudo apt-get install ros-electric-rtt-ros-integration
  • git clone http://git.mech.kuleuven.be/robotics/rtt_geometry.git
• Orocos kinematics and dynamics
  • sudo apt-get install ros-electric-orocos-kinematics-dynamics
• rFSM
  • needs lua:
    • sudo aptitude install liblua5.1-0-dev
    • sudo aptitude install liblua5.1-0
    • sudo aptitude install lua5.1
    • git clone git://gitorious.org/orocos-toolchain/rttlua_completion.git
  • git clone https://github.com/kmarkus/rFSM.git
• Trajectory Generators
  • git clone http://git.mech.kuleuven.be/robotics/trajectory_generators.git
• youbot hardware stack
  • git clone http://git.mech.kuleuven.be/robotics/youbot_hardware.git -b devel
    • this depends on: git clone http://git.mech.kuleuven.be/robotics/soem.git
  • git clone git://git.mech.kuleuven.be/robotics/motion_control.git -b devel
• youbot_description package of the youbot-ros-pkg (=a stack), no need to compile it! WARNING: there are two repos around, make sure you have this one!
  • git clone https://github.com/smits/youbot-ros-pkg.git
• iTaSC
  • git clone http://git.mech.kuleuven.be/robotics/itasc.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_core.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_solvers.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_tasks.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_robots_objects.git (+switch to devel branch)

Download the solution of the tutorial

Setup

• It is strongly recommended that you add the following to a setup script or your .bashrc
  • Make sure that all packages are added to you ROS_PACKAGE_PATH variable
  • Source env.sh in the orocos_toolchain stack
  • Set the LUA_PATH variable:

if [ "x$LUA_PATH" == "x" ]; then LUA_PATH=";;"; fi
if [ "x$LUA_CPATH" == "x" ]; then LUA_CPATH=";;"; fi
 
export LUA_PATH="$LUA_PATH;`rospack find ocl`/lua/modules/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find kdl`/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find rFSM`/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find rttlua_completion`/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find youbot_master_rtt`/lua/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find kdl_lua`/lua/?.lua"
 
export LUA_CPATH="$LUA_CPATH;`rospack find rttlua_completion`/?.so"
 
export PATH="$PATH:`rosstack find orocos_toolchain`/install/bin"

• Create the youbot.urdf file out of the youbot.urdf.xacro file
  • cd `rospack find youbot_description`/robots/ (part of the youbot-ros-pkg)
  • rosrun xacro xacro.py youbot.urdf.xacro -o youbot.urdf

Make

The tutorial

This tutorial explains how to create an iTaSC application, starting from existing packages. The scheme we want to create is depicted in following figure:

itasc_youbot_app scheme

The tutorial will follow the design workflow as explained here.

List of all tasks/motions to be executed

1. Let the end effector go to a start position, with respect to a certain point in space
2. Let the end effector trace a lissajous figure in the air, with respect to a certain point in space

Draw the kinematic loops for each task

kinematic loop

• frames
  • o_1=f_1=end effector
  • o_2=f_2=fixed object
• feature coordinates
  • X_fI=(-)
  • X_fII=in this case actually the full pose!
  • X_fIII=(-)
• outputs
  • y= X_f = T

itasc_youbot_app kinematic loop

Draw the behavior of the application at runtime

In the figures, a (sub-)FSM is represented by a purple rounded box, a state by a rounded black box and a possible state transition by an arrow. State transitions are triggered by an event or combination of events. The state transitions of the task subFSM that are indicated by a colored arrow and circle, are caused by the event with the corresponding color, fired in the composite_task_fsm.lua. composite task FSM of the youbot_lissajous_app To automatically transition from the MoveToStart to the TraceFigure state, an event indicating that the start position is reached must be fired. This event will be generated by the 'cartesian_generator'. running_cartesian_tracing_fsm of the youbot_lissajous_app

Create or download all necessary packages

• general components and scripts (from the itasc_core stack)
  • scene
  • start from the itasc and application level script templates in the script subdir
• robots and objects (from the itasc_robots_objects stack)
  • youbot: itasc_youbot package
  • fixed_object: fixed_object package
• task (from the itasc_tasks stack)
  • cartesian_tracing: cartesian_motion package
• solver (from the itasc_solvers stack)
  • solver: wdls_prior_vel_solver package
• trajectory generators (from the trajectory_generators stack)
  • cartesian_generator: cartesian_trajectory_generator package
  • lissajous_generator: lissajous_generator package

Overview of the modifications needed:

• Computation: No modifications needed
• Coordination:Modifications needed
  • for the specific behavior at runtime, see 'behavior at runtime'
  • for the other behavior, see following sections
• Configuration: Modifications needed
  • see 'Check, create or adapt the configuration files of your components'
• Communication and Composition:
  • see 'Create a deploy script'

Create an empty ROS-package for your application and create 2 subdirectories:

• scripts: this subdirectory will contain the for your application adapted scripts
• cpf: this subdirectory will contain all (non-standard) property files for your application

Create the FSM of the cartesian_tracing task

• cartesian_tracing_fsm.lua: this is the actual FSM, other files are loaded in certain states of this FSM,
• running_cartesian_tracing_coordination.lua: this part takes care of the coordination of the algorithm at runtime, it is part of the running state of the actual FSM,
• running_cartesian_tracing_fsm.lua: this part takes care of the coordination of the behavior at runtime, it is also part of the running state of the actual FSM and is executed after the coordination of the algorithm,

Templates of these files can be found in the cartesian_motion package, scripts subdirectory (cartesian_tracing is an instance of cartesian_motion).

• cartesian_tracing_fsm.lua: you can use the template without modifications: no need to change the name, just use cartesian_motion_fsm.lua,
• running_cartesian_tracing_coordination.lua: you can use the template without modifications: no need to change the name, just use running_cartesian_tracing_coordination.lua,
• running_cartesian_tracing_fsm.lua: this file has to be edited (application dependent), copy this file to the scripts subdirectory of the package you have created for this application, leave it for now, in the section 'Create the FSMs that coordinate the behavior at runtime' is explained what to edit here,

The actual FSM is loaded in the cartesian_tracing_supervisor component (which is written in the lua language, hence the .lua file). Since you'll (probably) need to add functions to execute RTT specific code in the running_cartesian_tracing_fsm, make a copy of this file to your scripts subdirectory of the package you have created for this application. Leave it for now.

The FSM for this application consists of multiple files on different locations, the cartesian_tracing_supervisor has properties that contain the path to these files. Create a property file (cartesian_tracing_supervisor.cpf for example) in your scripts subdirectory of the package you have created for this application and edit these properties.

There is no timer_id_property for task supervisors, because tasks are triggered by the iTaSC level by events.

Create the FSM on the iTaSC level

• itasc_fsm.lua: this is the actual FSM, other files are loaded in certain states of this FSM, it is responsible, among other things, to configure, start, stop (and cleanup) the iTaSC level components,
• running_itasc_coordination.lua: this part takes care of the coordination of the algorithm at runtime, it is part of the running state of the actual FSM,
• composite_task_fsm.lua: this part takes care of the coordination of the behavior at runtime, it is also part of the running state of the actual FSM and is executed after the coordination of the algorithm,
• itasc_configuration: this file contains the description of the scene and the composition of the kinematic loops.

Templates of these files can be found in the itasc_core package, scripts subdirectory.

• itasc_fsm.lua: you can use the template without modifications, because this application makes use of the template components of the cartesian_motion package, which are configured/started... by the template FSM,
• running_itasc_coordination.lua: you can use the template without modifications,
• composite_task_fsm.lua: this file has to be edited (application dependent), copy this file to the scripts subdirectory of the package you have created for this application, leave it for now, in the section 'Create the FSMs that coordinate the behavior at runtime' is explained what to edit here,
• itasc_configuration.lua: this file has to be edited (application dependent),copy this file to the scripts subdirectory of the package you have created for this application.

Edit the itasc_configuration.lua file you just have copied: define the scene and kinematic loops as depicted in the figures of the first steps of this tutorial. Look at the comments in the template for more information on the syntax.

The actual FSM is loaded in the itasc_supervisor component (which is written in the lua language, hence the .lua file). Since you'll (probably) need to add functions to execute RTT specific code in the composite_task_fsm, make a copy of this file to your scripts subdirectory of the package you have created for this application. Leave it for now.

The FSM for this application consists of multiple files on different locations, the itasc_supervisor has properties that contain the path to these files. Create a property file (.cpf) in your scripts subdirectory of the package you have created for this application and edit these properties. The itasc_supervisor and application_supervisor have a property "application_timer_id": make sure these have the same value. Take in this case eg. 1. The timer id makes sure that both components are triggered by the same timer.

Create the FSM on the application level

• application_fsm.lua: this is the actual FSM.

A template of this file can be found in the itasc_core package, scripts subdirectory.

• application_fsm.lua: this file has to be edited (application dependent), copy this file to the scripts subdirectory of the package you have created for this application.

The application FSM is loaded in the application_supervisor component (which is written in the lua language, hence the .lua file). Since you'll (probably) need to add functions to execute RTT specific code in the application_fsm, make a copy of this file to your scripts subdirectory of the package you have created for this application.

Edit the application_fsm and application_supervisor files:

• Check the functions called in the application_fsm and verify that the right rtt specific code is present in the application_supervisor, e.g. configureTrajectoryGenerators(), an example can be found in the template file itself.
• Add new functions for application specifics: the idea is to put a function in the FSM and the implementation with the RTT specifics in the supervisor.

Make sure that you configure, start, stop (and cleanup) all application level components in this state machine!

The FSM for this application can be on different locations, the application_supervisor has properties that contain the path to these file. Create a property file (application_supervisor.cpf) in your scripts subdirectory of the package you have created for this application and edit these properties. The itasc_supervisor and application_supervisor have a property "application_timer_id": make sure these have the same value. Take in this case eg. 1. The timer id makes sure that both components are triggered by the same timer.

Check, create or adapt the configuration files of your components

Configuration of the robot and object

• the urdf_file property to the file location of the urdf file of the youbot on your system,
• leave the other properties as they are (all elements of W=1, joint names of the arm count up from 1 till 4, the joint names of the base are baseTransX, baseTransY, baseRotZ, in this order).

Configuration of the solvers

Configuration of the task

• All feature coordinates have the same weight: put all elements of W to 1.
• Tune the control values: Kp (put 2 for all for now).
• We want to use velocity feed-forward: put all elements of Kff to 1.
• put the rotation type on 0 (RPY), and rotation reference frame to 1 (=object 1)

Configuration of the trajectory generators

• In the cartesian_generator.cpf file, add:
  • the maximum velocity in m/s (in cartesian space): put for now 0.05,
  • the maximum acceleration in m/s^2 (in cartesian space): put for now 0.02
• In the lissajous_generator.cpf file, add:
  • the frequency of the sine in the x direction, in Hz: 0.06,
  • the frequency ratio of the sine in the y direction vs. the sine in the x direction: 0.666666 (meaning the second frequency will be 0.4Hz),
  • amplitude of the sine in the x direction, in m: 0.25,
  • amplitude ratio of the sine in the y direction vs. the sine in the x direction: 1,
  • phase difference between the sines in radians (phase of x-phase of y): 0,
  • the index of the yd vector (containing the desired positions), that needs a fixed value as constraint, starting from 0. In our case z: 2,
  • the constant desired value that we constrain on the position determined with the previous property, in meters: 0.45.

Create a deploy script

• the loading of components (making an instance of a certain component type),
• the composition of the non-iTaSC level components,
• the connection of the non-iTaSC level components,
• starting the supervisors and timers to get everything running.

Start from the following templates, which you can find in the itasc_core package, scripts subdirectory:

• run.ops
• run.sh

Copy these files to the package you have created for this application.

Edit the run.ops file (see also comments in template):

• import components, requires correctly setup RTT_COMPONENT_PATH or ROS_PACKAGE_PATH
• load components
• set activities
  • periodic activities: general and application level components
  • non-periodic activities: all iTaSC and task level components and the application_supervisor
• connect peers
• execute lua files (important that it is before loading property files)
• configure lua components (already here so they can create the ports and properties before we connect/load them)
• load property files
• connect ports
  • create connectionPolicies: buffered/ non-buffered connections
  • timer ports
  • event ports
  • application_supervisor connections
    • add connections between the application supervisor and your tasks for the priority events only, with a bufferedcp
  • itasc_supervisor connections
    • add connections between the itasc supervisor and your tasks for all types of events, with the right type of connection
  • add connections between components that fire events and the FSMs that need to react on them
  • task ports
    • add connections between application level components and task level components, e.g. trajectory generators and CC, with the right connection, normally cp
    • add connections between the CC and VKC for non standard ports, e.g. the pose in case of a cartesian_motion task, standard itasc level ports are connected automatically
• configure timers
• start timers
• start the supervisors
• order is of importance! First tasks, then itasc_supervisor, then application_supervisor !!
• Set up timer
  • first argument: timer identity number:
  • second argument: timer period in seconds
  • Make sure the all application and itasc level supervisors that have to be triggered at the same moment have the same timer_id, in this case: application_supervisor and itasc_supervisor. They have a property application_timer_id for this purpose, standard set to 1

put before configuring the timer:

# we have to configure it first to get the ports connected, maybe better to put all this in the application_fsm.lua
youbot_driver.configure()                                    
connect("youbot.qdot_to_arm", "youbot_driver.Arm1.joint_velocity_command", cp)
connect("youbot.qdot_to_base", "youbot_driver.Base.cmd_twist", cp)       
connect("youbot_driver.Arm1.jointstate", "youbot.q_from_arm", cp)   
connect("youbot_driver.Base.odometry", "youbot.q_from_base", cp) 

The template creates automatically an eventFirer, which is a component with ports connected to the event ports of the itasc- and application_supervisor. This allows easy firing events yourself at runtime, by writing an event on one of the ports.

Create the FSMs that coordinate the behavior at runtime

For both levels:

• The idea is to put a function in the FSM and the implementation with the RTT specifics in the supervisor.
• Make sure the task FSM is reacting on the right events, send out by the composite_task_fsm.
• The section 'Create a deploy script' explains how to get the events from other state machines and components in your state machine.

iTaSC level

The event needed for the transition from the MoveToStart to the TraceFigure state, will be send out by the 'cartesian_generator'. Look in his code for the event name.

task level

Configuration of the solution of the tutorial

• Setup binaries to avoid running as root (required to grant the ethercat driver non-root raw socket access)
  •  roscd ocl/bin/
  •  for i in deployer* rttlua*; do sudo setcap cap_net_raw+ep  $i; done
• Platform configuration
  •  roscd youbot_master_rtt/lua/
  • in youbot_test.lua configure your youbot type and network interface to which the youbot ethercat is connected.
    • FUERTE_YOUBOT=false -- false Malaga, true is FUERTE
    • ETHERCAT_IF='ethcat'

Execution of the solution of the tutorial

Connection with ethercat check (optional)

• roscd soem_core/bin
• sudo ./slaveinfo eth0 (or ethcat...)

Calibration

• Open a terminal window
• roscd youbot_master_rtt/lua
• rttlua-gnulinux -i youbot_test.lua
• When youbot test got up and running without errors:
  • start_calib()
  • kill the application when it reached the home position (upright): ctrl+C

Application

• Open 2 terminal windows
• In the first, run a roscore: roscore
• In the second, go to itasc_youbot_lissajous_app: roscd itasc_youbot_lissajous_app
• run the application: ./run.sh
• When your application has gone trough the configuration and starting phase, it will reach the running state: You should find a line on your screen saying "===Application up and running!==="
• To interact with the composite_task_fsm, you can send events to it:
  • Start the full application (go to start pose and start tracing the lissajous figure after that): event_firer.itasc_common_events_in.write("e_start")
  • More eventnames can be found in scripts/composite_task_fsm.lua => transitions

FAQ

• Q: My robot is not moving, when I send eg. the e_start event
• A:
  1. Check the values send by the solver to the youbot: go to the scene in the taskbrowser and type: youbot_qdot.last if this is NaN, see answer on that question...
  2. Check whether your arm is in Velocity mode: go to the youbot_driver in the taskbrowser and type: Arm1.control_mode.last, if it responds with 'MotorStop', type: Arm1.setControllerMode(Velocity)
• Q: The solver is sending NaN as qdot
• A: Check that your Wq is not a zero matrix (in itasc_youbot package, cpf/youbot.cpf)

Youbot lissajous tracing tutorial (ERF2012)

Summary

Installation

Ubuntu Installation with ROS Electric

Ubuntu 12.04 Installation with ROS Fuerte

• Install Fuerte ROS using Debian packages for Ubuntu Precise (12.04) or later. In case you don't run Ubuntu you can use the ROS install scripts. See the ros installation instructions.

• Install the Orocos Toolchain, if you don't have it yet, install the Orocos Toolchain the ROS way

• Make sure the following debian packages are installed:

sudo apt-get install ros-fuerte-pr2-controllers
sudo apt-get install ros-fuerte-pr2-simulator

• Create a directory in which you want to install all the demo source (for instance erf)

mkdir ~/erf

• Add this directory to your $ROS_PACKAGE_PATH

export ROS_PACKAGE_PATH=~/erf:$ROS_PACKAGE_PATH

• Get rosinstall

sudo apt-get install python-setuptools
sudo easy_install -U rosinstall

• Get the workshop's rosinstall file . Save it as erf_fuerte.rosinstall in the erf folder.

• Run rosinstall

rosinstall ~/erf erf_fuerte.rosinstall /opt/ros/fuerte/

• As the rosinstall tells you source the setup script

source ~/erf/setup.bash

• Install all dependencies (ignore warnings)

rosdep install itasc_examples
rosdep install rFSM

Setup

• Add the following functions in your $HOME/.bashrc file:

useERF(){
    source $HOME/erf/setup.bash;
    source $HOME/erf/setup.sh;
    source `rosstack find orocos_toolchain`/env.sh;
    setLUA;
}
 
setLUA(){
    if [ "x$LUA_PATH" == "x" ]; then LUA_PATH=";;"; fi
    if [ "x$LUA_CPATH" == "x" ]; then LUA_CPATH=";;"; fi
    export LUA_PATH="$LUA_PATH;`rospack find rFSM`/?.lua"
    export LUA_PATH="$LUA_PATH;`rospack find ocl`/lua/modules/?.lua"
    export LUA_PATH="$LUA_PATH;`rospack find kdl`/?.lua"
    export LUA_PATH="$LUA_PATH;`rospack find rttlua_completion`/?.lua"
    export LUA_PATH="$LUA_PATH;`rospack find youbot_driver_rtt`/lua/?.lua"
    export LUA_PATH="$LUA_PATH;`rospack find kdl_lua`/lua/?.lua"
    export LUA_CPATH="$LUA_CPATH;`rospack find rttlua_completion`/?.so"
    export PATH="$PATH:`rosstack find orocos_toolchain`/install/bin"
}
 
useERF

Make

rosmake itasc_erf2012_demo

The tutorial

List of all tasks/motions to be executed

Draw the kinematic loops for each task

Draw the behavior of the application at runtime

Create or download all necessary packages

Create the FSM on the iTaSC level

Create the FSM on the application level

Check, create or adapt the configuration files of your components

Create a deploy script

Create the FSMs that coordinate the behavior at runtime

Execution of the solution of the tutorial

Execution

Gazebo simulation

• Open a terminal and run roscore

roscore

• Open another terminal and launch an empty gazebo world

roslaunch gazebo_worlds empty_world.launch

• Open another terminal and go to the itasc_erf_2012 package:

roscd itasc_erf2012_demo/

• Run the script that starts the gazebo simulator (and two translator topics to communicate with the itasc code)

./run_gazebo.sh

• Open another terminal and go to the itasc_erf_2012 package:

roscd itasc_erf2012_demo/

• Run the script that starts the itasc application

./run_simulation.sh

• Look for the following output:

itasc fsm: STATE ENTER
root.NONemergency.RunningITASC.Running
[cartesian generator] moveTo will start from

Real youbot

• Make sure that you are connected to the real youbot.
• Open another terminal and go to the itasc_erf_2012 package:

roscd itasc_erf2012_demo/

• Check the name of the network connection with the robot (for instance eth0) and put this connection name in the youbot_driver cpf file (cpf/youbot_driver.cpf)
• Run the script that starts the itasc application

./run.sh

• Look for the following output:

itasc fsm: STATE ENTER
root.NONemergency.RunningITASC.Running
[cartesian generator] moveTo will start from

Command the robot

• One way is to send events in the Orocos task browser through the event_firer component.
  • To send an event, e.g. "e_my_event", type in the Task Browser:

event_firer.itasc_common_events_in.write("e_my_event")

• • Possible events (as indicated on the composite_task_fsm scheme)
    • e_start_tracing: start tracing the figure, default: z off, rot on, penalize base
    • e_toggle_robot_weights: toggle between penalize base and penalize arm
    • e_toggle_z_constraint: toggle between z off and z on
    • e_toggle_rot_constraint: toggle between rot off and rot on
• (RECOMMENDED) Another, more user-friendly way, is to send events on the /itasc/ ros_common_events_in ROS topic. This can also be used in a graphical way, by using the run_eventgui.sh executable. It launches a QT based GUI that uses the configuration files in the launch directory. It will show the possible events to interact with the application as clickable buttons. You'll need to download the following code in order to use this GUI: https://bitbucket.org/apertuscus/python_gui

FAQ

• I get the message "starting from" (a unity matrix) and the simulated robot doesn't move
  • In the new Youbot model, the /odom topic changed to /base_odometry/odom, this is adapted on the master branch, or change it manually in the run_simulation.ops file. You can check whether this is causing the problem by reading the youbot.q_from_base port and checking if it returns "NoData".
• I get the error "[PropertyLoader:configure] The type 'KDL.JntArray' did not provide a type composition function, but I need one to compose it from a PropertyBag."
  • This is a KDL typekit problem, see http://bugs.orocos.org/show_bug.cgi?id=996, for now, revert the following commits of rtt_geometry (master branch on git):

54398d0653067580edd5c5ec66bda5eac0aa29e4 and 81e5fab65ee3587056a4d5fda4eb5ce796082eaf

• Where can I post questions or remarks?
  • iTaSC related questions can be asked on the Orocos users mailing list: http://www.orocos.org/user/1347/user_mailman_register#user_mailman_register
  • ROS related questions (PR2, ...) can be asked on http://answers.ros.org/
  • Gazebo related questions can be asked on: http://answers.gazebosim.org/
  • Youbot model related questions can be asked on the youbot user mailing list: http://lists.youbot-community.org/mailman/listinfo

human-PR2 comanipulation demo

Summary

comanipulation: Human-Robot Comanipulation

Installation

Dependencies

• itasc and it's dependencies
• trajectory_generators
• ROS Electric or Fuerte is required for this tutorial (core and PR2 functionality)
• the following ROS packages
  • tf
  • tf_conversions
  • geometry_msgs
  • pr2_controllers
  • pr2_kinematics
  • pr2_robot

Instructions for ROS Electric

• ROS Electric and how to find the debian packages on ros.org
• PR2 related code look at
  • real PR2
  • simulated PR2
• Orocos toolchain (use version/branch toolchain-2.5)
  • get the Orocos toolchain, if you don't have it yet, it makes sense for this application to get it the ROS way
  • sudo apt-get install ros-electric-rtt-common-msgs
  • sudo apt-get install ros-electric-rtt-ros-comm
  • sudo apt-get install ros-electric-rtt-ros-integration
  • git clone http://git.mech.kuleuven.be/robotics/rtt_geometry.git
• Orocos kinematics and dynamics
  • sudo apt-get install ros-electric-orocos-kinematics-dynamics
• rFSM
  • needs lua:
    • sudo aptitude install liblua5.1-0-dev
    • sudo aptitude install liblua5.1-0
    • sudo aptitude install lua5.1
  • rtt-lua tab completion: git clone git://gitorious.org/orocos-toolchain/rttlua_completion.git
  • git clone https://github.com/kmarkus/rFSM.git
• opencv_additions (dependencies of findFace)
  • git clone http://git.mech.kuleuven.be/robotics/opencv_additions.git
• Trajectory Generators
  • git clone http://git.mech.kuleuven.be/robotics/trajectory_generators.git
• iTaSC
  • git clone http://git.mech.kuleuven.be/robotics/itasc.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_core.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_robots_objects.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_solvers.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_tasks.git
• rtt-ros integration messages (more info)
  • rosrun rtt_rosnode create_rtt_msgs pr2_controllers_msgs

Instructions for ROS Fuerte

• ROS Fuerte
• PR2 related code look at
  • real PR2
  • simulated PR2
• Orocos toolchain (use version/branch toolchain-2.5)
  • get the Orocos toolchain, if you don't have it yet, it makes sense for this application to get it the ROS way
  • git clone http://git.mech.kuleuven.be/robotics/rtt_common_msgs.git
  • git clone http://git.mech.kuleuven.be/robotics/rtt_ros_comm.git
  • git clone http://git.mech.kuleuven.be/robotics/rtt_ros_integration.git
  • git clone http://git.mech.kuleuven.be/robotics/rtt_geometry.git
• Orocos kinematics and dynamics
  • sudo apt-get install ros-fuerte-orocos-kinematics-dynamics
• rFSM
  • needs lua:
    • sudo aptitude install liblua5.1-0-dev
    • sudo aptitude install liblua5.1-0
    • sudo aptitude install lua5.1
  • rtt-lua tab completion: git clone git://gitorious.org/orocos-toolchain/rttlua_completion.git
  • git clone https://github.com/kmarkus/rFSM.git
• opencv_additions (dependencies of findFace)
  • git clone http://git.mech.kuleuven.be/robotics/opencv_additions.git
• Trajectory Generators
  • git clone http://git.mech.kuleuven.be/robotics/trajectory_generators.git
• iTaSC
  • git clone http://git.mech.kuleuven.be/robotics/itasc.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_core.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_robots_objects.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_solvers.git
  • git clone http://git.mech.kuleuven.be/robotics/itasc_tasks.git
• rtt-ros integration messages (more info)
  • rosrun rtt_rosnode create_rtt_msgs pr2_controllers_msgs

Installation of the tutorial

Setup

• Make sure that all packages are added to you ROS_PACKAGE_PATH variable
• Source env.sh in the orocos_toolchain stack
• Set the LUA_PATH variable:

if [ "x$LUA_PATH" == "x" ]; then LUA_PATH=";;"; fi
if [ "x$LUA_CPATH" == "x" ]; then LUA_CPATH=";;"; fi
 
export LUA_PATH="$LUA_PATH;`rospack find ocl`/lua/modules/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find kdl`/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find rFSM`/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find rttlua_completion`/?.lua"
export LUA_PATH="$LUA_PATH;`rospack find kdl_lua`/lua/?.lua"
 
export LUA_CPATH="$LUA_CPATH;`rospack find rttlua_completion`/?.so"
 
export PATH="$PATH:`rosstack find orocos_toolchain`/install/bin"

Make

Don't forget to...

• Run the convert_xacro.sh script in the itasc_pr2 package:

roscd itasc_pr2
./convert_xacro.sh

The example

comanipulation layout

Execution

• Open two terminals
• go in both to itasc_comanipulation_demo_app: roscd itasc_comanipulation_demo_app
• (On a simulated PR2: open an extra terminal and start the PR2 in simulation
• run the PR2 low level controllers in the first (P controller with reduced gain): ./runControllers
• run the application in the second terminal: ./run.sh
• When your application has gone trough the configuration and starting phase, it will reach the running state: You should find a line on your screen saying "=>iTaSCFSM=>Running=>CompositeTaskFSM->Initialized State"
• To interact with the CompositeTaskFSM, you can send events to it: send events to interact (don't forget to go in and afterwards out!!!) eg.:
  • event_firer.itasc_common_events_in.write("e_parallelIn")
  • event_firer.itasc_common_events_in.write("e_parallelOut")
  • event_firer.itasc_common_events_in.write("e_obstacleForceParallelLimitsIn")
  • event_firer.itasc_common_events_in.write("e_obstacleForceParallelLimitsOut")
  • more eventnames can be found in scripts/composite_task_fsm.lua => transitions

FAQ

Joint and segment mapping

Compilation problems

• Q: When I compile itasc_solvers, I get a linking error, he can't find choleski_semidfinite...
• A: You probably forgot to source the env.sh in the orocos_toolchain stack