 [Documentation] [TitleIndex] [WordIndex]

## 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 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. So, our goal is to move the head so that the detected pixel of the green ball is equal to T. Using the Kinect topic camera/depth/points we project the pixel difference (T-T) 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))$$

2020-09-26 12:32