When attempting to solve my mathematical program, drake produces the RuntimeError: PyFunctionConstraint: Output must be of scalar type AutoDiffXd, but unable to infer scalar type.
The constraint responsible for the error is as follows:
def non_penetration_constraint(q):
plant_ad.SetPositions(context_ad, q)
Distances = np.zeros(6, dtype=q.dtype)
i = 0
for name in Claw_name:
g_ids = body_geometries[name]
assert(len(g_ids) == 1)
Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(
Worldbody_id, g_ids[0]).distance
i += 1
return Distances
Called with:
for n in range(N-1):
# Non penetration constraint
prog.AddConstraint(non_penetration_constraint,
lb = [0] * nf,
ub = [0] * nf,
vars = (q[:, n]))
Has anybody had a similar issue before?
I suspect that your query_object (and perhaps other values) are not from the autodiff version of your plant/scene_graph.
Constraints implemented like this can potentially be called with q[0] either a type float or type AutoDiffXd. See the "writing custom evaluator" section of this Drake tutorial. In your case, the return value from your constraint is coming out of the query_object, which is not impacted directly by the plant_ad.SetPositions call (which seems suspicious). You'll want to make sure the query_object is generated from the autodiff scene_graph, and presumably after you set the positions to your constraint value.
Related
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.
The error I get:
query.lua:25: attempt to compare number with string
The rest of my code - I'm not sure why its not working, because I made sure that both values were in fact numbers, and the error still keeps happening, I have tried looking it up, and I cant find a solution, any help would be greatly appreciated.
print("Enter L,W,H values")
x = read()
y = read()
z = read()
length = 5
width = 5
height = 5
length = x
width = y
height = z
volume = length * width * height
print(volume.." blocks to mine")
turtle.refuel()
turtleFuel = turtle.getFuelLevel()
fuelNeeded = turtleFuel - volume
if fuelNeeded >= 0 then
print("Enough Fuel Detected")
else
print("not Enough fuel, error,"..fuelNeeded..", fuel required "..volume)
end
length1 = length
while length1 > 0 do
turtle.forward()
length1 = length1 - 1
end
In Computercraft, read() returns a string. You should change the first few lines of your code:
Instead of
x = read()
y = read()
z = read()
write
x = tonumber(read())
y = tonumber(read())
z = tonumber(read())
Do note, that read() is not the same as io.read(). One is a standard Lua function (which is also implemented in Computercraft) and read() is a function created specifically for CC computers and that can take a few extra parameters, and that you can use to do a few tricks once you get more advanced. (I am saying this because someone in the comments mentioned checking io.read())
Also, you mentioned that you "checked the type of the variable in the editor". Lua is a dynamically typed language, and the type of a variable CANNOT be known before the program is actually run. What your editor is probably doing is guessing the type of the variable based what it expects your program to do. These guesses are almost always inaccurate. That's why you should understand what your program is doing, and not rely on your editor. If you want to be extra sure, you want to run some tests, or the type of the variable simply is not known before the program is actually executed (because it may vary), you can use type(), like you have, to check its type at runtime. type() returns a string, representing the type of the variable:
type(nil) ---> nil
type("helloworld") ---> "string"
type(42) ---> "number"
type(function()print("HELLO")end) ---> "function"
type({1,2,3}) ---> "table"
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 questions is strongly related to adding-forces-to-body-post-finalize
I would like to be able to apply an external force to simple geometric primitives in pydrake.
This is to perform an evaluation of interactions between bodies.
My current implementation:
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.001))
parser = Parser(plant)
cube_instance = parser.AddModelFromFile('cube.urdf', model_name='cube')
plant.Finalize()
force = builder.AddSystem(ConstantVectorSource(np.zeros(6)))
builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())
diagram = builder.Build()
However when I run it, I get the following error:
builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())
RuntimeError: DiagramBuilder::Connect: Cannot mix vector-valued and abstract-valued ports while connecting output port y0 of System drake/systems/ConstantVectorSource#0000000002db5aa0 to input port applied_spatial_force of System drake/multibody/MultibodyPlant#0000000003118680
I have an inclination that I have to implement a LeafSystem which implements the abstract-value port on the plant.
Update based on Suggestion
Use: AbstractValue and ConstantValueSource
value = AbstractValue.Make([np.zeros(6)])
force = builder.AddSystem(ConstantValueSource(value))
ref_vector_externally_applied_spatial_force = plant.get_applied_spatial_force_input_port()
builder.Connect(force.get_output_port(0), ref_vector_externally_applied_spatial_force)
I got the following error:
RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource#0000000002533a30 (type pybind11::object) to
input port applied_spatial_force of System drake/multibody/MultibodyPlant#0000000002667760 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)
Which makes sense the types of the input-output ports should match.
The expected type seems to be a vector of ExternallyAppliedSpatialForce.
I then changed the Abstract type as follows:
value = AbstractValue.Make(ExternallyAppliedSpatialForce())
RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource#0000000002623980 (type drake::multibody::ExternallyAppliedSpatialForce<double>)
to input port applied_spatial_force of System drake/multibody/MultibodyPlant#00000000027576b0 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)
I am getting closer. However, I was not able to send a vector of ExternallyAppliedSpatialForce. If I try to send it as a list, I get a complaint that it is unable to pickle the object. I did not see in the AbstractValue examples how to create such a vector of objects.
Any additional help would be greatly appreciated.
Solution was to use the type VectorExternallyAppliedSpatialForced
full solution to be posted later.
Here is a working example of applying an external static force to a rigid object:
sim_time_step = 0.001
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, sim_time_step)
object_instance = Parser(plant).AddModelFromFile('box.urdf')
scene_graph.AddRenderer("renderer", MakeRenderEngineVtk(RenderEngineVtkParams()))
ConnectDrakeVisualizer(builder, scene_graph)
plant.Finalize()
force_object = ExternallyAppliedSpatialForce()
force_object.body_index = plant.GetBodyIndices(object_instance).pop()
force_object.F_Bq_W = SpatialForce(tau=np.zeros(3), f=np.array([0., 0., 10.]))
forces = VectorExternallyAppliedSpatialForced()
forces.append(force_object)
value = AbstractValue.Make(forces)
force_system = builder.AddSystem(ConstantValueSource(value))
builder.Connect(force_system.get_output_port(0), plant.get_applied_spatial_force_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant.SetPositions(context, object_instance, [0, 0, 0, 1, 0, 0, 0])
time_ = 0
while True:
time_ += sim_time_step
simulator.AdvanceTo(time_)
time.sleep(sim_time_step)
However, I was not able to change externally applied force in the simulation loop afterwards.
To achieve this I had to make a LeafSystem.
An implementation that allows you to change the force applied to a rigid object over time can be found here
Both static and dynamic examples can be found here.
No need to implement a LeafSystem — we have a ConstantValueSource that is analogous to ConstantVectorSource, but for Abstract types.
https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_constant_value_source.html
And you’re correct that you need the abstract type for this port: https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ab2ad1faa7547d440f008cdddd32d85e8
Some examples of working with the abstract values from python can be found here: https://github.com/RobotLocomotion/drake/blob/d6133a04/bindings/pydrake/systems/test/value_test.py#L84
I've come across some rather unusual behaviour in a bit of Delphi code. When using the in keyword to check if an item is in a constant array, I get the following compilation error:
E1012 Constant expression violates subrange bounds
The constants are defined as follows:
type TSomeEnum = (seFoo = 1000,
seBar = 2000,
seBoo = 3000,
seFar = 4000,
seFooBar = 5000,
seBooFar = 6000,
seLow = 1000,
seHigh = 6000,
seCount = 6);
The line that is failing is the following:
if someObj.someProperty in [seFoo, seFar, seFooBar] then
...
Whilst I understand the reasoning behind the error showing in another question posted here, where bounds checking on integer arrays wasn't done at compile time when using a variable, it seems odd that I'm getting the same problem with a constant array which is most certainly within bounds.
For now, I've replaced the line with a (much larger) statement comprising of or clauses. However, this is clearly not ideal. Can anyone shed any light on why I'm getting this problem?
Documentation about Sets says :
The base type can have no more than 256 possible values, and their ordinalities must fall between 0 and 255.
So even if you can have enums of any value, the if xx in [a,b,c] statement will fail here, since a set cannot hold a value larger than 255.
Use a case statement instead:
case xx of
a,b,c : // Make something
end;