Upper body motion and play_motion2

PAL robots come equipped with a motion engine designed to execute predefined movements involving the upper body’s joints. It includes a default library containing various motions, and users have the flexibility to integrate new motions, playable at their convenience. The motions engine provided with is based on play_motion2 ROS 2 package.

Within this package lies a ROS 2 Action Server /play_motion2, functioning as a demultiplexer, that sends goals to various action servers, each responsible for controlling different groups of joints. This architecture enables efficient management and coordination of joint commands.

The figure below shows the role of play_motion2 in order to play back predefined upper body motions.

The following tables enumerates the various groups of actuated joints corresponding to different robots

TIAGo ProTIAGoTIAGo++

Group of joints	Joints included
Torso	torso_lift
Head	head_1, head_2
Arm left	arm_left_1, arm_left_2, arm_left_3, arm_left_4, arm_left_5, arm_left_6, arm_left_7
Arm right	arm_right_1, arm_right_2, arm_right_3, arm_right_4, arm_right_5, arm_right_6, arm_right_7
Gripper left	gripper_left_finger
Gripper right	gripper_right_finger

The motions reproduced by play_motion2 are formulated as sequences of joint positions to be achieved within predefined time intervals. These motions are articulated within configuration files, delineating the activated joint groups and the trajectory to replicate. Additionally, considering that the initial joint positions may be further than expected, an approach time to reach the first position is calculated as a safety measure. To ensure safety and precision, a motion planner can be configured to generate a trajectory using MoveIt 2, which is then integrated with the original motion. This guarantees the safe and accurate execution of the motion, starting from the initial position and proceeding smoothly through the predefined trajectory. Users can specify the details of the motion planner within a dedicated configuration file. For detailed instructions on these configuration files, consult the play_motion2 documentation

Motions library

TIAGo ProTIAGoTIAGo++

The motion library is stored in tiago_pro_bringup/config/motions/tiago_pro_motions_general.yaml, containing motions not moving the end-effector joints. The files tiago_pro_bringup/config/motions/tiago_pro_motions_X-Y.yaml, contain motions that include the end-effector joints, where X refers to the type of left end-effector and Y to the right end-effector. Furthermore, the motion planner is configured in tiago_pro_bringup/config/motion_planner/motion_planner_X-Y.yaml The paths of these files are passed as parameters when the play_motion2 action server is launched. The yaml sorting the predefined motions file can be edited as follows:

colcon_cd tiago_pro_bringup
cd config/motions/
nano tiago_pro_motions_general.yaml

While the yaml file configuring the motion planner can be edited as follows:

colcon_cd tiago_pro_bringup
cd config/motion_planner/
nano motion_planner_X-Y.yaml

The list of motions already defined in the library can be retrieved with the following command:

ros2 service call /play_motion2/list_motions play_motion2_msgs/srv/ListMotions

New motions can be added to the library by editing the yaml file in the tiago_pro_bringup package.

The /play_motion2 action plays back the desired motion, in order to have more information about the goal field and how to send it to the action, refer to play_motion2 documentation.