Task Tutorial

Solving a complex task

In this tutorial, we want to show the user how to solve a complex task by using the already created services. The simulation environment starts with a table, a garbage can and the mobile robot from mobile tutorial 3.

 

A box stack is positioned on the table. The number of stacked boxes can vary from one to three; additionally the x- position of the stack is randomly generated.

 

Main Task:

The main task in this tutorial is to drive the platform in front of the table, grab the top box from the stack and drop it into the garbage can.

Dividing the main task into sub tasks:

This whole task can be reduced to 3 problems:

• Find the table and drive the platform to it
• Identify the box stack and grab the top box
• Drive the platform to the garbage can and drop the box.

 

These tasks are implementedwith the help of a state machine. As the handlers for the laser sensors on the platform and the arm are triggered 4 times a second, they are used to start the execution of the state machine. We define two internal Ports to which we post the sensor values, when the handlers are called.

For reasons of synchronization, we register the handlers for the laser sensors non-persistant, every time we need new laser data. Before we can do this, we need to delete all data on the notification port, except the last one. Additionally, we define a join for the laser data ports to call the state machine method whenever both laser data values (for the arm AND the platform) are available.

 

Problem 1:
Find the table and drive the platform in front of it.

Solution:

1.

The platform moves forward until the table appears as a peak (sensor data values greaterless than 8000) on the very right side of the platform sensor data. At the same time, the robot arm drivesmoves PTP style into a position, which enables the webcam on the end-of-arm to see the environment. This action is performed asynchronous, since it must be finished before the platform reaches the table. Otherwise some errors can occur in the current algorithm. These movements are called from the start handler of the TaskTutorial. As soon as the table is detected to the right, the platform stops (TransitionSearchTable). While performing motions of the platform or the arm, we set the flags ArmIsMoving or PlatformIsMoving of the state to indicate what motion is in progress at the moment.<

2.

The platform rotates 80 degrees towards the pickup table. We chose 80 degressdegrees instead of 90 degrees so that the platform reaches the table with some deviation in orientation (TransitionRotatePlatform).The ‘_state._platformIsRotating’ flag is used only for notifications to the dashboard, to avoid the task tutorial to be reset,whenwhileathe rotation is in progressexecuted.

3.

Now the platform can drive towards the table which might not be straight ahead. For this reason, the platform searches for the minimum distance value in the a peak near the middle of its laser scanner data and calculates the deviation from the middle. If the deviation is greater than a specified value in the state, the platform adjusts its orientation by slowing down one wheel (by subtracting ‘_state.DriveSpeedDifferenceCorrections’). Else both wheels drive the same speed.

 

To allow the platform to drive at higher velocities without loosing accuracy, its speed is reduced step by step, when it comes closer to the table. As soon as the laser sensor reports a distance below a specified value, the platform stops (TransitionMovePlatformToDestinationObject).

4.	When arriving at the pickup table, the arm moves down using a simple PTP motion to a position where the laser scanner can measure the relative orientation of the table plate (Transition MoveArmPTP).

 

 

5.

The current arm position is used to correct the orientation of the platform. For this purpose the distance values of the arm sensor are used. We take two values that have a distance of 50 indexes from the middle of the sensor data array and calculate the difference between their sensor values. If this difference is greater than a specific value specified in the state, the platform corrects its orientation by rotating until it stands parallel to the table (TransitionAdjustPlatformOrientation). Otherwise the drives are stopped and the next state is reached.

 

This results in the following state machine that is programmed inside the handlers for the arm and platform laser range sensors. On the left side, the states that are implemented in the platform sensor handler are shown, and on the right side the ones that use the arm laser sensor.

 

Problem 2:
Identify the box stack and grab the top box

Solution:

1.

After correcting the orientation of the platform, the arm moves above the table to scan to position of the box stack. (Transition MoveArmPTP)

2.

The box stack, which stands at a random x – position on the table, appears as a peak on the sensor data of the arm’s LRF. With this data, the box position relative to the arm can be calculated. As a next step, the arm moves to this position. The position may be a bit inaccurate since the box was measured from the side, so it gets measured again after the arm motion is finished. If the current arm position differs too much from the newly calculated box position, the arm position is adjusted, otherwise the next state is reached (TransitionMoveArmToBoxPileMoveArmToBoxStack). After a notification of a finished arm movement arrives, we wait for half a second, to let the arm swinging arm reach its final position.

 

3.

To measure the size of the box stack, the arm stays at its position and just rotates the end-of-arm joint by 90 degrees. Now the laser sensor draws a vertical line on the box stack and the position of the topmost box can be calculated. (Transition ScanBoxPileHeight)

 

4.

The platform starts to drive closer to the table into a position in which the arm can easily reach the box stack. The speed of this movement is rather slow, because otherwise the platforms position becomes too inaccurate. At this point we use the above mentioned ‘MovePlatformToDestinationObject’ transition again. At the same time the position of the top box is calculated and the arm moves to a position above in front of itthe box stack (Transitions MoveArmInFrontOfBoxStack and MovePlatformToDestinationObjectare executed concurrently)

5.

After the platform finished its movement AND the arm is in front of the top box, the distance from the TCP to the top box gets measured. If the arm stops before the platform reaches its final position (PlatformIsMoving flag is true), the above shown “if” case is executed to ensure, that the platform stops at the specified distance. Else the notification handler for the arm was executed after the platform stopped and we can go directly to the next state. The arm moves into a position directly in front of the box stack and performs some more distance measurements to adjust the gripper position along the x-axis if necessary. This ensures that the box is in the middle of the gripper. Afterwards, the arm moves forward with a LIN motion. For all these LIN movements, the destination position is calculated with help of the data from the laser sensor on the arm. (TransitionMoveArmToGripPosition).

6.	The gripper closes (TransitionCloseGripper) This method is rather simple: After the successful response message for closing the gripper was received, the next state is reached.

 

The next picture shows the above described states and transitions together with the states and transitions of solution to problem 1 in one state diagram. Below that we show the implementation in the state machine.

 

Problem 3:
Drive the platform to the garbage can and drop the box

 

Solution:

1.

The arm performs a PTP movement to get the robot into its drive position, where the box is held in front of the robot. We wait 1.5 seconds before the platform starts to rotate around 180 degrees. This is done to avoid knocking the table over before the arm is sufficiently retracted (above described TransitionsRotatePlatformandMoveArmPTP).

2.

The platform starts to move towards the garbage can. It measures the peak that this entity creates in the platform laser data and tries to keep it in the middle of this array (see also description and code sample under transitionmovePlatformToDestination-Object). When the platform reaches the garbage can, the speed decreases until it stops.

3.

At the garbage can the platform rotates by 90 degrees so is aligned towards the ramp. Concurrently, the arm moves with the box into its drop position over the garbage can. (Transition getIntoDropPosition).

 

 

4.

After both of the previous motions finished, the platform reaches its final orientation by trying to get the ramp echo into the middle of the platform laser sensor array. For this it calculates the middle of the ramp in the sensor data by knowing both borders of the ramp. Afterwards the platform rotates as long as the ramp is not straight ahead (Transition AdjustPlatformOrientation).

5.	The platform starts to drive quite slowly towards the ramp (Transition DriveSpeed).
6.	After the garbage can is at a specified position left to the platform, the gripper opens and the box falls into the can (Transition DropBox).

 

7.	The arm performs a PTP movement into its fast-drive position (pointing backwards for stability as well as aero-dynamical reasonsJ). This is done in the transition MoveArmPTP.

8.

The platform increases its speed every laser sensor notification by 2 until after 3,75 second the drives stop their movement. At this time, the platform should have reached the end of the ramp and dropped to the ground again. After 25 laser sensor notifications, either the tutorial and the simulation environment are reset to the original configuration or the idle state is reached. This is decided through the current state of the ‘Reset application’ checkbox.

 

 

By implementing all these states and transitions, we obtain the following state diagram:

Services for the Task Tutorial

 

As shown in the figure above, there are 3 new services for the task tutorial:

• TaskTutorialDashboard:   User Interface to show webcam image, visualized laser range finder data, current joint angles of robot arm and the status of the TaskTutorialApplication Service. The task tutorial can be started or reset.
• TaskTutorialApplication:   This is the main service of the task tutorial. It contains a state machine and all used transition methods to solve the task.
• TaskTutorialSimulation:    Starts the simulation environment and builds up the initial situation for the task tutorial.

The diagram shows the two connections between the dashboard and the TaskTutorialApplication service.

Channel to the TaskTutorialApplication service:
Contains operations for starting the solution of the task and resetting the state of the service and the simulation environment. The first one initializes some state values and establishes a connection to the laser sensors if the has not already been done. Afterwards a response is sent back to the source service. The state machine is now in its initial state and the task solution begins. The last operation resets the used variables, stops the platform, opens the gripper and drives the arm into its starting position. After that, it unsubscribes itself from the laser sensor services and resets the simulation engine. This operation can not be executed if the platform is rotating, waiting for a notification or if the arm is moving. If we would allow this operation during those circumstances, method calls to non existing visual entities may occur that result in exceptions, or the application may run into a deadlock while waiting for a notification that is not sent.

 

Channel from the TaskTutorialApplication service: Notifications containing status information about the current state in the state machine. The transmitted boolean values are used to enable and disable the shown “Start Application” and “Reset Application” buttons.

 

Furthermore the dashboard receives notifications from the following services:

• Motion Planning:
  When arm movements are finished, the current arm angles are displayed in the GUI.
• Simulated LRF:
  The laser data shown in the GUI is updated four times a second.
• Simulated Webcam:
  To receive a new webcam image, the dashboard sends every 250 ms an ‘QueryFrame’ message to obtain the current frame.
  

The received frame data is converted into a bitmap that is shown in the GUI.

 

The task tutorial can send a ResetSimulation operation to the TaskTutorialSimulation service. The simulation service then deletes all shown visual entities as well as the joints between them and re-inserts them at the initial position. Hence, the task can be solved again.

 

Repopulating the world would normally start the controlling services for each entity. This has to be avoided when the simulation engine is reset, because the services are already running. For this reason, we use a flag that is set after the first initialization of the simulation environment. Now the starting of a service looks like this: