I am trying to create LQR for acrobot system from scratch:
file_name = "acrobot.sdf" # from drake/multibody/benchmarks/acrobot/acrobot.sdf
acrobot = MultibodyPlant()
parser = Parser(plant=acrobot)
parser.AddModelFromFile(file_name)
acrobot.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
acrobot.Finalize()
acrobot_context = acrobot.CreateDefaultContext()
shoulder = acrobot.GetJointByName("ShoulderJoint")
elbow = acrobot.GetJointByName("ElbowJoint")
shoulder.set_angle(context=acrobot_context, angle=0.0)
elbow.set_angle(context=acrobot_context, angle=0.0)
Q = np.identity(4)
R = np.identity(1)
N = np.zeros([4, 4])
controller = LinearQuadraticRegulator(acrobot, acrobot_context, Q, R)
Running this script I receive error at the last string:
RuntimeError: Vector-valued input port acrobot_actuation must be either fixed or connected to the output of another system.
None of my approaches to fix/connect input ports were eventually successful.
P.S. I know that there exists AcrobotPlant, but the idea is to create LQR from sdf on the run.
P.P.S. Why acrobot.get_num_input_ports() return 5 instead of 1?
Here are the deltas that I had to apply to have it at least pass that error:
https://github.com/EricCousineau-TRI/drake/commit/e7167fb8a
Main notes:
You had either (a) use plant_context.FixInputPort on the relevant ports, or (b) use DiagramBuilder to compose systems by using AddSystem + Connect(output_port, input_port.
I'd recommend naming the MBP instance plant, so that way you can refer to model instances directly.
Does this help some?
P.P.S. Why acrobot.get_num_input_ports() return 5 instead of 1?
It's because it's a MultibodyPlant instance, which has several more ports. Preview from plot_system_graphviz:
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.
I want to create a custom aggregator where the state is the unique client state of each client. To initialize I can define client states as usual, and then use federated_collect to place #SERVER placement since thats what initialize_fn() wants. I can also do the same for creating new_state in the next_fn(). The problem is once I don't know how I can "broadcast" these states back into clients. Normally federated_broadcast takes say A#SERVER and then makes copies of it equal to number of clients. so for two clients it would be {A}#CLIENTS lets say (A A). What I want is to have AB#SERVER turning into (A B).
I am currently defining client states outside the aggregation process, and then passing these in run_one_round of iterative process. use federated_collect to collect these states from measurements of aggregator, and then unstack it outside. So from the outside of the federated computations it looks like
server_state, train_metrics , client_states, aggregation_state = iterative_process.next(
server_state, sampled_train_data, client_states, aggregation_state)
client_states = [x for x in client_states]
In TFF
output = aggregation_process.next(aggregation_state, client_outputs.weights_delta, client_states)
new_aggregation_state = output.state
round_model_delta = output.result
new_client_states = output.measurements
in aggregator
measurements = tff.federated_collect(new_client_states)
return tff.templates.MeasuredProcessOutput(
state=new_state, result= round_model_delta, measurements=measurements)
But I am trying to define and handle these client states completely inside the aggregator so that I can plug this aggregator into tff.learning.build_federated_averaging_process like
iterative_process = tff.learning.build_federated_averaging_process(
model_fn,
client_optimizer_fn=lambda: tf.keras.optimizers.SGD(learning_rate=0.02),
server_optimizer_fn=lambda: tf.keras.optimizers.SGD(learning_rate=1.0),
model_update_aggregation_factory=my_aggregation_factory)
Is that possible? if so how?
tff.federated_collect is likely not the desired tool in this situation, and it will be removed in future versions of TFF (see commit #030a406).
Alternatively, A tff.federated_computation can both take #CLIENTS parameters as input, and return #CLIENT placed values as output. Instead of collecting all the values on the server first (implying that the system is communicating the states); it maybe best to leave the values on the clients.
When executing TFF in a simulation environment (e.g. invoking a tff.Computation in a Colab notebook) a T#CLIENT placed value will be returned as a list of T objects; one for each client. This can later be used as a parameter to future tff.Computation invocation.
Example:
#tff.tf_computation(tf.int32)
def sqrt(value):
return tf.math.sqrt(tf.cast(value, tf.float32))
#tff.federated_computation(tff.types.at_clients(tf.int32))
def federated_sqrt(values):
return tff.federated_map(sqrt, values)
client_values = [1,2,3,4]
federated_sqrt(client_values)
>>> [<tf.Tensor: shape=(), dtype=float32, numpy=1.0>,
<tf.Tensor: shape=(), dtype=float32, numpy=1.4142135>,
<tf.Tensor: shape=(), dtype=float32, numpy=1.7320508>,
<tf.Tensor: shape=(), dtype=float32, numpy=2.0>]
Important caveat: the order of inputs and outputs is not necessarily guaranteed to be the same. An example of how to index and track states across invocations can be found in the tensorflow_federated/python/examples/stateful_clients/ directory inside the repository.
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
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.
When creating a graph of calculations using delayed I'm trying to assign names so that if I visualize the graph it's readable. However, for delayed variables that are dependent on functions the name parameter doesn't seem to affect the key. Here's a toy example:
def calc_avg(a, b):
return pd.concat([a, b], axis=1).mean(axis=1)
def calc_ratio(a, b):
return a / b
a = delayed(pd.Series(np.random.rand(10)), name='a')
b = delayed(pd.Series(np.random.rand(10)), name='b')
c = delayed(pd.Series(np.random.rand(10)), name='c')
x = delayed(calc_avg, name='avg_result')(a,b)
y = delayed(calc_ratio, name='ratio_result')(x,c)
y.visualize()
You can see the visualization here (I can't embed images), but rather than seeing 'avg_result' I see 'calc_avg-#0' and rather than see 'ratio_result' I see 'calc_ratio-#1'. If I look at x.key or y.key they do not match the names that I provided. Is this the expected behavior?
The key of a dask result needs to be unique for every combination of the function that was delayed, and the inputs you give it. What you see above is the expected behaviour: you are naming the function, but a call with different inputs would expect a different output, so the key must be different.
You can specify the key you'd like associated not when you define the delayed function, but when you call it:
x = delayed(calc_avg)(a, b, dask_key_name='avg_result')
y = delayed(calc_ratio)(x, c, dask_key_name='ratio_result')