Simulation final assessment with simple and incomplete example code

In this workshop we are going to try and put everything that we have learned so far into a single program that can execute the Final Assessment tasks. This will require being able to code with ROS 2, transforming between different coordinate frames, analysing and computing kinematics, and computing joint trajectories.

The main tasks that your code needs to achieve are:

1. get position of task objects A B C

2. find joint configurations reaching the 3 objects with end-effector

3. plan trajectories according to task instructions

4. execute trajectory with grasping actions

Setup

0. if not already present, clone the repo: git clone https://github.com/LCAS/RBT1001.git

   NOTE: If you are on a Windows PC the following two additional steps are required before cloning:

   • Open a terminal(e.g., window's powershell), type git config --global core.autocrlf false and press Enter
   • Make sure docker is running by launching the docker desktop application

1. Open a terminal and move to the RBT1001 folder; for example: cd ~/RBT1001

2. git checkout tiago-gazebo

3. Now open the folder in VSCode and launch the webcontainer as usual.

Starting everything

To start the simulation and RVIZ:

1. Open a terminal and launch ros2 launch tiago_gazebo tiago_gazebo.launch.py moveit:=true to start the simulation.
2. Open the noVNC window with your browser. You should see the simulation of the robot running
3. Launch rviz in another terminal for visualising the trajectories you plan before execution: rviz2 -d src/week8/config/display_traj.rviz

To start the assessment demo code

There is a script that will be used also for your assessment that spawn three objects in space. These are the A, B and C objects that your robot need to reach and grasp based on the task you have been assigned.

1. Open a new terminal, launch python3 src/assessment2/scripts/spawn_objects.py

This code creates virtual objects in gazebo, and also broadcasts the objects frames (frames A,B,C) with respect to the robot's base_frame.

You should see them both in gazebo and rviz:

Note: every time you relaunch the script, it will respawn the three objects in a randomised location.

Obtaining taget object positions

Given that the spawn_objects.py script advertises the frames of the objects w.r.t. the base_frame, their pose can be retrieved using the `tf2 functionalities as seen in Week 2.

If you open the code week9/scripts/example_assessment_task.py, you will find an example of retrieving an object pose with respect to the arm_1_link frame.

Computing inverse kinematics

The following is the arm in the zero configuration (all the joints have value = 0.0):

The example_assessment_task.py code, contains an example of how to compute a joint configuration that touches an object. Since the tiago robot's arm is highly redundant, there are infinite configurations that can be found to achieve most of the end-effector poses. However, you do not necessarily need to use all the joints all the time and can fix some of them to a static configuration throughout the task.

The code provided makes the following semplification assumptions:

• arm_1_link is always oriented such that x1 points intersects with the z axis of the objects in cosideration.
• the third joint is fixed at -pi.
• the last three joints (the wrist joinst) are fixed at <0.0, 0.0, 0.0>
• The previous three contraints, simplifies our 7 DOF problem to that of a 2 DOF revolute-revolute planar arm by keeping all the links co-planar (they lie on the same plane), we can solve the problem by simply solving for q2 and q4. In this case, l1 is the distance from arm_2_link to arm_4_link and l2 is the distance from arm_4_link to the centre of the end-effector fingers.

This is solved with the solution from the lecture:

Note: The code does not take into account joint limits and self-collisions.

1. How would you improve it to consider limits and self-collisions? (joint limits are specified in the xacro files)

2. How can you find alternative joint configurations to achieve the task?

Using the gripper

For this assessment, you simply need to be able to open and close the gripper as atomic actions. This can be done by calling a ROS 2 action, as demonstrated in the example_assessment_task.py code.

Computing the trajectories

Once you have found suitable joint configurations for the robot, you can generate joint trajectories from the current state to the desired state as we have done in Week 8.