# Add ability to specify equality-constrained systems. #4151

Open
opened this Issue Nov 17, 2016 · 12 comments

Projects
None yet
4 participants
Contributor

### jadecastro commented Nov 17, 2016 • edited

 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: 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. 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). Existing optimization problems constrained with respect to the system's dynamics models should also be able to handle nonlinear equality constraints.
Contributor

### amcastro-tri commented Nov 17, 2016

 @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.
Member

### sherm1 commented Nov 17, 2016

 @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.
Contributor

### jadecastro commented Nov 17, 2016

 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.
Contributor

### amcastro-tri commented Nov 17, 2016

 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!
Member

### sherm1 commented Nov 17, 2016

 @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.
Contributor

### amcastro-tri commented Nov 17, 2016

 @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.
Member

### sherm1 commented Nov 18, 2016

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

### jadecastro commented Nov 18, 2016 • edited

 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.
Contributor

### RussTedrake commented Nov 18, 2016

 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
Contributor

Member

### sherm1 commented Nov 18, 2016

 @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"?
Contributor

### RussTedrake commented Nov 18, 2016

 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.

Open