Overview
To move the head, we use actionlib’s SimpleActionClient and the action interface head_traj_controller/point_head_action. Given an initial position, an obstacle, and a goal, DDP outputs the locally-optimal trajectory T. Define T[0] to be the initial pixel of the green ball, then, we would like that at the next time step the pixel of the green ball is at T[1]. So, our goal is to move the head so that the detected pixel of the green ball is equal to T[1]. Using the Kinect topic camera/depth/points we project the pixel difference (T[1]-T[0]) as a three dimensional point, p, in the frame ‘openni_rgb_optical_frame’. We then transform p into the ‘base_link’ frame and, using the aforementioned action interface, tell the PR2 to move to the transformed point.
Differential Dynamic Programming (DDP)
DDP is an algorithm that solves locally-optimal trajectories given a cost function over some space. In essence it works by locally-approximating the cost function at each point in the trajectory. It uses this approximation to finds the optimal change to the trajectory (via a set of actions) that minimizes some cost metric (e.g. cumulative cost). In the limit it converges to the optimal trajectory.
Dynamics & Cost
Our dynamics is simply linear in two-dimensions, without velocities:
$$ xt = A xt-1 + B ut $$ |
where $$A = B = I_2$$. Our cost function has a quadratic minimum and Gaussian obstacles:
$$ ct = $$\frac{1}{2}$$(qh ut2 + cb b(xt)) $$ |