This documentation will provide insights into the Tiago robot's manipulation system, along with the developments I have made within the team over the course of three months dedicated to the project.

By integrating all existing manipulation capabilities with MoveIt!, a widely adopted motion planning framework , the robot has been able to compute smarter trajectories while considering the environment through Octomap.

Here is a Moveit tutorials documentation provided by ROS planning (especially Move Group Python Interface): Getting Started — moveit_tutorials Noetic documentation

The pick_motions.yaml file contains predefined motion sequences for various manipulation tasks, with the name of each motion added alongside the position of each joint.

play_motion:
  motions:
    head_look_around:
      joints: [head_1_joint, head_2_joint]
      points:
      - positions: [0, 0]
        time_from_start: 2.0
      - positions: [0, -0.7]
        time_from_start: 4.0
      - positions: [-0.7, -0.7]
        time_from_start: 6.0
      - positions: [-0.7, -0.5]
        time_from_start: 9.0
      - positions: [0, -0.5]
        time_from_start: 12.0
      - positions: [0.7, -0.5]
        time_from_start: 15.0
      - positions: [0.7, -0.7]
        time_from_start: 18.0
      - positions: [0, -0.7]
        time_from_start: 21.0
      - positions: [0, -0.5]
        time_from_start: 24.0

Before executing any motion, you'll need to load parameters from the pick_motions.yaml file, executing the following command:

rosparam load `rospack find tiago_manipulation`/config/pick_motions.yaml

After loading the parameters, you can execute predefined motions using the run_motion command. For example, to make the robot's head look around, run:

rosrun play_motion run_motion head_look_around

There are several other motions in the file, including the following motion, created for pointing towards people during the Robocup's Receptionist task.

########################### PEOPLE POINTING ###########################   
    pointing:
      joints: [arm_1_joint,
      arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint,
      arm_6_joint, arm_7_joint]
      points:
      - positions: [1.56, -1.50, -3.14, 2.19, 1.59, 0.58, 0.0]
        time_from_start: 10.0

The manipulation.py script located in isr_tiago/planning/actions_tiago/src/actions_tiago_ros/manipulation provides functionalities for manipulation actions.

After adding the "point" motion to the **pick_motions.yaml**configuration file, it is necessary to include a function in this Python script so that, in addition to being called by the explained command, it can also be invoked through the Tiago API. The "point()" function, along with other similar functions within this script, moves the robot to a pointing pose by executing predefined motions.

def point(self):
		rospy.loginfo("Moving to point pose...")
		self.start_predefined_motion()
		self.run_predefined_motion(motion_name='pointing', wait=True, run_safely=True)
		self.run_predefined_motion(motion_name='close', wait=True, run_safely=True)

Here are some quick tutorials from the ROS Wiki on getting started with TIAGo's motion planning and Octomap: Robots/TIAGo/Tutorials/MoveIt - ROS Wiki

Octomap is a probabilistic occupancy mapping framework used in robotics to represent three-dimensional environments. It divides the environment into small volumetric cells, typically voxels, and assigns a probability value to each cell to indicate the likelihood of it being occupied by an obstacle. This representation allows robots to navigate and interact with their surroundings effectively.

The octomap is generated using the following command, which establishes a connection between the topic originating from the camera's point cloud and the topic been read by MoveIt!.

rosrun topic_tools throttle messages /xtion/depth_registered/points 2  /throttle_filtering_points/filtered_points

Before starting manipulation, it's important to execute the "head_look_around" motion so that it builds the octomap and captures the entire environment near the robot:

rosrun play_motion run_motion head_look_around

The octomap has an update rate, but it can always be refreshed using the service:

/clear_octomap

This service can be invoked both in the robot's code and by a button in the motion planning section of RViz.

The MoveIt Setup Assistant is a graphical user interface for configuring any robot for use with MoveIt. Its primary function is generating a Semantic Robot Description Format (SRDF) file for your robot. Additionally, it generates other necessary configuration files for use with the MoveIt pipeline.

This package is built upon your robot's URDF. Any alterations to the URDF necessitate regenerating the package to update its collision matrix. This ensures the robot remains constantly aware of its state, facilitating trajectory planning that avoids self-collisions.

A faster and more straightforward method exists to update this matrix. Just execute the following command. !! IMPORTANT: Executing this command will recreate the SRDF file, potentially erasing any previously created group states. Ensure you have a backup of these poses before proceeding.

rosrun tiago_moveit_config collisions_updater --help

A MoveIt! configuration package typically includes essential files for setting up and configuring a robot's integration with MoveIt!.

move_group.launch: This launch file initializes MoveGroup, which provides a high-level interface for robot motion planning. It offers services for trajectory planning, motion execution, and user interaction.

planning_context.launch: This launch file sets up the planning environment, including defining the robot's kinematic model, workspace settings, motion constraints, and other parameters relevant to motion planning.

sensor_rgbd.yaml: This configuration file defines parameters related to the RGB-D sensor, commonly used for perceiving the robot's surroundings. It specifies settings such as image resolution, maximum and minimum detection distances, among others. This file is where Octomap is added and it ensures that MoveIt! can utilize accurate and relevant sensor data during motion planning.

These files are crucial for configuring the MoveIt! planning environment and enabling safe and efficient robot motion in its surroundings.

An SRDF (Semantic Robot Description Format) file is a key component in configuring a robot for use with MoveIt!. It provides semantic information about the robot's structure, such as joint groups, end effectors, and allowed collision pairs.

1. Joint Groups: The SRDF file defines logical groups of joints, such as arm joints or gripper joints. These groups are used for motion planning and control purposes.
2. End Effectors: It specifies the end effectors of the robot, such as grippers or tool attachments. End effectors are important for defining grasp poses and manipulating objects.
3. Allowed Collision Pairs: This section lists pairs of links that are allowed to collide during motion planning. It helps in defining collision-free motion paths for the robot.
4. Group States: SRDF files can include predefined states for joint groups. These states represent specific configurations of the robot, such as "home" or "grasping", which can be used for planning or execution.

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /dev/stdin                     | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="tiago">
    <group name="arm">
        <joint name="arm_1_joint" />
        <joint name="arm_2_joint" />
        <joint name="arm_3_joint" />
        <joint name="arm_4_joint" />
        <joint name="arm_5_joint" />
        <joint name="arm_6_joint" />
        <joint name="arm_7_joint" />
        <joint name="arm_tool_joint" />
    </group>
    <group name="arm_torso">
        <joint name="torso_lift_joint" />
        <joint name="arm_1_joint" />
        <joint name="arm_2_joint" />
        <joint name="arm_3_joint" />
        <joint name="arm_4_joint" />
        <joint name="arm_5_joint" />
        <joint name="arm_6_joint" />
        <joint name="arm_7_joint" />
        <joint name="arm_tool_joint" />
    </group>
    <group name="gripper">
        <link name="gripper_left_finger_link" />
        <link name="gripper_right_finger_link" />
        <link name="gripper_link" />
        <joint name="gripper_left_finger_joint" />
        <joint name="gripper_right_finger_joint" />
    </group>
    <group_state name="open" group="gripper">
        <joint name="gripper_left_finger_joint" value="0.04492134120735346"/>
        <joint name="gripper_right_finger_joint" value="0.04500002058570873"/>
    </group_state>
    <group_state name="torso_up" group="arm_torso">
        <joint name="torso_lift_joint" value="0.1991896384433041"/>
        <joint name="arm_1_joint" value="0.19997370085812083"/>
        <joint name="arm_2_joint" value="-1.3313564795228423"/>
        <joint name="arm_3_joint" value="-0.19882978578096022"/>
        <joint name="arm_4_joint" value="1.9086415892559678"/>
        <joint name="arm_5_joint" value="-1.5706802129258746"/>
        <joint name="arm_6_joint" value="1.3673411332771712"/>
        <joint name="arm_7_joint" value="2.0202878109110145e-05"/>
    </group_state>
    <group_state name="home" group="arm">
        <joint name="arm_1_joint" value="0.19999291035873945"/>
        <joint name="arm_2_joint" value="-1.335775517396673"/>
        <joint name="arm_3_joint" value="-0.19938034394240134"/>
        <joint name="arm_4_joint" value="1.9244450682068184"/>
        <joint name="arm_5_joint" value="-1.5703580078388448"/>
        <joint name="arm_6_joint" value="1.3688040565359998"/>
        <joint name="arm_7_joint" value="5.5821724757265656e-05"/>
    </group_state>
    <group_state name="point" group="arm">
        <joint name="arm_1_joint" value="1.56"/>
        <joint name="arm_2_joint" value="-1.5"/>
        <joint name="arm_3_joint" value="-3.14"/>
        <joint name="arm_4_joint" value="2.19"/>
        <joint name="arm_5_joint" value="1.59"/>
        <joint name="arm_6_joint" value="0.58"/>
        <joint name="arm_7_joint" value="0.0"/>
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector group="gripper" name="gripper" parent_group="arm_torso" parent_link="arm_tool_link" />
    <disable_collisions link1="arm_1_link" link2="arm_2_link" reason="Adjacent" />
    <disable_collisions link1="arm_1_link" link2="arm_3_link" reason="Never" />
    <disable_collisions link1="arm_1_link" link2="arm_4_link" reason="Never" />
    <disable_collisions link1="arm_1_link" link2="arm_5_link" reason="Never" />
    <disable_collisions link1="arm_1_link" link2="arm_6_link" reason="Never" />
    <disable_collisions link1="arm_1_link" link2="arm_7_link" reason="Never" />
...

This script automatically adds group states to the SRDF file. To work in the moveit_config package, which is stored on the computer inside the robot, you need to access the robot and run commands to allow the files to be editable. To do this, simply follow the 'access the robot' tutorial.

Once inside the robot, just run the following command and it will prompt you for the group and name of the pose you want to add.

rosrun tiago_manipulation save_poses.py

On the TIAGo robot, the existing groups are "arm", "arm_torso", and "gripper". If needed, additional groups can be added in your MoveIt! setup, such as creating a group for head joints or a separate group for torso manipulation.

To call these new poses in Tiago's API, all you need to do is add the execute_pose() function to manipulator.py, as described below. This should be done after the objects have been initialized in the code, as the example.

def __init__(self):
		## Instantiate a `RobotCommander`_ objects
		robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    move_group = moveit_commander.MoveGroupCommander("arm")

def execute_pose(self, move_group, pose_name):    
		move_group.set_named_target(pose_name)    
		success = group.go(wait=True)
		return success

MoveIt! provides built-in functions for adding objects to an environment. Once the robot is utilizing OctoMap, it will no longer perform simple picks as it will detect collisions with objects. Therefore, MoveIt! has created functions to add an object to the scene and attach it to the robot, ensuring that this object no longer collides and allowing the robot to grasp it.

1. addCollisionObject(): This function adds a collision object to the planning scene. The object can be defined by specifying its geometry, pose, and other relevant attributes.
2. attachObject(): This function attaches a collision object to the robot. Once attached, the object moves with the robot's end effector and is considered part of the robot's collision model.
3. detachObject(): This function detaches a previously attached object from the robot. The object then becomes stationary in the scene.
4. removeCollisionObject(): This function removes a collision object from the planning scene. It is typically used when an object is no longer needed or when the scene needs to be cleared.

These functions allow for dynamic manipulation of objects within the planning scene, enabling complex motion planning tasks such as grasping and manipulation.

It is beneficial to become familiar with these functions for adding objects to the scene, as you can extend beyond the manipulation of these objects and "assemble" a virtual environment for the robot to become more acquainted with the working environment.

Flora Aidar

• [email protected]
• +55 (11) 996279604