I have a system consisting of a MultibodyPlant (and scene graph), which loads an iiwa manipulator from a URDF file, and a LeafSystem that is creating ROS TF messages corresponding to the geometry stored in the MultibodyPlant.
The LeafSystem has an input port that receives a graph query object, and an output port that produces ROS TF messages. The input port is connected to the scene graph's graph query port. The output port is connected elsewhere, which is not relevant to my problem.
The LeafSystem's output port's value is calculated by this callback. It inspects the scene graph and creates a TF frame for every frame in the scene graph.
The problem I'm having is that the scene graph has no knowledge of the hierarchical relationship between bodies in the MultibodyPlant. This means that the TF messages produced contain a set of frames that are all rooted in the world frame. I would like to instead have them reflect the actual hierarchy of joints and links that is in the URDF. The MultibodyPlant has this information. What architecture or ports should I use to make the information available in my LeafSystem's output port value-calculating callback?
Can the solution to this be generic to any system that contains geometry, or does it only make sense for a MultibodyPlant? (I may not be understanding systems correctly here - I'm relatively new to Drake.)
Related
I'm working on a project that requires me to add a model through Parser (which requires the plant to be of the same type as the array used) before Setting the position of the model in said plant and taking distance queries. These queries only work when the query object generated from the scene graph is of type float.
I've run into a problem where setting the position doesn't work due to the array being used being of type AutoDiff. A possible solution would then be converting the plant of type float to Autodiff with plant.ToAutoDiff(), but this only creates a copy of the plant without coupling it to the scene graph (and in turn the query object) from which the queries are derived. Taking queries with a query object generated from the original plant would then fail to reflect the new position passed to the AutoDiff copy.
Is there a way to create a new scene graph from the already finalized symbolic copy of the original plant, so that I can perform the queries with it?
A couple of thoughts:
Don't just convert the plant to autodiff. Convert the whole diagram. That will give you a converted, connected network.
You're stuck with the current workflow. Presumably, your proximity geometries are specified in your parsed file (as <collision> tags). The parsing process is ephemeral. The declaration is consumed, passed through MultibodyPlant into SceneGraph. If there is no SceneGraph at parse time, all knowledge of the declared collision geometry is forgotten.
So, the typical workflow is:
Create a float-valued diagram.
Scalar convert it to an AutoDiff-valued diagram.
Keep both around to serve the different roles.
We don't have a tutorial that directly shows scalar converting an entire diagram, but it's akin to what is shown in this MultibodyPlant-specific tutorial. Just call ToScalarType() on the Diagram root.
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())
In Abaqus, I want to compute the force resulting from a pressure I apply on a surface. This force is the sum of the nodal reactions of all nodes belonging to the surface.
Using history output, the only thing I can do is exporting the individual nodal reactions, which becomes awkward to handle when there is a lot of nodes.
So, is there a simple way, in the CAE interface or in the .inp input file to do this in a straightforward way?
In Abaqus/Standard, you may print nodal and/or element output to the data file (.dat) using the *node print input file keyword. In Standard or Explicit, you may print to the results file (.fil/.sel) using *node file. The keyword can be used for an individual node or for an entire node set. You can control whether the values are totaled, whether the output is in a local or global coordinate system, whether a summary is also printed, and the frequency the output is written to file.
The options and defaults are slightly different between *node print and *node file - for example, the summary and totals args are only available for node print. See the docs for more detail.
These keywords have to be placed within a Step definition. Assuming you have already defined the nset of interest, you can do something like:
*node print, nset=my_nset, totals=yes, global=yes
RF,
*node file, nset=my_nset, global=yes, frequency=999
RF,