Author: Alessandro Di Fava < alessandro.difava@pal-robotics.com >

Maintainer: Jordi Pages < jordi.pages@pal-robotics.com >

Support: tiago-support@pal-robotics.com

Source: https://github.com/pal-robotics/tiago_dual_tutorials.git

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

Playing pre-defined upper body motions

Description: Tutorial on how to play back pre-defined upper body motions with TIAGo++ using the play_motion package.

Keywords: play_motion, joint_trajectory_controller, motion planning

Tutorial Level: INTERMEDIATE

tiago_play_motion_title.jpg

Purpose

The purpose of this tutorials is to show how to run pre-defined upper body motions with TIAGo++. This is pretty easy by using the play_motion package, which enables executing simultaneous trajectories in multiple groups of joints.

Pre-requisites

First make sure that the tutorials are properly installed along with the TIAGo++ simulation, as shown in the Tutorials Installation Section.

Execution

First open two consoles and source the public simulation workspace as follows:

  • $ cd ~/tiago_dual_public_ws
    $ source ./devel/setup.bash

Launching the simulation

In the first console launch for example the following simulation

  • roslaunch tiago_dual_gazebo tiago_dual_gazebo.launch public_sim:=true world:=empty

Gazebo will show up with TIAGo++ in an empty world.

When the simulation is launched, the play_motion action server is automatically launched and the motions defined in tiago_dual_bringup/config/motions/ related to the current configuration of the robot have been loaded in the ros parameter server.

In order to see the names of the motions defined run the following instruction in the second console:

  • $ rosparam list | grep  "play_motion/motions" | grep "meta/name" | cut -d '/' -f 4

For the version of TIAGo++ with Hey5 hands and force torque sensor on each wrist the following motions are available in the public simulation:

  • close_both
    close_left
    close_right
    home
    home_left
    home_right
    horizontal_reach
    offer
    offer_left
    offer_right
    open_both
    open_left
    open_right
    reach_floor
    reach_floor_left
    reach_floor_right
    reach_max
    reach_max_left
    reach_max_right
    vertical_reach
    wave

The relevant information of a motion, i.e. the wave motion, is stored in the following parameters:

  • /play_motion/motions/wave/joints
    /play_motion/motions/wave/points

In the case of the wave motion the joints parameter specifies which joints intervene in the motion:

  • $ rosparam get /play_motion/motions/wave/joints
    [arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, torso_lift_joint]

which denotes that in order to execute this motion the group of joints of the arm will be used.

The joint trajectory, i.e. timed sequence of positions (see http://wiki.ros.org/joint_trajectory_controller for details), that composes this motion can be retrieved as follows

  • $ rosparam get /play_motion/motions/wave/points
    - positions: [-1.1, 1.4679, 2.714, 1.9347, -1.5703, 1.3699, 0.0008, -1.1, 1.4679,
        1.2, 1.9348, -1.571, 1, 1.57, 0.15]
      time_from_start: 0
    - positions: [-1.1, 1.4668, 2.714, 1.9331, -1.5703, 1.3698, 0.0009, -0.0002, 1, 0,
        1.7, -1.5699, 0.5, 1.57, 0.15]
      time_from_start: 2
    - positions: [-1.1, 1.4658, 2.714, 1.9315, -1.5703, 1.3698, 0.0009, 0, 0.5, -0.0002,
        2.2105, -1.5698, 0.3, 1.57, 0.15]
      time_from_start: 3.2
    - positions: [-1.1, 1.4648, 2.714, 1.9299, -1.5703, 1.3698, 0.0009, 0, 0.6, -0.0002,
        1.7, -1.5698, -0.3, 1.57, 0.15]
      time_from_start: 4.4
    - positions: [-1.0999, 1.4637, 2.714, 1.9282, -1.5702, 1.3698, 0.001, 0, 0.5, -0.0002,
        2.2869, -1.5698, 0.3, 1.57, 0.15]
      time_from_start: 5.6
    - positions: [-1.1, 1.4648, 2.714, 1.9299, -1.5703, 1.3698, 0.0009, 0, 0.6, -0.0002,
        1.7, -1.5698, -0.3, 1.57, 0.15]
      time_from_start: 6.8
    - positions: [-1.0999, 1.4637, 2.714, 1.9282, -1.5702, 1.3698, 0.001, 0, 0.5, -0.0002,
        2.2869, -1.5698, 0.3, 1.57, 0.15]
      time_from_start: 8
    - positions: [-1.1, 1.4679, 2.714, 1.9347, -1.5703, 1.3699, 0.0008, -1.1, 1.4679,
        1.2, 1.9348, -1.571, 1, 1.57, 0.15]
      time_from_start: 10
    - positions: [-1.1, 1.4679, 2.714, 1.7095, -1.7012, 1.3898, 0, -1.1, 1.4679, 2.714,
        1.7095, -1.7012, 1.3898, 0, 0.15]
      time_from_start: 12.5

Each row of positions denote the position that each joint has to attain at the given time with respect to the start of the motion.

Playing motions

As said, the play_motion is an action server. In order to run a motion the following graphical action client can be used

  • $ rosrun actionlib axclient.py /play_motion

axclient_play_motion.jpg

In the GUI the name of the motion to run must be filled. The skip_planning is set by default to False, which means that the transition to the first position of the joint trajectory will use planning to prevent collisions. The remaining of the trajectory is always executed without planning as it is responsibility of the programmer to define a safe trajectory. Set skip_planning to True only if you are sure that it is safe to reach the first positions in the trajectory from the current kinematic configuration of the robot.

By pressing the SEND GOAL button the request will be sent to /play_motion Action Server and the motion will start executing as shown in the pictures.

tiago_dual_wave.jpg

tiago_dual_offer.jpg

tiago_dual_reach_floor.jpg

tiago_dual_reach_max.jpg

tiago_dual_horizontal.jpg

tiago_dual_vertical.jpg

C++ and Python action clients

In tiago_tutorials/run_motion there are play_motion action clients in C++ and Python.

The usage of the C++ action client node is as follows

  • rosrun play_motion run_motion wave

where the argument is the name of the motion. If no motion is given the list of available motions will be printed out.

The Python action client node is run as follows

  • rosrun play_motion run_motion_python_node.py home

Wiki: Robots/TIAGo++/Tutorials/motions/play_motion (last edited 2019-09-09 07:44:40 by AlessandroDiFava)