Geometric Control Optimization and Planning Library (GCOP)

Our lab (ASCO) maintains and actively develops a C++ library for optimal control, planning, and estimation of dynamic systems.  The package provides a framework for optimal control of dynamic systems using different well known optimization algorithms such as Cross-Entropy Sampling, Differential Dynamic Programming (DDP), Levenberg-Marquardt, and Simultaneous Perturbation and Stochastic Approximation (SPSA).  The library is templated, so it is easy to extend to new systems and optimization algorithms.  The library can be found here.

Aerial Autonomy Framework

Our lab maintains a ROS package for developing aerial autonomy applications, with a focus on aerial manipulation.  The goal is to allow users to easily construct application-specific state machines consisting of reusable, modular behaviors and event-triggered state transitions.  The library includes useful controllers and state estimators that depend only on a generic interface to the hardware and are agnostic to the particular drone/robotic arm/sensors being used.   The package can be found here.

Robust Policy Search

This work studies the design of reliable control laws of robotic systems operating in uncertain environments.  We introduce a new approach to stochastic policy optimization based on probably approximately correct (PAC) bounds on the expected performance of control policies. An algorithm is constructed which directly minimizes an upper confidence bound on the expected cost of trajectories instead of employing a standard approach based on the expected cost itself. This algorithm thus has built-in robustness to uncertainty, since the bound can be regarded as a certificate for guaranteed future performance.  More information can be found in the related paper.

Visual-inertial State Estimation

This an ongoing project which aims to develop a system which fuses IMU and camera measurements to estimate the trajectory of a robot and the structure of the environment using maximum likelihood estimation.

Real-time Object Tracking in 3D

As part of our lab’s work on aerial manipulation, I’ve developed a ROS package for tracking the 3D pose of an object using a monocular camera.  The package takes as input a textured CAD model of the object to be tracked.  For a given image, point-based features are extracted and matched to a virtual rendering of the CAD model (using its last known pose).  A RANSAC+PnP algorithm uses these 2D-3D correspondences to find the new pose of the object.  The pose of the object is initialized by matching an input image to several images in a finite set of renderings of the CAD model.  The image with the largest number of geometrically consistent inliers is taken to be the pose of the object.  A constant velocity motion model or Extended Kalman Filter which fuses IMU measurements (if an IMU is available) can be used for more reliable tracking.

The package also has the capability to perform edge-based tracking (based on the work of Choi and Christensen in Real-time 3D model-based tracking using edge and keypoint features for robotic manipulation).

This package can also be used for image-based localization in a known map.

The package can be found at



Optimal Visual Servoing for Underactuated Systems

This work considers a hybrid visual servoing technique for differentially flat underactuated systems.  Given a goal image, the objective is to navigate to a position which aligns the camera with the desired image while keeping salient features in view along the way. We formulate the problem as an optimal control problem that can be solved using Gauss-Newton optimization. As a cost function, we use the average squared distance between SURF features matched between the desired image and the image projected from the final state of the trajectory. The optimization is performed over a polynomial parametrization of the flat outputs of the system to decrease the dimensionality of the optimization. Preliminary experimental results show that the technique has better performance (in terms of faster navigation without failure) compared to a typical position-based visual servoing system for the case considered, which is a quadrotor with a front-facing camera. This method assumes feature depths are known or can be estimated (e.g. using stereo camera or structure from motion algorithms).  More information can be found in the associated paper.

(Sorry for the bad video quality… This was before I discovered proper video editing software)

DSL GridSearch: D*-lite on a uniformly spaced 3D or 2D grid

This ROS package is an implementation of D*-lite graph search on a uniformly spaced 3D or 2D grid for use in global path planning. This package provides the ability to create an occupancy grid from a .stl mesh or to specify a grid of a given size. The user can specify start and goal positions by publishing to the relevant topics, and the generated paths will be published by the node. The user can also publish messages to set grid cells to be occupied or unoccupied, or mesh messages can be sent to set all cells which intersect with the mesh as occupied. The 2D version is implemented as a traversability map, where the cost of an edge is equal to the height gradient between two grid cells.  It can be found here.