Trajectory Control Modes

Depending on the context defined by the control primitives, different control strategies must be developed. Online trajectory generator gives a flexible solution to build these controllers, which can easily react to unforeseen sensor events and adapt the kinematic parameters, mainly velocity, to the environment context. Switching to a new trajectory or a new frame in which the trajectory is controlled is also possible.

The main idea of the controller is to compute and follow a trajectory while joining up the target trajectory or a target point from the current state. Several control modes are defined to solve the reactive HRI manipulation problem.

Control Mode 1: Target tracking. If we suppose the robot is in an area without risk of

collision, the system can track the end point of the trajectory. In this case, the controller generates iteratively a trajectory to reach the end point and send the first step of this trajec- tory to a low-level controller. In the special case where the controller does target tracking with visual system, it does visual servoing.

Figure5.13shows the details of the trajectory control mode for Target Tracking. The

object is at position O at current time t, and moves following the curveTob j.

This curve is obtained by a simple Kalman filter, building a movement model from the

results of 3D vision system. Fris the robot base frame,Fc andFoare camera frame and

object frame, respectively. Also, Rc

r is the 4 × 4 transformation matrix fromFr toFcand Ro_c the transformation matrix fromFc toFo. They are all in dashed line and they change with time when the human or the object moves. The camera direction is adjusted to center the object in the image. Initially, the robot is at point Pe, since there is no risk of collision, the controller can simply track point P2, which is the end point of the segment. It is also

Figure 5.13: Control Mode 1. The robot tracks the point P2. The object moves to the right, it is drawn at two times: firstly in brown for time t1 and then in green at time t2. In both cases, the entry point P2of the trajectoryTgis drawn relatively to the object frameFo.

possible for the robot to join up the trajectory at another point Pjoint defined in the object frame which is the task frame. The details of the algorithm is given in Algorithm2where: T: duration of one control cycle.

Mr: current motion condition of the robot, so Mr= (Xr,Vr, Ar). δ : distance threshold to stop the tracking process.

Mg(t): motion conditions at time t on trajectoryTg.

MP2: motion conditions of the target P2on the main trajectory, which is calculated by the

planner.

TMax: the maximum time the controller to track the target or the trajectory. Once the

time exceeds the value, the trajectory controller is suspended and a signal is sent to the supervisor, requiring the replanning of new task or a new path to realize the task. X, Q: input signal in Cartesian space and in joint space for low-level controller.

Control Mode 2: Trajectory tracking in task frame.Once robot reaches point P2, it starts

the grasping movement, which corresponds to trajectoryTgin Figure5.9and Figure5.15.

The object is still moving, but as the robot is in the high cost zone, it should track the main trajectory in the task frame. The details of the control mode is given in Algorithm3.

Figure5.15shows the details of the control mode, all the frames and object movements

are the same as in Figure 5.13, but the robot is at point Pe0. The robot tracks Tg in the

can be applied in numerous situations. For example, when PR2 needs to grasp an object on a moving conveyor belt, the grasping movement is achieved by tracking a trajectory in the object frame, as shown by Figure5.14.

Figure 5.14: One application for Control Mode 2. The robot needs to grasp an object on a moving conveyor belt. The object is moving w.r.t. the robot and the grasping movement is planned as the red part of the trajectory. This part of the trajectory should be tracked in the object frame.

Algorithm 2: Control for target tracking (Control Mode 1) input : Target point P2;

while (distance(P2, Mr) > δ ) ∧ (t < TMax) do system time t = t + T , Loop = Loop + 1; Update perception data;

if Collision Detected then Emergency stop; if Suspend Events Occur then Suspend task; Coordinates transformations;

Generate Type V control trajectoryT (t), for which: IC = Mr, FC = MP2;

X = getMotion(t + T,T (t));

Inverse kinematics: X → Q; Qto the position servo system; end

Control Mode 3: Path re-planning and trajectory switching: during the execution, a

path can be re-planned, for example when an obstacle moves (see Fig. 5.9). A new tra-

jectory is computed by the planner MHP and given to the controller that switches to the new trajectory. While the controller is following the trajectoryTm, an obstacle moves and invalidates the initial trajectory. In this case, the controller can suspend the execution, and decide to switch the new trajectory provided by MHP, written asT0m, beginning at time t1

O

O'

Figure 5.15: Control Mode 2. Object at time t1is colored in light brown, and green at time t2. It follows a movement model given as the blue trajectoryTob j. The purple trajectory for graspingTg stays unchanged in the object frame. The robot tracks the trajectoryTgas it does the grasping movement.

Algorithm 3: Control for trajectory tracking in a moving work frame (Control Mode 2)

input : Trajectory segmentTg;

while (distance(Pc, Mr) > δ ) ∧ (t < TMax)) do system time t = t + T , Loop = Loop + 1;

Update perception data and object movement model; if Collision Detected then Emergency stop;

if Suspend Events Occur then Suspend task; Coordinates transformations; M_Tg= getMotion(t + T,Tg); Mob ject= getMotion((t + T,Tob j); X = M_TgL∗ Mob ject; Inverse kinematics: X → Q; Qto the position servo system;

*L

denotes the vector addition of two Motion Conditions. end

in the future. The controller anticipates the switch, and when the robot reaches Pt1 at time

t1, the robot switches to the new trajectoryT0m. Because the new trajectoryT0mis calcu- lated using the state of the robot at time t1as its initial condition, the trajectory is switched without problem.

held by the human counterpart. For other tasks, like picking an object, giving an object to human or putting an object on the table, the same functionalities can be used. For ex- ample, putting an object on a moving platform would require the end segment of the main trajectory to be controlled in the frame of the platform, which moves in the robot frame. Likewise, giving an object to a moving human hand will require the manipulator to track the exchange point, normally planned by a human-aware motion planner till the detection that human grasps the object successfully. Although the algorithm to decompose the tasks into control primitives is still to improve, the basic HRI tasks can all be controlled by the control modes discussed above.

Figure 5.16: Results of robot tracking a target: position (in m), velocity (in m/s) and ac- celeration (in m/s2) during the tracking for 25 seconds. The black dashed line is the target position, with noise of the 3D vision, and the red line is the position of the robot, which tracks the target with a delay. The positions of the robot are calculated from measured joint values and the kinematic model, while velocity and acceleration are estimated. The velocity, acceleration and jerk are always limited, maintaining a smooth tracking process.