Arm Tutorial 1 project build order

1. ArticulatedArm (in abstract services)
2. Util
3. SimulatedLBR3Arm
4. KUKATutorial1MotionPlanning
5. KUKATutorial1Dashboard
6. KUKAArmTutorialSimulation
Moving the end of arm (EOA) from point to point
To move a tool from position A to position B using a six axis robot, you have to tell each axis of the robot which positions they should assume so that the end of arm reaches the desired position and orientation.
Position A:
Position B:
Axis specific programming:
One way to tell the robot its destination is to give it a target value for each axis.
To get to the target position the controller needs to calculate the required movements for each axis. Unless specified otherwise by the operator, the arm control takes the current axis values and calculates the shortest distances to the desired axis values
to avoid large and long movements.
Movement of one axis:
To reach the target value, the drive for an axis needs to accelerate to a certain velocity, and to decelerate in time before it reaches the target. In order to plan these times correctly you need to calculate a so called velocityprofile for the axis. The
following picture shows this profile for one axis – the upper curve shows the acceleration input for the drive, the lower picture shows the resulting velocity (LINK See the mathematical background here)
Green area: acceleration phase
Yellow area: constant velocity phase
Red area: deceleration area
The velocity profile is used to send precise axis values at predetermined time intervals to the drive. The time interval must be well adapted to the target drive. In the MSRS simulation environment, since the drives are modeled as springs, we need to find a
good interval to hide the spring nature of the drives at reasonable motion speeds. We found that we come to a nice robotic motion when using 10ms as the interval length.
Movement of multiple axis:
In order to reach the EOA target position, usually multiple axis have to move. A velocity profile has to be calculated for every axis. For the overall motion, those profiles can be coordinated in three different ways:
For the exact mathematical description of the motion calculation, please visit the MATH CORNER
Asynchronous PTP motion:
In an asynchronous PTP motion, each drive moves as fast as possible to reach the desired angle. In this case, some axis will reach their target value before others. In order to reach the final position and orientation, each axis has to have arrived at its
final value. The velocity profiles of two axis could look like the following:
In this picture, the maximum velocities are shown at different levels for the two axis, since in the real world the different drives do have different maximum velocities and acceleration capabilities.
Synchronous PTP motion:
Since the final position of the EOA is not reached before each axis has reached the target angle, one axis will need the most driving time. This axis is called the leading axis. To minimize abrasion of the drives, all drives except the one for the leading
axis should drive on a slower than maximum speed. So the time that is needed to reach the destination position is synchronized. The velocity profiles for the slower moving axis are recalculated, so that the times for arriving at the end positions match:
Full synchronous PTP motion:
Abrasion of drives is even more prevented if not only the time to reach the destination is synchronized, but also the times for acceleration and deceleration are synchronized. This is called a fully synchronized PTP motion.
Coding for Tutorial 1:
In the robotics field, calculating these profiles is called motion planning. Therefore we created a service called ArmTutorial1MotionPlanning. The ‘1’ indicates that we have different versions of the motion planning service throughout the tutorials. This
has been done for clarity reasons, to only include the necessary code for the corresponding tutorial.
The following figure shows the orchestration of the main services in ArmTutorial1:
 ArmTutorial1Dashboard: User Interface
 ArmTutorial1MotionPlanning: Calculates and executes the motion
 SimulatedLBR3: Service for the simulated LBR arm
 ArticulatedArm: Abstract Base Class for SimulatedLBR3
 KUKAArmTutorialSimulation: Startproject of the ArmTutorial1 Solution
This tutorial executes through the following steps:
1) The dashboard accepts target angle values and sends those through a PTP motion command to the motion planning service.
2) The motion planning service calculates the required times and intermediate positions for all axis and sends those values to the Simulated LBR3
The following passages show how the motion planning service calculates the different PTP types:
Asynchronous movement (method asynchronousPTP)
The waypoint calculations _{} for an axis
_{} have no dependencies to all other axes and base on the given values
_{},_{} and
_{}.
Step 1:
First we determine the maximum amplitude for the velocity ramp. We have to check whether the distance
_{}is sufficient to let the velocity grow up to
_{} or not. If not, we run into the velocity triangle situation (Fig. 3) and must limit
_{} down to
_{} [8]. Therefore we evaluate [9]:
_{}
Step 2:
Next we compute the acceleration duration _{}, the deceleration start time
_{} and the total motion time
_{}. We get the acceleration length
_{} through equation [1]. Equation [5] returns the total motion time
_{} and the deceleration start time
_{} can be calculated via [2]:
_{}
_{}
_{}
Step 3:
Now we determine the count of waypoints. The acceleration phase length _{} and the entire motion process time
_{} are divided into time slices with a period time
_{} [11]:
Step 4
Finally we compute all waypoints via [12]:
_{}
Synchronous PTP movement (method synchronousPTP)
In general the calculation for the synchronous PTP movement happen to be the same as for the asynchronous PTP movement except that the several velocities
_{} must be chosen in that way that all total motion durations coincide to
_{}:
Step 1:
First we look for the total motion time _{}. Like in the asynchronous PTP movement we compute the total movement times
_{} of all axes_{}:
_{}
_{}
_{}
The longest total motion time becomes _{}:
_{}
Step 2:
Next we calculate the velocities _{}. Each axis must finish its total motion exactly at
_{} [5]:
_{}
We solve the quadratic equation for _{}. From the two possible results the constraint
_{} annuls the larger one:
_{}
Step 3:
Since we have already the total travel time _{} we just have to calculate the acceleration duration
_{} and the deceleration start time
_{}. Like in the asynchronous PTP movement we get the
_{} and
_{} through the equations [1] and [2]:
_{}
Step 4:
For the calculation of the time slices and waypoints refer to the asynchronous PTP movement description steps 3 and 4.
Fully synchronous PTP movement (method fullySynchronousPTP)
In addition to the synchronous PTP movement, in the fully synchronous PTP movement we must also synchronize the acceleration time lengths
_{} and the deceleration start times
_{} for all axes.
Step 1:
Like in the synchronous PTP movement description step 1 we compute _{}. In the same way we also compute
_{} and
_{}:
_{}
_{}
_{}
_{}
_{}
_{}
_{}
Step 2:
The acceleration time lengths _{}, deceleration stop times
_{} and total motion times
_{} must be synchronized:
Therefore the maximum velocity and acceleration are computed individually for each axis:
_{}
_{}
Step 3:
For the calculation of the time slices and waypoints refer again to the asynchronous PTP movement description steps 3 and 4.
Math Corner
General ramp PTP movement
The point to point (PTP) ramp movement consists of three phases, acceleration, constant velocity and deceleration. The acceleration phase starts at
_{}and ends at
_{}, the constant velocity phase reaches up to
_{}and the deceleration phase ends at_{}.
In the physics the radial acceleration _{} is the derivation of the radial velocity
_{} means that the velocity equals to the area enclosed by the acceleration trajectory and the time axis. Analogous the radial velocity
_{}is the derivation of the radial angle
_{}. Vice versa one can write_{}.
E.g.: Having a constant acceleration _{}the velocity_{}grows with the time
_{} (assuming zero velocity at
_{}).
Within the acceleration phase the velocity increases from zero to _{}, remains changeless in the constant velocity phase and declines in the deceleration phase back down
to zero. At _{} as well at
_{} of the total motion process there velocity is zero. Because the absolute value of the velocities' gain rate
_{} is the same as the absolute value of the declining rate
_{} the durations for both acceleration and deceleration conform (Fig. 1).
Fig.1: Relation velocity ramp and acceleration
Presumed a constant acceleration _{}, the velocity reaches its maximum level
_{} at the acceleration phase length
_{}:
_{}
[1]
As mentioned the acceleration time length and the deceleration time length are the same, therefore the deceleration phase starts at:
_{}
[2]
Fig.2: Relation velocity ramp and distance
The proceeded distance equals to the area enclosed by the velocity ramp and the abscissa (Fig. 2, hatched area):
_{}
_{}
[3]
Because of the symmetry _{} we state:
_{}
[4]
Isolating [4] for the total movement duration _{} results in:
_{}
[5]
Now we know all parameters to compute the velocity for any point of time:
_{}
[6]
At last we have to consider the case where the distance _{} isn't sufficient to let the velocity grow up to its target level
_{}. Then the velocity ramp becomes a triangle (Fig. 3). The acceleration phase length is now half of the total motion duration. The side condition
_{} applies now to the distance equation [6] and [4]. Therefore we substitute
_{}for
_{} in [4]. As a result we get the equation that relates the distance to the velocity peak level:
_{}
[7]
Fig. 3: Velocity triangle
Substituting _{}into [7] and solving it for
_{} leads to:
_{}
[8]
The triangle case has to be considered when using [6]; for that reason we have to limit the given velocity
_{} when it exceeds
_{}:
_{}
[9]
Time discretization
To enforce that the motion adheres to the velocity ramp trajectory the motion planning service must send continuously command positions to the simulated lbr3 arm service in small time intervals
_{} (sample rate period time). Hence the total PTP movement is divided into
_{} waypoints matching discrete points of time_{}. We get the time discrete distance function
through substituting _{} with
_{} in [6] (Fig. 2):
_{}
[10]
The counts of discrete time steps _{},
_{}and
_{} multiplied with the period time
_{} must match to
_{},
_{} and
_{}:
_{}
_{}
_{}
[11]
Hence the time discrete waypoints can be calculated as follows:
_{}
[12]
However, in practice _{},
_{} and
_{} usually won't match exactly an integer multiple of
_{}. To avoid inaccuracies some corrections have to be made for
_{} and
_{}. After an adjustment of
_{} and
_{} to satisfy [11] the velocity
_{} and the acceleration
_{} have to be computed again via
_{} and_{}. The implementation of these adjustments is left for the user as an exercise.
General approach to the distance equation:
The radial distance _{} can be calculated through the integration
_{}. Function_{} describes the velocity and
_{} the acceleration trajectory. Let's begin with the acceleration function:
_{}
[13]
The first integration results into the velocity function:
_{}
[14]
The constant _{}takes any prior velocity into account. The second integration results into the distance equation, where the constants
_{} and
_{} take any prior velocity and position into account:
_{}
_{}
[15]
Now we introduce our side conditions. In our application acceleration and deceleration have the same absolute values, therefore we state:
_{}
[16]
We focus on relative movements starting with zero velocity:
_{}
[17]
We assume that the velocity zeros again at _{}:
_{}
[18]
Taking the side condition [16], [17] and [18] in [14] into account we get:
_{}
[19]
Therefore the velocity equation [14] can be stated as:
_{}
[20]
Evaluating the velocity function [20] for the time _{} results in:
_{}
[21]
Side conditions [16] and [17] and equation [19] turn [15] into:
_{}
[22]
Finally we simplify the distance equation [22] by using [19] and [21]:
_{}
[23]
Legend:
_{} : = Acceleration time length
_{} : = Acceleration time length interim value
_{} : = Acceleration time length all axes' maximum value
_{} : = Deceleration start time
_{} : = Deceleration start time interim value
_{} : = All axes' maximum value of the deceleration start time
_{} : = Movement end time
_{} : = Movement end time interim value
_{} : = All axes' maximum value of the total movement time
_{} : = Maximum acceleration
_{} : = Predetermined acceleration maximum
_{} : = Velocity ramp maximum
_{} : = Predetermined velocity ramp maximum
_{} : = Velocity ramp maximum interim value
_{} : = Predetermined distance to move
_{} : = Sample rate period time
_{} : = Count of axes
_{} : = Time discrete step indexer
_{} : = Time discrete position