New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add ability to specify equality-constrained systems. #4151

Open
jadecastro opened this Issue Nov 17, 2016 · 12 comments

Comments

Projects
None yet
4 participants
@jadecastro
Contributor

jadecastro commented Nov 17, 2016

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

This comment has been minimized.

Show comment
Hide comment
@amcastro-tri

amcastro-tri Nov 17, 2016

Contributor

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

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.

@sherm1

This comment has been minimized.

Show comment
Hide comment
@sherm1

sherm1 Nov 17, 2016

Member

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

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.

@jadecastro

This comment has been minimized.

Show comment
Hide comment
@jadecastro

jadecastro Nov 17, 2016

Contributor

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

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.

@amcastro-tri

This comment has been minimized.

Show comment
Hide comment
@amcastro-tri

amcastro-tri Nov 17, 2016

Contributor

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!

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!

@sherm1

This comment has been minimized.

Show comment
Hide comment
@sherm1

sherm1 Nov 17, 2016

Member

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

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.

@amcastro-tri

This comment has been minimized.

Show comment
Hide comment
@amcastro-tri

amcastro-tri Nov 17, 2016

Contributor

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

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.

@sherm1

This comment has been minimized.

Show comment
Hide comment
@sherm1

sherm1 Nov 18, 2016

Member

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

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.

@jadecastro

This comment has been minimized.

Show comment
Hide comment
@jadecastro

jadecastro Nov 18, 2016

Contributor

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

jadecastro commented Nov 18, 2016

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

This comment has been minimized.

Show comment
Hide comment
@RussTedrake

RussTedrake Nov 18, 2016

Contributor

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

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

@RussTedrake

This comment has been minimized.

Show comment
Hide comment
@RussTedrake

RussTedrake Nov 18, 2016

Contributor

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.

Contributor

RussTedrake commented Nov 18, 2016

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

This comment has been minimized.

Show comment
Hide comment
@sherm1

sherm1 Nov 18, 2016

Member

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

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

@RussTedrake

This comment has been minimized.

Show comment
Hide comment
@RussTedrake

RussTedrake Nov 18, 2016

Contributor

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.

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment