Arm Tutorial 4
Arm Tutorial 4 project build order
A) If you have built all the other projects from the previous solutions step by
step, you just need to build the projects in Column A.
B) If you havent built any of the previous tutorial solutions, you need to build
all included projects in the order shown in Column B.
Column A
1. KUKATutorial4Orchestration
2. KUKATutorial4Dashboard
Column B
1. Util
2. KUKACommandTypes
3. ArticulatedArm (in abstract
services)
4. KUKAUniversalMotionPlanning (in
abstract services)
5. SimulatedLBR3Arm
6. KUKAArmTutorialSimulation
7. Transformation
8. KUKATutorial3MotionPlanning
9. KUKATutorial4Orchestration
10. KUKATutorial4Dashboard
Queuing motion statements
Industrial robot programs usually consist of long series of motion commands.
These motion commands describe paths that move a tool to its destination
positions, e.g. a grip position, a place position, a tool-reload position, or of
course process positions like welding spots or gluing paths. Often the paths
between process positions and non process positions need a couple of extra
points that help maneuver the robotic safely in a very crowded cell.
Even today in industrial environments, path positions for a robot arm are
usually recorded manually in a cell-ramp-up phase. For this, the operator
brings the robot arm into position and uses a record function to tell the robot
how to move to this position within the path. These recording steps generate
the list of motion commands. In more modern cases, paths come predefined
out of a CAD/CAM system. But even in these cases human operators usually
need to adjust path positions to fit the real cell environment, since the
simulation model and the reality never match completely. So also in the halfautomated case robot operators need to deal with lists of adjacent motion
commands.
In this tutorial we want to show a way of how we can queue motion
commands for our simulated robot arm. The challenge in this highly parallel
environment is not so much about where to define the queue, but how to wait
with the next command until the arm has finished the current command.
The dashboard has now a list control that queues motion commands. After a
cartesian or axis-specific target position has been entered, choosing the
motion type (PTP or LIN) will add a motion command to the queue.
Unlike the previous tutorials, the robot will only move after the Start button
triggers the execution of motion commands within the queue.
In the following example, we queue three motions:
1) PTP motion to the cartesian position (-0,2; 0,2; 0,2);
2) LIN motion to the cartesian position (0,2; 0,2; 0,2);
3) PTP motion to the axis position (0, 0, 0, 0, 0, 0, 0);
Now we have a very simple path for the arm. It would be nice of course if you
could save or load these robot programs. A simple serialization of the
command queue would already be a start, but since this is out of scope for
this tutorial we did not include this in our source code.
When pressing the start button, the upper most command in the queue is
processed. As soon as the motion is finished, the command is taken out of the
queue and the next command if available is processed. After the
processing of each command, the end-of-move notification for the
orchestration also contains the current position as axis values and in cartesian
coordinates. These are then posted to the dashboard. After finishing the
movement, all commands reappear in the queue and they can be executed
again.
PTP
PTP
LIN
You will notice that the arm makes a short stop at the end of each motion.
This is due to the fact that every motion is planned by itself with its ramps for
acceleration and deceleration. Commercial robot controllers usually offer the
programmer to define blended motions. When interpreting such a motion, the
controller knows that it shouldnt come to a stop at the destination point, but to
continue with the next motion without loosing too much speed. To enable the
arm to execute such a motion, the programmer needs to allow a certain path
deviation around the destination point.
blended LIN
LIN
actual end-of-arm path in a blended motion
The blending algorithm is also out of scope for this tutorial and might come
with a future release. For now, since we offer the programmer to look
programmatically at several motion commands in the queue at the same time
before motion execution, we encourage our readers to try generating a
blending algorithm at this point.
Coding for Arm Tutorial 4:
All commands coming from the dashboard are sent to the orchestration
service. So the only partner the dashboard needs to know is the orchestration
service. In this case, it might seem overkill to insert an extra orchestration
service between the dashboard and the arm. But already in the second mobile
tutorial, when new devices are attached and controlled through the
dashboard, the benefits of the orchestration service become clear.
Arm Tutorial 4 Services Overview:
New services for arm tutorial 4:
ArmTutorial4Dashboard:
Orchestration:
User Interface with an added list
control for queued motions
Queues motion commands for the
robot arm
The diagram shows the two connections between the dashboard and the
orchestration service:
- Channel to the orchestration service: Adding commands, deleting
commands and start of the current
command queue
- Channel from the orchestration service: Notifications about changes in the
current command queue (e.g. when a
command has finished)
Sending commands to the orchestration service
In the following code snippet you see how a request for a PTP motion from
the dashboard service is processed. A new instance of the type PTPMove is
wrapped into a DSSPOperationWrapper class. This wrapping is needed to
be able to send the PTPMove within the body of a CommandRequest. (ump
stands for the universal motion planning proxy, where the PTPMove type is
defined.)
/// <summary>
/// handler for PTP movements to a point defined by angles
/// </summary>
/// <param name="ptp">ptp parameters</param>
/// <returns>tasks to perform</returns>
IEnumerator<ITask> OnDrivePTPHandler(OnDrivePTP ptp)
{
//Generate Operation
[Link] op = new [Link]();
[Link] req = new [Link]();
[Link] = [Link];
[Link] = [Link];
[Link] = req;
//Send operation to orchestration service
yield return [Link](
_orcMainPort.AddCommand(new [Link](new [Link](op))),
delegate(DefaultUpdateResponseType resp)
{
LogInfo("Successful Response received");
},
DefaultFaultHandler
);
yield break;
Then the wrapped object is put into a AddCommand request and sent to the
orchestration port _orchPort.
}
The PTPMove object is ultimately derived from the abstract class
CommandType. This base class type is used for the command queue itself,
so that all incoming command requests can be handled the same within the
orchestration service.
Filling the command queue
The command queue is defined as a List of DsspOperations.
/// <summary>
/// Kukatutorial 4orchestration State
/// </summary>
[DataContract()]
public class Kukatutorial4orchestrationState
{
List<DsspOperation> _operations = new List<DsspOperation>();
public List<DsspOperation> Operations
{
get { return _operations; }
set { _operations = value; }
}
}
Any incoming command is processed in the AddCommandHandler. It is
required for each command to be derived from CommandType. If this is the
case, the command is added to the command queue (_state.Operations) and
is inserted into a notification object (not), which holds all current commands
in a list. This List is then sent back to the Dashboard through the subscriber
mechanism, so that the Dashboard can now update its queue display with the
current remaining commands in the queue.
/// <summary>
/// Handler for adding commands to the queue
/// </summary>
/// <param name="command">command parameters</param>
/// <returns>tasks to perform</returns>
[ServiceHandler([Link])]
public IEnumerator<ITask> AddCommandHandler(AddCommand command)
{
//Only add CommandTypes to the OperationsToDo List and the notification message
if ([Link] is [Link])
{
_state.[Link]([Link]);
[Link]([Link] as [Link]);
}
else throw new Exception("Command type not supported!");
[Link]([Link]);
//update notification and send it
[Link] = null;
[Link] = null;
[Link] = false;
[Link]<Notification>(_subMgrPort, not);
yield break;
}
Executing the command queue
As soon as a Start message arrives at the orchestration service, the start
message handler will process the command queue. As long as commands
are in the queue, the first command is examined to see the general type of the
command. Depending on the type, the orchestration service decides on where
to forward this command. In the following code snippet, you can see that the
[Link] is checked for being a UniversalMotionPlanningRequest. If that is
the case, the command is sent to the Motionplanning port (_mpPort), and two
anonymous handlers are activated for a fault or a success response.
The command TryPostUnknownType is used because the operation that is
sent is of the general type DSSPOperation. TryPostUnknownType will
check by itself if any of the main port handlers of the target service can handle
this operation. If not, this function call returns false.
while (_state.[Link] > 0)
{
op = _state.Operations[0];
//Send message to Motion Planning service
if ([Link] is [Link])
{
success = _mpPort.TryPostUnknownType(op);
//Handle the result, especially the fault
PortSet<[Link], Fault> resp = [Link] as
PortSet<[Link], Fault>;
yield return [Link](
resp,
delegate([Link] umpr) { },
delegate(Fault f) {
[Link] = f;
[Link] = [Link];
[Link]<NotificationFault>(_subMgrPort, notFault);
exceptionInTargetService = true;
}
);
}
if (exceptionInTargetService)
yield break;
if (!success) LogError("Command not accepted by receiver!");
//update state and notification
_state.[Link](0);
_state._finishedOperations.Add(op);
[Link](0);
//Wait for notification
yield return ([Link]<[Link]>(
false, _mpNot, delegate([Link] not2)
{
[Link] = [Link];
[Link] = [Link];
[Link]<Notification>(_subMgrPort, not);
}
));
}
//After finishing the sequence, reset the motion commands and send notification
foreach (DsspOperation operation in _state._finishedOperations)
{
_state.[Link](operation);
[Link](([Link])[Link]);
}
_state._finishedOperations.Clear();
[Link] = true;
[Link]<Notification>(_subMgrPort, not);
[Link] = false;
yield break;
}
Now that the motion planning service has answered the sent command with
either a success or a fault message, the code waits for a notification that
indicates that the motion is finished. Therefore a handler for the notification
NotificationMoveFinished is activated.
If the motion planning service answered the command positively with a
UniversalMotionPlanningResponse, it will send a NotificationMoveFinished
as soon as the motion has finished execution by the arm. Then the destination
values of this position are written into a notification object from the
orchestration service. This notification then is sent to all subscribers. In our
code, this notification is received by the dashboard service that now shows
the new position values of the end of arm. Finally, the original command
queue is restored and sent to the GUI.