I want to use a symbolic expression as a MathematicalProgram constraint but am unsure how to achieve this. My best go so far is the following (simplified example):
x = Variable("x")
expression = x**2
prog = MathematicalProgram()
v = prog.NewContinuousVariables(1)
prog.AddConstraint(
lambda a: Evaluate(np.array([expression]), {x: a[0].value()}),
lb=np.array([0.0]),
ub=np.array([0.0]),
vars=v,
)
result = Solve(prog)
I'm getting the error PyFunctionConstraint: Output must be of scalar type AutoDiffXd. Got float instead.. Using lambda a: Evaluate(np.array([expression]), {x: a[0]}) does not work due to incompatible function arguments.
I'd highly appreciate any help with this.
I don't think we currently support adding symbolic::Expression as constraint in pydrake yet. On the other hand, we do support ExpressionConstraint in C++ version of Drake.
May I ask why you would like to impose the constraint using symbolic Expression? It is generally much faster to evaluate the constraint, if pass a function directly, something like this
def foo(x):
return np.array([x[0] **2])
prog.AddConstraint(foo, np.array([0.]), np.array([0.]), vars=v)
#Hongkai Dai's answer with the ExpressionConstraint in C++ led me in the right direction. There is such a constraint in pydrake (see here). However, it currently does not support array inputs. The second required insight was that it is possible to use prog.NewContinuousVariables in symbolic expression operations (e.g. Jacobian).
Using these insights, I solved my problem with something similar to the following:
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
expression = x[0]**2
J = expression.Jacobian([x[0]])
for i in range(2):
prog.AddConstraint(J[i], 0.0, 0.0)
result = Solve(prog)
Related
I am wondering how to compare two expressions in C++ z3. The following code generates two equal expressions, but the result shows they do not share the same id, which is different from this post. A way to do this is to simplify before checking but the speed is slow due to the simplify overhead. Is there an efficient way to solve it?
z3::context c;
z3::expr z1 = c.bool_const("z1");
z3::expr z2 = c.bool_const("z2");
z3::expr z11 = z1 && z2;
z3::expr z22 = z2 && z1;
auto res = Z3_is_eq_ast(c, z11, z22);
Simple answer: No.
Note that two terms that are semantically identical can still yield False, even after a call to simplify. The only way to check equivalence for sure is to call check_sat.
The way to think about Z3_is_eq_ast is that if it says True, then you absolutely have the same term. If it says False, then it may or may not be the same term, you just don't know. (It's essentially hash-consing, an old idea, and all the caveats apply. See here: https://en.wikipedia.org/wiki/Hash_consing).
I am looking for a way to describe the constraints of the Direct Collocation method in pydrake.
I got the robot model from my own URDF by using FindResource as this(l11-16).
Then, I tried to make some functions which calculate the positions of the joints as swing_foot_height(q) of this.
However there is a problem.
It is maybe a type error.
I defined q as following
robot = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
robot.RegisterAsSourceForSceneGraph(scene_graph)
file_name = FindResource("models/robot.urdf")
Parser(robot).AddModelFromFile(file_name)
robot.Finalize()
context = robot.CreateDefaultContext()
dircol = DirectCollocation(
robot,
context,
...(Omission)...
input_port_index=robot.get_actuation_input_port().get_index())
x = dircol.state()
nq = biped_robot.num_positions()
q = x[0:nq]
Then, I used this q for the function like swing_foot_height(q).
The error is like
SetPositions(): incompatible function arguments. The following argument types are supported:
...
q: numpy.ndarray[numpy.float64[m, 1]]
...
Invoked with:
...
array([Variable('x(0)', Continuous), ... Variable('x(9)', Continuous)],dtype=object)
Are there some way to avoid this error?
Right. In the compass gait notebook that you cited, there was an important line:
# overwrite MultibodyPlant with its autodiff copy
compass_gait = compass_gait.ToAutoDiffXd()
so that multibody plant that was being used in the constraint is actually an AutoDiffXd version of the plant.
The littledog notebook has more examples of this, with a more robust implementation that works for both float and autodiff constraint evaluations.
As far as I understand this, trajectory optimization with DirectCollocation converts the data type of the decision variables (in your case, x and q) to AutoDiffXd type. That is the type you're seeing here in the "Invoked with" error message. This is the type used for automatic differentiation which is used for finding the gradients for the optimization solver.
You'll need to convert back to float to use the SetPositions() function.
Drake has an interface where you can give it a generic function as a constraint and it can set up the nonlinearly-constrained mathematical program automatically (as long as it supports AutoDiff). I have a situation where my constraint does not support AutoDiff (the constraint function conducts a line search to approximate the maximum value of some function), but I have a closed-form expression for the gradient of the constraint. In my case, the math works out so that it's difficult to find a point on this function, but once you have that point it's easy to linearize around it.
I know many optimization libraries will allow you to provide your own analytical gradient when available; can you do this with Drake's MathematicalProgram as well? I could not find mention of it in the MathematicalProgram class documentation.
Any help is appreciated!
It's definitely possible, but I admit we haven't provided helper functions that make it pretty yet. Please let me know if/how this helps; I will plan to tidy it up and add it as an example or code snippet that we can reference in drake.
Consider the following code:
from pydrake.all import AutoDiffXd, MathematicalProgram, Solve
prog = MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
def cost(x):
return (x[0]-1.)*(x[0]-1.)
def constraint(x):
if isinstance(x[0], AutoDiffXd):
print(x[0].value())
print(x[0].derivatives())
return x
cost_binding = prog.AddCost(cost, vars=x)
constraint_binding = prog.AddConstraint(
constraint, lb=[0.], ub=[2.], vars=x)
result = Solve(prog)
When we register the cost or constraint with MathematicalProgram in this way, we are allowing that it can get called with either x being a float, or x being an AutoDiffXd -- which is simply a wrapping of Eigen's AutoDiffScalar (with dynamically allocated derivatives of type double). The snippet above shows you roughly how it works -- every scalar value has a vector of (partial) derivatives associated with it. On entry to the function, you are passed x with the derivatives of x set to dx/dx (which will be 1 or zero).
Your job is to return a value, call it y, with the value set to the value of your cost/constraint, and the derivatives set to dy/dx. Normally, all of this happens magically for you. But it sounds like you get to do it yourself.
Here's a very simple code snippet that, I hope, gets you started:
from pydrake.all import AutoDiffXd, MathematicalProgram, Solve
prog = MathematicalProgram()
x = prog.NewContinuousVariables(1, 'x')
def cost(x):
return (x[0]-1.)*(x[0]-1.)
def constraint(x):
if isinstance(x[0], AutoDiffXd):
y = AutoDiffXd(2*x[0].value(), 2*x[0].derivatives())
return [y]
return 2*x
cost_binding = prog.AddCost(cost, vars=x)
constraint_binding = prog.AddConstraint(
constraint, lb=[0.], ub=[2.], vars=x)
result = Solve(prog)
Let me know?
this is a three part question on the use of the Python API to Z3 (Z3Py).
I thought I knew the difference between a constant and a variable but apparently not. I was thinking I could declare a sort and instantiate a variable of that sort as follows:
Node, (a1,a2,a3) = EnumSort('Node', ['a1','a2','a3'])
n1 = Node('n1') # c.f. x = Int('x')
But python throws an exception saying that you can't "call Node". The only thing that seems to work is to declare n1 a constant
Node, (a1,a2,a3) = EnumSort('Node', ['a1','a2','a3'])
n1 = Const('n1',Node)
but I'm baffled at this since I would think that a1,a2,a3 are the constants. Perhaps n1 is a symbolic constant, but how would I declare an actual variable?
How to create a constant set? I tried starting with an empty set and adding to it but that doesn't work
Node, (a1,a2,a3) = EnumSort('Node', ['a1','a2','a3'])
n1 = Const('n1',Node)
nodes = EmptySet(Node)
SetAdd(nodes, a1) #<-- want to create a set {a1}
solve([IsMember(n1,nodes)])
But this doesn't work Z3 returns no solution. On the other hand replacing the 3rd line with
nodes = Const('nodes',SetSort(Node))
is now too permissive, allowing Z3 to interpret nodes as any set of nodes that's needed to satisfy the formula. How do I create just the set {a1}?
Is there an easy way to create pairs, other than having to go through the datatype declaration which seems a bit cumbersome? eg
Edge = Datatype('Edge')
Edge.declare('pr', ('fst', Node), ('snd',Node))
Edge.create()
edge1 = Edge.pr(a1,a2)
Declaring Enums
Const is the right way to declare as you found out. It's a bit misleading indeed, but it is actually how all symbolic variables are created. For instance, you can say:
a = Const('a', IntSort())
and that would be equivalent to saying
a = Int('a')
It's just that the latter looks nicer, but in fact it's merely a function z3 folks defined that sort of does what the former does. If you like that syntax, you can do the following:
NodeSort, (a1,a2,a3) = EnumSort('Node', ['a1','a2','a3'])
def Node(nm):
return Const(nm, NodeSort)
Now you can say:
n1 = Node ('n1')
which is what you intended I suppose.
Inserting to sets
You're on the right track; but keep in mind that the function SetAdd does not modify the set argument. It just creates a new one. So, simply give it a name and use it like this:
emptyNodes = EmptySet(Node)
myNodes = SetAdd(emptyNodes, a1)
solve([IsMember(n1,myNodes)])
Or, you can simply substitute:
mySet = SetAdd(SetAdd(EmptySet(Node), a1), a2)
which would create the set {a1, a2}.
As a rule of thumb, the API tries to be always functional, i.e., no destructive updates to existing variables, but you instead create new values out of old.
Working with pairs
That's the only way. But nothing is stopping you from defining your own functions to simplify this task, just like we did with the Node function in the first part. After all, z3py is essentially Python library and z3 folks did a lot of work to make it nicer, but you also have the entire power of Python to simplify your life. In fact, many other interfaces to z3 from other languages (Scala, Haskell, O'Caml etc.) precisely do that to provide a much easier to work with API using the features of their respective host languages.
I'm working on Arrows in F# and I wanted to create a *** operator. I note, however, that (***), the necessary way to express an operator in a function definition, overlaps with the F# block comment syntax. So how could you actually express this?
I thought of maybe .***. but I think that will actually treat the dots as part of the operator, which I'd rather avoid.
Yes, but you need to add spaces between the parentheses and the asterisks:
let ( *** ) x y = x * y
let z = 4 *** 5