I am trying to use drake to control a kuka iiwa robot and use the ManipulationStationHardwareInterface for Lcm communication. Before deployment on the real robot, I use mock_station_simulation to test. One thing that I find is that after initializing the simulator (which I think should trigger the initialization event?), Eval HardwareInterface's output will give the default values instead of the current lcm message values. For example,
drake::systems::DiagramBuilder<double> builder;
auto *interface = builder.AddSystem<ManipulationStationHardwareInterface>();
interface->Connect();
auto diagram = builder.Build();
drake::systems::Simulator<double> simulator(*diagram);
auto &simulator_context = simulator.get_mutable_context();
auto &interface_context = interface->GetMyMutableContextFromRoot(&simulator_context);
interface->GetInputPort("iiwa_position").FixValue(&interface_context, current_position);
simulator.set_publish_every_time_step(false);
simulator.set_target_realtime_rate(1.0);
simulator.Initialize();
auto q = interface->GetOutputPort("iiwa_position_measured").Eval(interface_context);
std::cout << "after initialization, interface think that the position of robot is" << q << std::endl;
q will be a zero vector.
This behavior bothers me when I try to use the robot_state input port of DifferentialInverseKinematicsIntegrator. DifferentialInverseKinematicsIntegrator will use this q to initialize its internal state rather than the robot's real position. The robot will move violently. As a workaround, I need to read the robot's position first and use the SetPositions method of DifferentialInverseKinematicsIntegrator and do not connect the robot_state input port. Another thing is that LogVectorOutput will always have the default value as the first entry, which is of little use.
I think this problem should be related with the LcmSubscriberSystem. My question is that is it possible to use the Lcm message to initialize the system rather than using the default value?
Thank you.
This is an interesting (and very reasonable) question. I could imagine having an initialization event for the LcmSubscriber that blocks until the first message to arrive. But currently I don't believe that we guarantee the order of the initialization events in a Diagram (likely the order is determined by something like the order the system was added to the Diagram, and we don't have a nice mechanism for setting it). It's possible that the diff IK block could initialize before the LcmSubscriber.
In this case, I think it might be better to capture the first LcmSubscriber message yourself outside the simulation loop, and manually set the diff IK integrator initial state. Then start the simulation.
I'll see if I can get some of the other Drake developers to weigh in.
Related
I am working with a diagram which includes a MultiBodyPlant with a Propeller connected to it. The Propeller actually realizes numerous physical propellers which are distributed among the bodies of the MultiBodyPlant.
I am able to simulate the dynamics of the combined system by setting the prop forces with FixValue, so I'm on the right track.
What I'd like to be able to do is, given a configuration for the system (i.e. the MultiBodyPlant context) and a chosen propeller command, compute the generalized forces acting on the system. My sense is that this is not immediately available since the simulation is actually using the RNEA, and so does not aggregate the forces all together in that way. For what I'm doing (and even just as a sanity check), I would like to compute the forces directly, not just their effect on the evolution of the state.
Is there an existing method to compute this built into Drake, or should I compute it manually using the spatial jacobian of each propeller frame and the applied SpatialForce of the corresponding propeller? (Something along the lines of this question: How to get the matrix that maps external forces to generalized forces?)
Many thanks for your help.
I understand better now. It's a very reasonable request! You have two systems at play: the Propeller and MultibodyPlant. Unfortunately, the quantity you want is all of Propeller and just a piece of MultibodyPlant. We don't offer direct access to the B(q) matrix in that case.
What you can do is call with either AutoDiffXd or symbolic::Expression, call CalcImplicitTimeDerivativesResidual on the Diagram to get the entire dynamics in implicit form (to avoid taking M(q) inverse). You could call it twice -- once with the Propeller inputs set up via FixValue as AutoDiffXd and/or symbolic::Variable and again with them as zero, then subtract the difference.
Note: CalcImplicitTimeDerivativesResidual is relatively new; I haven't pushed the python bindings for it yet (but it's been on my list). Do you need it from python?
I think that perhaps you are looking for the MultibodyPlant reaction_forces output port?
I have been trying to set up a custom manipulation station with Kuka IIWA hardware in drake. I got the hardware interface working. When running a joint teleoperation code (adapted from drake/examples/manipulation_station/joint_teleop.py), the robot jerks violently (all joints tries to move to 0 position) at first and then continues to operate normally. On digging deeper, I found that this is caused by the FirstOrderLowPassFilter system. While advancing the simulation a tiny bit (simulator.AdvanceTo(1e-6)) to evaluate the LCM messages to set the initial GUI sliders-filter_initial_output_value-plant joint positions etc., to match the hardware, the FirstOrderLowPassFilter outputs a momentary value of 0. This sets the IIWA_COMMAND position to zero for an instance and causes a jerk.
How can I avoid this behavior?.
As a workaround, I am subscribing separately to the raw LCM message from the hardware, before initializing the drake systems and sets the filter_initial_output_value before advancing the simulation. Is this the recommended way?.
I think what you're doing (manually reading the LCM message) is fine.
In the alternative, look how a DiscreteDerivative offers the suppress_initial_transient = true option. Perhaps we could add a similar option (via unrestricted update event) to FirstOrderLowPassFilter so that the initial output value was sampled from the input at t == 0. But the event sequencing of startup may still be difficult. We essentially need to initialize the systems in their dataflow order, including refreshing output ports as events fire, which is not natively supported.
In another alternative, perhaps we could configure the IIWA_COMMAND publisher to not publish at t == 0, instead publishing only t >= 0.005.
FirstOrderLowPassFilter has a method to set the initial value. https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_first_order_low_pass_filter.html#aaef7539cfbf1acfa0cf487c371bc5360
It is used in the example that you copied from:
https://github.com/RobotLocomotion/drake/blob/master/examples/manipulation_station/joint_teleop.py#L146
I am trying to perform trajectory optimization for a robot arm attached to a free-floating base (to simulate a spacecraft-robot arm scenario).
For the base-torque actuation, I had previously tried attaching pseudo-links with actuators to the base CoM but these approaches did not work (see this). In the end, I decided to use the generalized forces to control the system instead of actuators as they seem to better represent my use of space-robots (see here). For linearization, I manually change the B (actuation) matrix to include the base torque actuators (reaction wheels).
With this background, I am trying to perform trajectory optimization using Direct Transcription where I aim to find u(t) which in my case is generalized forces instead of actuator forces. I add constraints that make certain generalized forces zero (translation forces on Base CoM) to better represent the real system (reaction wheels).
Now, when I use DirectTransciption function for the input_port_index parameter I provide input_port_index=plant.get_applied_generalized_force_input_port().get_index() to find trajectory using generalized forces instead of actuation forces.
However, when running Solve(), I get the following error:
RuntimeError: Actuation input port for model instance planarSC must be connected.
The error makes sense as indeed I do not connect the actuation input port. But the error still stays after I use plant.get_actuation_input_port().FixValue(plant.CreateDefaultContext(), np.zeros((plant.num_actuators(),1))) to set the actuation input port to zero.
Is there a way to perform DirectTranscription on generalized forces as u(t) instead of the actuation forces?
Update: I've corrected the context being provided to DirectTranscription. The python script now has the following code but still gives the same error about not having the actuation port connected:
trajOptContext = plant.CreateDefaultContext()
jointAcutation = np.zeros((plant.num_actuators(),1))
plant.get_actuation_input_port().FixValue(trajOptContext, jointAcutation)
prog = DirectTranscription(plant, trajOptContext, N,
input_port_index=plant.get_applied_generalized_force_input_port().get_index())
Apologies, in trying to be concise and clear my previous description of my question turned into a special case of the general case I'm trying to solve.
New Description
I'm trying to Compare the last emitted value of an Aggregation Function (Let's say Sum()) with a each element that I aggregate over in the current window.
Worth noting, that the ideal (I think) solution would include
The T2(from t-1) element used at time = t is the one that was created during the previous window.
I've been playing with several ideas/experiments but I'm struggling to find a way to accomplish this in a way is elegant and "empathetic" to Beam's compute model (which I'm still trying to fully Grock after many an article/blog/doc and book :)
Side inputs seem unwieldy because It looks like I have to shift the emitted 5M#T-1 Aggregation's timestamp into the 5M#T's window in order to align it with the current 5M window
In attempting this with side inputs (as I understand them), I ended up with some nasty code that was quite "circularly referential", but not in an elegant recursive way :)
Any help in the right direction would be much appreciated.
Edit:
Modified diagram and improved description to more clearly show:
the intent of using emitted T2(from t-1) to calculate T2 at t
that the desired T2(from t-1) used to calculate T2 is the one with the correct key
Instead of modifying the timestamp of records that are materialized so that they appear in the current window, you should supply a window mapping fn which just maps the current window on to the past one.
You'll want to create a custom WindowFn which implements the window mapping behavior that you want paying special attention to overriding the getDefaultWindowMappingFn function.
Your pipeline would be like:
PCollection<T> mySource = /* data */
PCollectionView<SumT> view = mySource
.apply(Window.into(myCustomWindowFnWithNewWindowMappingFn))
.apply(Combine.globally(myCombiner).asSingletonView());
mySource.apply(ParDo.of(/* DoFn that consumes side input */).withSideInputs(view));
Pay special attention to the default value the combiner will produce since this will be the default value when the view has had no data emitted to it.
Also, the easiest way to write your own custom window function is to copy an existing one.
So i've gone thru the Audio Unit Hosting Guide for iOS and they hint all along that one might always want to use an AUGraph instead of direct connections between AUs. Among the most notable reasons they mention a high-priority thread, being able to reconfigure the graph while it is running and general thread-safety.
My problem is that I'm trying to get as close to "making my own custom dsp effects" as possible given that iOS does not really let you dynamically load custom code. So, my approach is to create a generic output unit and write the DSP code in its render callback. Now the problem with this approach is if I wanted to chain two of these units with custom callbacks in a graph. Since a graph must have a single output au (for head), trying to add any more output units won't fly. That is, I cant have 2 Generic I/O units and a Remote I/O unit in sequence in a graph.
If I really wanted to use AUGraphs, I can think of one solution along the lines of:
A graph interface that internally keeps an AUGraph with a single Output unit plus, for each connected node in the graph, a list of "custom callback" generic output nodes that are in theory connected to such node. Maybe it could be a class / interface over AUNode instead, but hopefully you get the idea.
If I add non output units to this graph, it essentially connects them in the usual way to the backing AUGraph.
If however, I add a generic output node, this really means adding the node's au to the list and whichever node I am connecting in the graph to, actually gets its input scope / element 0 callback set to something like:
for each unit in your connected list:
call AudioUnitRender(...) and merge in ioData;
So that the node which is "connected" to any number of those "custom" nodes basically pulls the processed output from them and outputs it to whatever next non-custom node.
Sure there might be some loose ends in the idea above, but I think this would work with a bit more thought.
Now my question is, what if instead I do direct connections between AUs without an AUGraph whatsoever? Without an AUGraph in the picture, I can definitely connect many generic output units with callbacks to one final Remote I/O output and this seems to work just fine. Thing is kAudioUnitProperty_MakeConnection is a property. So I'm not sure that once an AU is initialized I can re set properties. I believe if I uninitialize then it's ok. If so, couldn't I just get GCD's high priority queue and dispatch any changes in async blocks that uninitialize, re connect and initialize again?