Drake: How to involve a decision variable into a boolean expression?

[vinbo]

When my code was doing a nonlinear programming, I used something like sign(x) in the middle of expression as a constraint, where x is decision variable, and got the error info:

RuntimeError: You should not call __bool__ / __nonzero__ on Formula. If you are trying to make a map with Variable, Expression, or Polynomial as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.

May I ask how could I do this? I went to the document of EqualToDict https://drake.mit.edu/pydrake/pydrake.common.containers.html, but the item is too concise to be useful to me. Thanks

[jwnimmer-tri]

@vinbo Per https://drake.mit.edu/getting_help.html#asking-your-question we say _"If you are seeking assistance (e.g. tutorials or a brief example), please post a question on StackOverflow with the drake tag."_ We'd like to use the issue tracker for bug reports; for user help, please use StackOverflow. Thank you!

[hongkai-dai]

Our nonlinear program does not support parsing sign(x) directly in AddConstraint, namely the following code is not supported

prog.AddConstraint(np.sign(x) ==1)

On the other hand, if you use a lambda then that is fine:

prog.AddConstraint(lambda x: np.array([np.sign(x)]), np.array([1.]), np.array([1.]), vars=x)

The reason is that when you call prog.AddConstraint(np.sign(x) == 1), it tries to convert np.sign(x) == 1 to a symbolic expression, and this conversion is not successful. On the other hand, when you call AddConstraint with a lambda function, the code goes through another routine, that doesn't require symbolic computation.

The following code should also work

# Define a function
def foo(x):
    return np.array(np.sign(x))

prog.AddConstraint(foo, np.array([1.]), np.array([1.]), vars=x)

When writing Drake's MathematicalProgram, I did not intend to support prog.AddConstraint(nonlinear_expression == bound) or prog.AddConstraint(nonlinear_expression >= bound), where nonlinear_expression is a nonlinear function of decision variables. It happens to work sometimes since we have a symbolic engine that supports nonlinear expression (like prog.AddConstraint(np.sum(x**2) == 1), but evaluating a nonlinear expression through symbolic engine is not fast. On the other hand, we do have good support for adding linear expression as constraint, so adding linear constraint like prog.AddConstraint(x[0] + 2*x[1] <= 1) is encouraged.

I would suggest not to add nonlinear constraint with symbolic expressions, but using lambda or a function as shown above. The computation is a lot faster with lambda or function.

Besides, as Jeremy suggested, it is better to use StackOverflow to seek assistance, thanks!

[vinbo]

Define a function

def foo(x):
return np.array(np.sign(x))

prog.AddConstraint(foo, np.array([1.]), np.array([1.]), vars=x)

Thank you for the detailed reply. I never knew drake could be used this way. But Neither of the code segments you suggested work. Once I run the code, the service was shut, and the kernel restarted. The error info given is
WARNING:root:kernel fc3042f3-31e9-4d62-9dd7-084f97db9526 restarted
The code I use to test the new grammar is below:

from pydrake.all import MathematicalProgram, Solve, IpoptSolver, SolverOptions
import numpy as np

def foo(x):
return np.array(np.sign(x))

prog = MathematicalProgram()
x = prog.NewContinuousVariables(1)

prog.AddConstraint(np.sign(x)==1)

prog.AddConstraint(lambda x: np.array([np.sign(x)]), np.array([1.]), np.array([1.]), vars=x)

prog.AddConstraint(foo, np.array([1.]), np.array([1.]), vars=x)

prog.AddLinearCost(1*x[0])
result=Solve(prog)
print result.is_success()
print result.GetSolution()

Do you what happened? Thank you.

[hongkai-dai]

I can't reproduce your failure. I tried both Snopt and Ipopt, and both generate results without kernel crash. Here is my code

import numpy as np
import pydrake.solvers.mathematicalprogram as mp
from pydrake.solvers.ipopt import IpoptSolver
def foo(x):
    return np.sign(x)

prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1)
prog.AddConstraint(foo, [1.], [1.], vars=x)
prog.AddLinearCost(1 * x[0])
result = mp.Solve(prog, np.array([10.]), None)
print(result.is_success())
print(result.GetSolution(x))

It sounds like you run your code in jupyter notebook? Can you run the code as a standalone script using python?

BTW, imposing a constraint with sign(x) in it could be bad. Neither Snopt nor Ipopt solved the above problem correctly (they reported optimal solution as -1e20 or 10). It is better to reformulate the problem above like min x[0] subject to x[0] >= 0. With the following code

import numpy as np
import pydrake.solvers.mathematicalprogram as mp
from pydrake.solvers.ipopt import IpoptSolver

prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1)
prog.AddConstraint(x[0] >= 0)
prog.AddLinearCost(1 * x[0])
ipopt_solver = IpoptSolver()
result = ipopt_solver.Solve(prog, np.array([10.]), None)
print(result.is_success())
print(result.GetSolution(x))

The problem is solved to the right optimality (x*=0)

[vinbo]

I can't reproduce your failure. I tried both Snopt and Ipopt, and both generate results without kernel crash. Here is my code

import numpy as np
import pydrake.solvers.mathematicalprogram as mp
from pydrake.solvers.ipopt import IpoptSolver
def foo(x):
    return np.sign(x)

prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1)
prog.AddConstraint(foo, [1.], [1.], vars=x)
prog.AddLinearCost(1 * x[0])
result = mp.Solve(prog, np.array([10.]), None)
print(result.is_success())
print(result.GetSolution(x))

It sounds like you run your code in jupyter notebook? Can you run the code as a standalone script using python?

BTW, imposing a constraint with sign(x) in it could be bad. Neither Snopt nor Ipopt solved the above problem correctly (they reported optimal solution as -1e20 or 10). It is better to reformulate the problem above like min x[0] subject to x[0] >= 0. With the following code

import numpy as np
import pydrake.solvers.mathematicalprogram as mp
from pydrake.solvers.ipopt import IpoptSolver

prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(1)
prog.AddConstraint(x[0] >= 0)
prog.AddLinearCost(1 * x[0])
ipopt_solver = IpoptSolver()
result = ipopt_solver.Solve(prog, np.array([10.]), None)
print(result.is_success())
print(result.GetSolution(x))

The problem is solved to the right optimality (x*=0)

Thank you.

1. I saved you code into testDrake.py and run it under the terminal opened by the jupyter notebook, it gives:

python testDrake.py

Segmentation fault (core dumped)
Maybe I have to install drake totally independent of the jupyter?

1. I still dont' understand the grammar of this line exactly:
   prog.AddConstraint(foo, [1.], [1.], vars=x)
   will you please tell me the meaning of two parameter 1. and 1.? are they upper bound and lower bound?
2. although the solver is not expected to deal with a sign constraint in general, but by your running result, is it normal for problem as simple as this one?
3. unfortunatly, the sign(x) I wrote here is a simplified version of my own problem, the constraint in my code is like this:
   def angle(self,vector):
   return np.arctan2(vector[1],vector[0]) # (-pi,pi]
   def fsg(self,x,a,b):
   return 0.5(np.sign(x-a)-np.sign(x-b))
   def f123(self,x):# see def d_angle
   return self.fsg(x,-3np.pi,-np.pi)(x+2np.pi)+self.fsg(x,-np.pi,np.pi)x+self.fsg(x,np.pi,3np.pi)(x-2np.pi)
   def d_angle(self,angle_present, angle_previous):
   da=angle_present-angle_previous
   '''#f123 does the job
   if da>np.pi:
   da=da-2np.pi
   elif da<-np.pi:
   da=da+2np.pi
   '''
   da=self.f123(da)
   return da
   ...
   if self.d_angle(angle_free_next,angle_0)>=0: # intial rotation direction decide which way to go
   for i in range(2,10): # i start from 1 since state_over_time0 is in different format
   state_present=states_over_time[i,:]
   state_previous=states_over_time[i-1,:]
   angle_present=self.angle(state_present[0:2]-self.world_1_position)
   angle_previous=self.angle(state_previous[0:2]-self.world_1_position)
   mp.AddConstraint(self.d_angle(angle_present,angle_previous)>=0)

I havn't seen how to transform it into linear constraint yet...

[hongkai-dai]

Segmentation fault (core dumped)
Maybe I have to install drake totally independent of the jupyter?

I am not sure, how did you install Drake?

1. I still dont' understand the grammar of this line exactly:
   prog.AddConstraint(foo, [1.], [1.], vars=x)

What it says it to add a constraint that 1<= foo(x) <= 1. The first argument is the constraint evaluator. The second one is the lower bound, and the third one is the upper bound. vars=x means that this constraint should be evaluated with variables x.

1. although the solver is not expected to deal with a sign constraint in general, but by your running result, is it normal for problem as simple as this one?

I am surprised also. But generally I avoid any discontinuous functions in my code, for example, sign(x).

1. unfortunatly, the sign(x) I wrote here is a simplified version of my own problem, the constraint in my code is like this:

If I understand you correctly, you want to say that angle_present - angle_previous is in the range of [-2pi, pi] or [0, pi] or [2pi, 3pi], this is equivalent to requiring sin(angle_present - angle_previous) >=0. This is a continuous constraint, and I suppose the optimizer might like this continuous constraint better than a constraint with discontinuous functions.

[vinbo]

Segmentation fault (core dumped)
Maybe I have to install drake totally independent of the jupyter?

I am not sure, how did you install Drake?

Actually I am doing the problem from underactuated robotics, I installed the drake from the docker provided by the course website http://underactuated.csail.mit.edu/Spring2019/install_drake_docker.html. I saw your name submitting an issue there :)

1. unfortunatly, the sign(x) I wrote here is a simplified version of my own problem, the constraint in my code is like this:

If I understand you correctly, you want to say that angle_present - angle_previous is in the range of [-2pi, pi] or [0, pi] or [2pi, 3pi], this is equivalent to requiring sin(angle_present - angle_previous) >=0. This is a continuous constraint, and I suppose the optimizer might like this continuous constraint better than a constraint with discontinuous functions.

I am trying to solve the orbital transfer problem of the class, sending a rocket from the orbit of earth to that of moon. The trajectory is supposed to be like this:

but most most time, it is like this:

or even worse. I am trying to force the rotation direction under some condition by adding constraint angle_present - angle_previous>0, the reason I wrote it seeming complex is because I have to deal with the singular point generated by arctan2.
(did you do the problem set ? :) )

[hongkai-dai]

I didn't use Drake docker. It is better to ask the docker question on stack overflow. If you put drake tag on your question we will see it.

And I didn't do the problem set.

There are two approaches to rewrite your constraint

1. You could try to rewrite your constraint from using sign function to using sin(angle_present - angle_previous), mentioned in the previous comment.
2. I think what you actually want to impose is that the counterclockwise angle from the vector state_previous[0:2]-self.world_1_position to the vector state_present[0:2]-self.world_1_position is between [0, pi]. You could impose this constraint using cross product. If you have two vectors in 2D, one is (x1, y1), another vector is (x2, y2), and you want the counterclockwise angle from (x1, y1) to (x2, y2) is between [0, pi], then you could do the cross product cross( (x1, y1, 0) , (x2, y2, 0)), and check if the z component of the cross product is non-negative. Namely the constraint you want to impose is x1 * y2 - x2 * y1 >= 0. For your specific case, the constraint you want to impose is like this

(state_previous[0] - self.world_1_position[0]) * (state_present[1] - self.world_1_position[1]) - (state_previous[1] - self.world_1_position[1]) * (state_present[0] - self.world_1_position[0]) >= 0

This is probably better than approach 1 with sine function. It is computationally much simpler (without calling atan2 or sine).

[vinbo]

I didn't use Drake docker. It is better to ask the docker question on stack overflow. If you put drake tag on your question we will see it.

And I didn't do the problem set.

There are two approaches to rewrite your constraint

1. You could try to rewrite your constraint from using `sign` function to using `sin(angle_present - angle_previous)`, mentioned in the previous comment.

2. I think what you actually want to impose is that the counterclockwise angle from the vector `state_previous[0:2]-self.world_1_position` to the vector `state_present[0:2]-self.world_1_position` is between [0, pi]. You could impose this constraint using cross product. If you have two vectors in 2D, one is (x1, y1), another vector is (x2, y2), and you want the counterclockwise angle from (x1, y1) to (x2, y2) is between [0, pi], then you could do the cross product `cross( (x1, y1, 0) , (x2, y2, 0))`, and check if the `z` component of the cross product is non-negative. Namely the constraint you want to impose is x1 * y2 - x2 * y1 >= 0. For your specific case, the constraint you want to impose is like this

(state_previous[0] - self.world_1_position[0]) * (state_present[1] - self.world_1_position[1]) - (state_previous[1] - self.world_1_position[1]) * (state_present[0] - self.world_1_position[0]) >= 0

This is probably better than approach 1 with sine function. It is computationally much simpler (without calling atan2 or sine).

Thank you so much for the help. I was using an ugly way to solve the proble yesterday--I directly imposed the direction of u, ugly in math, effective in engineer. I love your 2nd way, why didn't I come out of it. Thank you dude, you really help.

[hongkai-dai]

Thanks, I worked on similar problems before. Let me know if the second approach doesn't work.

I will close this issue. Please reopen the issue, or submit to StackOverflow if you still have questions, thanks.