Add ability to specify equality-constrained systems

[jadecastro]

There are some systems of interest that that are expressible only with additional equality constraints.

For instance, a unicycle model may be written in terms of the state vector (x, y, theta) (where x and y are the Cartesian coordinates and theta is the heading) as follows:

x_dot = v * cos(theta)
y_dot = v * sin(theta)
theta_dot = u

where u and v are inputs. It may be equivalently written out in polynomial form by choosing the state space (x, y, s, c):

x_dot = v * c
y_dot = v * s
s_dot = u * c
c_dot = -u * s

but in this case s and c are required to satisfy the additional equality relation:

s^2 + c^2 - 1 = 0

The above example is useful to realize polynomial forms of certain non-holonomic systems (needed for posing sums-of-squares problems with respect to such systems without approximations - e.g. Taylor expansions). Note that this differs from more general (optimization-based) constraints in that the system description is incomplete without the equality relation.

More broadly, (based on f2f discussions with @liangfok and @amcastro-tri) equality constraints could be useful for systems in which a car's state is constrained to some (arbitrary) non-planar road surface or rigid-body plant constraints need to be preserved.

Currently, our systems and solvers don't support any user-defined equality constraints within a system. To remedy, I propose the following workflow for discussion: 1) Provide the user a utility (in System) to specify an equality constraint, EvalStateEqualityConstraint (similar to EvalTimeDerivatives). This particular utility would be consumed by Integrator to simulate the system without drifting away from the constraint. It would also be used to automatically populate equality constraints in user-defined optimization routines. 2) Incorporate the ability to integrate while maintaining the solution on a manifold. This of course requires that the integrator to accept the equality constraint and use an update scheme to keep the solution on the manifold. A similar idea has been implemented in System 2.0 for DAE constraints (c.f. the discussion on p. 4 of the System 2.0 design document: https://drive.google.com/a/trinst.com/file/d/0B-dn8WKPpzq4YkdZZDUzanJkaWc/view?usp=sharing). 3) Existing optimization problems constrained with respect to the system's dynamics models should also be able to handle nonlinear equality constraints.

[amcastro-tri]

@jadecastro, I just had a discussion with @sherm1 about this. While it is true that the general long term approach would involve some sort of projection, you can still with the functionality in master right now, give a solution to this problem.

An example of a case like this is RigidBodyPlant. RigidBodyPlant implements LoopJoint's as constraints and uses Baumgarte's stabilization to avoid drift during the numerical integration (do not ever use this with variable time step integrators!). The trick is to implement EvalTimeDerivatives so that xdot is computed taking into account your constraint.

Is your system 1st order? because in that case the numerical drift might be small enough that you can ignore it and you wouldn't need any stabilization on a first iteration.

Summarizing, you can start implementing this system with the functionality already in Drake. We can always have a f2f if you need to.

[sherm1]

@jadecastro, the form of these constraints is analogous to the use of quaternions in Drake now (those also have a normalization constraint). So I'm not sure any additional support is required, but I'm not certain. Please talk with @amcastro-tri to clarify.

[jadecastro]

Thanks @amcastro-tri and @sherm1, that helps. Based on a f2f with @amcastro-tri, I see now that System 2 has the capability to use take advantage of constraints within the system model by augmenting the system with a state-dependent function lambda. For sums-of-squares optimization, we would need this augmented system to be in polynomial form. @amcastro-tri and I whiteboarded how this augmented system would look, and we could not obtain a polynomial, which is a bummer for using SOS.

I will re-check the math, and will post if anything changes.

[amcastro-tri]

Basically with @jadecastro we added a Lagrange multiplier (lambda) to impose the constraint c^2 + s^2 = 1. As @jadecastro mentioned, we need to re-check the math to see if we can formulate it in polynomial form. Fun discussion @jadecastro!

[sherm1]

@amcastro-tri, is lambda actually enforcing the time derivative of the constraint? If so there may still be some drift off the position constraint manifold.

[amcastro-tri]

@sherm1, yes. I was thinking that if we take this route (depending on whether we can put it in polynomial form or not) we could either apply Baumgarte's stabilization or either project after each time step. Any other options? However, from f2f with @jadecastro it doesn't seem they'd need to numerically integrate this system (and hence have numerical drift errors) but rather they need to analyze it with a symbolic form. In that case I don't think drift would be an issue as long as the problem is correctly formulated.

[sherm1]

Projection is probably the only good option here because there is room for only one Baumgarte term and hence no damping.

[jadecastro]

There is another important reason I think the projection method may not work that I forgot to mention in our f2f. In particular, one way to solve for the equality constraint is to add a pair of constraints that look like this:

g(x) - lambda * p(x) is s.o.s.
-g(x) - lambda * p(x) is s.o.s.

where x is the state vector, g(x) = s^2 + c^2 - 1 in our example, p(x) is a given polynomial that is >0 for all x in our domain, and lambda is a decision variable. This form for the constraint will be missing if we adopt the projection approach. In particular, we will not be able to solve for lambda anymore.

So, I still think we still need to do (1) in the list above, exposing the constraint directly.

Inviting @hongkai-dai to discuss, since the issue follows somewhat from our f2f discussion.

[RussTedrake]

fwiw -- we definitely had the notion of state constraints for general systems in the matlab stack https://github.com/RobotLocomotion/drake/blob/master/drake/matlab/systems/%40DrakeSystem/DrakeSystem.m#L435 i remember documenting somewhere (but am embarassed now that i can't find that documentation on a quick search) that announcing phi(x)=0 would simply be providing extra information to the solvers/analysis methods -- the constraints were not actually being actively stabilized. (in other words, the system was responsible for ensuring that the constraints were enforced).

As @jadecastro points out, this was very important for a number of the SOS methods, which is why you'll see polynomial state constriants available in the polynomial system classes, too: http://drake.mit.edu/doxygen_matlab/class_polynomial_system.html#a6ce40992715cfc80594674cba7aafd5f

and that we explicitly add the unit circle constraints in https://github.com/RobotLocomotion/drake/blob/master/drake/matlab/systems/%40DrakeSystem/extractTrigPolySystem.m#L123

[RussTedrake]

hah. the documentation is here: http://underactuated.csail.mit.edu/underactuated.html?chapter=26 with the relevant excerpt below:

State constraints are additional information that you are providing to the solver and analysis routines. They should be read as "this dynamical system will only ever visit states described by ϕ(x)=0". Evaluating the dynamics at a vector x for which ϕ(x)≠0 may lead to an undefined or non-sensible output. Telling Drake about this constraint will allow it to select initial conditions which satisfy the constraint, simulate the system more accurately (with the constraint imposed), and restrict attention during analysis to the manifold defined by the constraints. However, \emph{the state constraints function should not be used to enforce an additional constraint that is not already imposed on the system by the governing equations.}. The state constraint should be simply announcing a property that the system already has, if simulated accurately. Examples might include a passive system with a known total energy, or a four-bar linkage in a rigid body whose dynamics are written as a kinematic tree + constraint forces. State constraints are implemented by overloading the stateConstraints method \emph{and} by calling setNumStateConstraints to tell the solver what to expect in that method.

[sherm1]

@RussTedrake, I think I understand what you're saying about state constraints above but want to check. In simulation, the state constraint ϕ(x)=0 will be satisfied by the initial conditions x0, and the system equations will satisfy exactly some differentiated version of that constraint. Under perfect solution conditions the original constraint would remain satisfied, but due to numerical errors it will drift away from the manifold. In that case the state constraint itself will be used for stabilization (either by projection or feedback) and so will have a (mild) effect on the solution. Does that match your meaning for "state constraint"?

[RussTedrake]

Everything you say is correct, but in the matlab version, the responsibility for the stabilization of the numerical drift fell on the author of each individual system. It would be much nicer to have stabilization happen at the simulator level, and it would have been possible to accomplish that with some of the simulink solvers, but I never got around to implementing it.

[sherm1]

cc @joemasterjohn This is related to the MbP constraint project, though more general since it needs to be exposed at the System framework level. The author reference a 5 year old design document that isn't too far off from what we have.