We need the concept of "world" in the multibody dynamics engine

[amcastro-tri]

An interesting conversation started in https://github.com/RobotLocomotion/drake/pull/2303#issuecomment-218277954 and moved to issue #2315.

What we are discussing is that the multibody dynamics engine in Drake does not have the concept of world. Even a bigger problem, there is no a common container that owns (and manages) all the objects in the simulation. For instance, the gravity vector now belongs to RigidBodyTree. Clearly this does not belong here. Imagine having two Atlas robots modeled as two RigidBodySystem's each with their own tree. Would we have two gravity vectors? In Drake right now this is solved by having the root of a common tree to be the "world". However this makes it difficult (if not impossible without a hack) to have a system like AtlasSystem with its own tree, sensors, actuators and controllers as a well tested unit. This would allow this system to be thoroughly tested and extended in future revisions. A user needing a dozen Atlas robots in a bigger world then would just instantiate a dozen instances of these systems and add them to a "world" system, like a MultibodyDynamicsSystem (containing a MultibodyDyanamicsWorld member), following the notion of systems are trees of systems.

This issue proposes to have a class MultibodyDyanamicsWorld. This class:

• Would be the owner of the DrakeCollision::Model: very necessary since there has to be a global manager of the collision elements in the entire simulation for all this objects to be able to interact.
• Would be the owner (std::unique_ptr's) of every object in the model (rigid body tree's, sensors, actuators, controllers, etc). This would simplify the rest of the code where only raw pointers will be needed.
• Gravity is a member of this world

Finally, solvers (another abstraction) work on this world by querying it when needing to compute jacobians, right hand sides or any other numerical operations.

[amcastro-tri]

I would also like the opinion of @sherm1 (I think we already talked about this) and @david-german-tri (see if System 2.0 supports this) here.

[liangfok]

May I suggest:

• RigidBody - a single link
• Joint - a single joint
• RigidBodyTree - a single robot, contains a set of RigidBody and Joint elements, and meta data on the kinematic tree and any kinematic loops that they form (expressed as constraints).
• RigidBodyForest - a collection of RigidBodyTree objects, a "world" software abstraction (which may be a RigidBody), and the floating DOFs that connect the "world" to each RigidBodyTree.
• MultiBodyDynamicsWorld - Contains one RigidBodyForest object, the DrakeCollision::Model, and the gravity vector, and any other constraints.

[sherm1]

One thing to untangle here: this discussion shouldn't involve System, which is Drake's dynamic system abstraction, like Simulink's "block". Those are black boxes that can have state, take input from other blocks, perform computations, and generate outputs; they are agnostic about what goes on inside the block. Except for direct feed-through signals the blocks are not solved simultaneously; and the feed-through mechanism is not well-suited to represent physical coupling.

Here we should focus on how to represent the shared environment that should be seen by all the physical elements of the system, some of which must be tightly coupled. The whole collection is likely to end up in a single System, so that all the coupled elements can be considered simultaneously when performing computations like calculating derivatives. Elements that are decoupled by delays or sampling could be candidates for being in separate Systems; I think it would be best to defer the System decomposition to some other discussion -- the environment issue is common to any multibody system.

[liangfok]

Right. I think we just want to ensure that MultiBodyDynamicsWorld and all software abstractions contained within it are usable by and useful to the System elements in all anticipated Drake applications.

[sherm1]

What is in the shared environment? Things that pop to mind:

• contact geometry and surface properties
• visual geometry because some of the sensors depend on it
• gravitational, magnetic, electrostatic effect models
• environmental conditions like wind and rain
• what else?

The world contributes to the dynamic system since some of the above properties can evolve in time. (BTW don't be an Earthist -- there are lots of interesting orbiting robots to simulate and gravity isn't a uniform field there.)

Question: can we insist on one-way coupling between the world and other elements of the model? We could decide to define the environment that way: anything that must be solved simultaneously with other physical elements can't be part of the world. Then feedback from physical elements that change the environment would always involve a delay, and physical elements that interact only through their effects on the world could be considered decoupled.

[liangfok]

Question: can we insist on one-way coupling between the world and other elements of the model?

My first reaction is this sounds good. However, before I decide for sure, I just wanted to clarify: Does this assumption prevent changes to be made in the world over time? For example, how would we model the changes in gravity that occur when a rocket ship blasts off a launch pad on Earth and goes into outer space? Would we have a dedicated System block that updates gravity each cycle of the simulation loop, which then impacts the subsequent simulation cycle?

[amcastro-tri]

@sherm1: I agree that System 2.0 should merely be a user of the multibody dynamics engine. However I decided to mention it here since the current design (with RigidBodyTree being a single tree with multiple robots attached to a single Earthist world with a constant gravity vector) implies that there always be a single RigidBodySystem. This design I propose will allow to have multiple RigidBodySystem's part of a bigger system.

Also regarding your question:

Question: can we insist on one-way coupling between the world and other elements of the model?

can you explain that to me, I didn't even understand what you meant :-)

[sherm1]

This design I propose will allow to have multiple RigidBodySystem's part of a bigger system.

I still think this is mixing apples and oranges. We can have a World object that is accessible to multiple RigidBodyTrees (to use current terminology). Neither World nor the RBTrees are Systems. Together those can be used to produce any number of System blocks, depending on the amount of coupling involved. If the equations are coupled (meaning they must be solved simultaneously) then they have to be in the same System. For example, we might have (RBTree1,World) used by System1 and (RBTree2,World) used by System2. That implies that we know RBTrees 1 & 2 are not coupled. We might have (RBTree3,RBTree4, World) used in System 3 because RBTrees 3 & 4 are known to be coupled.

By coupled I mean "must be solved simultaneously" NOT "one thing eventually influences the other".

[sherm1]

one-way coupling

If World is entirely constant this is not an issue. But if World is a dynamic system (say it has puddles with fluid dynamics), then you have to ask how those equations are coupled to those of the RigidBodyTrees. Do World's equations and RBTree's need to be solved simultaneously? That is, do I have to calculate time derivatives and outputs of World and the RBTrees simultaneously? Or can I first calculate World's time derivatives and outputs (using only past information from RBTrees), then use those to calculate RBTrees derivs & outputs? If the latter, then it would be conceivable to put World's dynamics into a different System than the RBTrees. Otherwise they have to be combined to achieve the coupling.

I am asking whether it would be acceptable simply to insist that the only dynamics allowed in World is dynamics which can be calculated independently of RBTree dynamics. In that case, a fully coupled fluid dynamics puddle wouldn't be allowed in World -- instead, it would be a separate Puddle object that could be combined with RBTrees to solve simultaneously. The System splits could then be System1(World), System2(RBTree,Puddle). Output from System1 would feed directly into System2. Output from System2 could then go through a SampleAndHold block and into the Input of System1 (where it could have a delayed influence on World's evolution.)

Not sure if I succeeded in clarifying anything.

[amcastro-tri]

Thanks, now I understand what you meant (I of course know the term one/two way coupling but I couldn't understand what you meant in this context). I started off simple by assuming everything would be two way coupled and that the structure of the code would allow for future optimizations. For instance, two robots (two RBT's) in one world would be computed in a monolithic approach. This might seem as an overkill if robot's do not interact but certainly very convening if they for instance shake hands. Of course you can think of more complex strategies where at run time you could identify these RBT's are not connected and therefore you could send them to a different thread each. But that is beyond the discussion here that is only focusing in the abstraction of a world (IMO).

[liangfok]

Just to be clear, does one-way coupling imply that, during a single cycle of the simulation loop, every block in the system diagram has access to the same world and can make changes to the world, but changes one block makes to the world will not be seen by any other block until the next simulation cycle?

[sherm1]

every block in the system diagram has access to the same world and can make changes to the world?

No block can make changes to anything; changes are made by solvers. Blocks produce outputs. Say there were two blocks: a WorldBlock (WB) and RigidBodyBlock (RBB). One-way coupling would mean you could connect WB's output directly to RBB's input, but you can't connect RBB's output to WB's input without an intervening sample & hold or delay block. Like this:

     --> WB -> RBB -->
     ^               v
     |<---- S&H <----|

Without the S&H block everything would have to be solved simultaneously so the WB and RBB blocks would have to be combined.

[sherm1]

Reminder: the underlying multibody system objects, which might include a World object and RBTree objects, are not blocks ("Systems" in Drake terminology). There are likely to be a lot of blocks that need access to the World object, for example.

[liangfok]

No block can make changes to anything; changes are made by solvers.

Are solvers hidden within blocks?

[sherm1]

Are solvers hidden within blocks?

No, they are separate. For a current example, consider Drake's simulate() method.That's a solver. It acts on the state of a composite System (i.e. block diagram) but is not part of any block.

[liangfok]

I see, so "solvers" are consumers of "blocks" and the "world" and can modify both? And, "blocks" are consumers of the "world" but cannot modify the "world"?

[sherm1]

I'm confused by your question, Liang. There is a System (==block diagram) abstraction that is const during a simulation. There is a state that changes during a simulation; solvers are what updates the state. Underneath there is tons of complicated code (like RigidBodyTree or lidar models) but those things are not Systems themselves.

[liangfok]

Let me rephrase, is it true that we are proposing to implement the following software abstractions?

1. State - Mutable and non-mutable variables. This includes the RigidBodyTree and the World.
2. System - A block diagram, i.e., System Framework 2.0, basically a component architecture that's more tightly coupled through shared State. It contains logic but no internal state (all state is maintained externally by the State software abstractions).
3. Solver - Software logic that uses State and System software abstractions to produce simulation results or control signals for a real system.

If the above is correct, I think the answer to the question about whether the World needs to be solved simultaneously with the system is "no" since the world only contains state and no logic to be solved.

[sherm1]

Now I'm really confused! In no sense are RigidBodyTree or World included in "State".

[liangfok]

How about the following?

Let me rephrase, is it true that we are proposing to implement the following software abstractions?

1. State - Basically a workspace for holding state that changes over time. This includes things like the current positions and velocities of all joints, the current wind velocity in the world, etc. It is used by the System and Solver.
2. RigidBody - A single rigid body. It contains details that do not change over time like its mass, inertia matrix, and dimensions.
3. Joint - Connects two RigidBody models together. Contains details that do not change over time like whether its rotational or prismatic, its range of motion, and velocity constraints, etc.
4. RigidBodyTree - A collection of RigidBody and Joint abstractions that describe a robot in the world.
5. World - Specifies the environment in which the RigidBodyTree models exist and are influenced by.
6. System - A block diagram, i.e., System Framework 2.0, basically a component architecture where individual components are more tightly coupled than usual through shared State, RigidBodyTree, RigiBody, and Joint instances. It contains logic but no internal state (all state is maintained externally by the State software abstraction).
7. Solver - Software logic that uses State and System software abstractions to produce simulation results or control signals for a real system.

If the above is correct, I think the answer to the question about whether the World needs to be solved simultaneously with the system is "no" since the world only contains state and no logic to be solved.

[sherm1]

I think this issue is just about World and got off topic. System 2.0 terminology -- we're using the term Context for the object that holds time, state, and other mutable things. So a solver (generic term; not proposing a class) acts on a const System to produce a series of Context values representing the trajectory.

[liangfok]

Hmm, I think the notions of "world" and "rigid body system" are lost somewhere in the statement:

So a solver (generic term; not proposing a class) acts on a const System to produce a series of Context values representing the trajectory.

If a solver can only act on a const System to produce a series of Context objects, there will not be sufficient expressiveness for a world or rigid body tree(s). Maybe this is why I was assuming earlier that the Context (a.k.a State) contains the world and rigid body tree(s).

[sherm1]

I think the notions of "world" and "rigid body system" are lost somewhere

As I said at the beginning we shouldn't be mixing the discussion of World with the orthogonal abstractions involved in the system block diagram. If the distinction isn't clear please ask @amcastro-tri or @david-german-tri for a quick whiteboard session -- it is too hard to convey from here.

[liangfok]

After talking with @david-german-tri, I believe the static information like link mass that is contained within a RigidBodyTree is encapsulated within a System Block, and the non-stack information like joint state is contained within a Context. Regarding the world, I don't see why it should be treated any differently (static information is contained within a System Block and dynamic information is contained within a Context).

[sherm1]

Right, no reason to treat it differently at all w/r/t System 2.0 architecture. However it is clearly different than a RBTree which is the point of the discussion above.

[liangfok]

If the rigid body tree and the world both provide a set of differential equations to the system block, what would be the argument for enforcing a one-way relationship between the two sets of differential equations? Lower expressiveness in return for lower computational complexity?

[sherm1]

what would be the argument for enforcing a one-way relationship between the two sets of differential equations

The argument would be that the block used to model World's dynamics could then exist separately from blocks that use RBTree or other physical simulation code. Otherwise every block that can affect World has to be solved simultaneously with world. By transitive closure, that means all World-interacting blocks are coupled to each other, so they must be combined into a single block containing everything. That could be annoying!

We could also allow full coupling for a subset of blocks, so there would be (world+all_coupled_stuff) block and then separate blocks for uncoupled things. That assumes we can tell a world-coupled block from an uncoupled one.

The reason to consider special handling for World is that it is shared by everyone so can easily cause widespread coupling. RBTrees could otherwise often be completely decoupled from one another, providing a lot of useful options for how to simulate them. (Reminder that "decoupled" here means "doesn't have to be solved simultaneously" -- there can still be interaction as long as there is some delay involved.)

[liangfok]

I believe I understand; thanks! Based on my understanding, I don't think we will be giving up much expressiveness by mandating that the world maintains a one-way relationship with the other system blocks, especially if we can also support the world + all coupled stuff.

To keep thing concrete, and ensure I understand, what are the implications of the one-way mandate on the rocket ship example I mentioned above? I believe that during each simulation cycle, the kinematic and dynamic state of the rocket ship can be updated based on the world state. The output of the system block can output a modified world state in terms of an updated gravity vector. The modified world state will not take affect until the next simulation cycle. This is as opposed to updating the gravity simultaneously with the rocket ship's state within a single simulation cycle, which would be possible if the world and system blocks can maintain a two-way dependency. Is this correct?

[sherm1]

You'll need a different example, Liang! In the rocket case there is no change to the world. It is happily sitting there being gravitational. The model you are proposing for gravity (a uniform field) is an invalid approximation in that case and shouldn't be used. Here is Newton's law of gravitation: Note the "r" in that formula! That would be the distance between the rocket and Earth center. The uniform field that you are used to assumes no significant change in r.

I don't have a great example in mind -- maybe someone else does. How about tires rolling on a road and leaving rubber behind that changes the road's friction coefficient that affects the car's motion? Do I need to solve the rubber-transfer equations and vehicle dynamics simultaneously? Or can I take the friction coefficient from the unblemished surface, move the vehicle accordingly, calculate rubber transfer, update the road surface in the world, repeat?

[liangfok]

Doh! Maybe if we extend the rocket ship example to the moon or Mars. In this case, the "G" parameter will need to vary as the rocket ship leaves Earth's gravitational field and enters the field of another body in space.

I think that tire-rubber-transfer-to-road surface example is good. I think it would be OK to use the current road surface friction to calculate the vehicle dynamics, and then subsequently update the surface friction, each cycle of the simulation loop.

Here's another question: Suppose we initially enforce a rule that the world equations cannot depend on the system block equations. How difficult would it be to violate this requirement in the future should we decide it is necessary? In other words, how much of the software API and infrastructure will need to change if we do eventually need to support bidirectional dependences between the world and system blocks?

[sherm1]

In this case, the "G" parameter will need to vary as the rocket ship leaves Earth's gravitational field ...

Good try, but no -- "G" is the universal gravitational constant. They don't call it "universal" for nothing! You are confusing it with "g", the strength of the gravitational field at the Earth's surface.

How difficult would it be to violate this requirement in the future ...

Good question. I think we would have to leave it as is forever but could add another "CoupledWorld" object that could be used instead, and would live in the same block with everything else.

[amcastro-tri]

I was trying to keep the concept of world here, first, as an abstraction. The world is this global container of all objects in the simulation. Not only palpable ones like rigid (or soft) bodies but also things like force elements (a force element could be as simple as a spring or as complex as the gravitational field caused by the mass distribution of several planets or even a fluid field). In any case, just an abstraction. From the c++ side of things this abstraction gives me the notion of container+ownership+management of these objects.

Digging deeper into the interesting approaches you mention that I'd prefer calling:

• Monolithic approach: a fully coupled world (two-way you called it).
• Partitioned approach: where chunks of this world are solved separately (possibly in parallel). Sometimes they might not interact, eg.: they are just two robots only interacting with the ground but not between them. But sometimes they might interact occasionally and a partitioned approach is computationally more attractive. e.g: The same two robots now carry a heavy box holding each a handle on each side (the box couples them but being the box real heavy the coupling is weak).

It is the responsibility of the solver now to choose the appropriate approach tough being the world a container there might be special cases. Imagine the hierarchy MultibodyWorld <-- OpenMPMultibodyWorld where the OpenMP one uses threads to manage the partition of the computation into threads (MPI would be an option if we ever go large).

And @sherm1, your model of gravity is also incorrect. It does not consider gravity waves! :-)

[liangfok]

In this case, the "G" parameter will need to vary as the rocket ship leaves Earth's gravitational field ...

Good try, but no -- "G" is the universal gravitational constant. They don't call it "universal" for nothing! You are confusing it with "g", the strength of the gravitational field at the Earth's surface.

Oh, my mistake. I didn't realize there was a difference between G and g.

Regarding @amcastro-tri's comment of "monolithic" vs. "partitioned" approach, isn't "partitioned" a superset of "monolithic"? In other words, if we design and implement a software framework that supports the "partitioned" approach, don't we automatically get the "monolithic" approach?

[amcastro-tri]

No we wouldn't. They are two different animals. Usually a monolithic approach will force you to assemble a matrix of the whole system, something you wouldn't need to do for partitioned approaches. However, I can think of ways of sharing as much code as possible between the two. For instance, an RBT "knows" how to build it's dynamic matrices. A solver can query the RBT to do this task and simply place it withing a bigger matrix coupling several components together. With this I am moving towards the process of assembly, a much technical problem. Again, the concept of world, at least at first, is just abstract and should not depend on what solution strategy we use.

Physically, think of the world as just that. The one space where we throw all of our dynamics components. The world could be a room, planet earth, a galaxy. In C++ your world is your container/owner/manager of all dynamics engine objects in your simulation.

[amcastro-tri]

sorry, pressed the wrong button and closed the issue!

[liangfok]

Usually a monolithic approach will force you to assemble a matrix of the whole system, something you wouldn't need to do for partitioned approaches.

Sorry to keep questioning, but, for the sake of understanding, don't we need to assemble a matrix of the whole sub-system within each partition? Isn't the monolithic approach simply the partitioned approach where the whole system is a single partition?

[amcastro-tri]

You could see it that way. But many times this partition is not obvious (requiring some complex logic to identify non-interacting clusters of objects), sometimes it is (like two walking robots no interacting but only with the ground) and sometimes you may want to exploit this structure to speed things up.

In any case to me the monolithic approach is what you'll write in this case first (brute force) and when a bottleneck is identified you can move to a partitioned approach.

[sherm1]

The world is this global container of all objects in the simulation... also things like force elements (a force element could be as simple as a spring)

I don't think that is the right abstraction -- I didn't realize you were thinking of it that way. I don't see a reason to think of the World as owning everything in it. For example, a robot with an internal spring element certainly ought to own that spring. What I think we need is a World object that can provide the common global frame, terrain, fixed objects, weather, force fields, etc. that every other simulated subsystem can interact with. That doesn't imply an ownership relationship. We may also need a container for everything, but I think that is very distinct from the need to model and share the physical properties of the World.

your model of gravity is also incorrect. It does not consider gravity waves! :-)

True! Actually that's a good example of one-way dynamic coupling from a World with gravity waves to objects interacting with it. Presumably we can neglect the tiny back coupling from objects to the waves, so we could model the dynamics of World to generate the current phase of the waves, then update the objects afterwards.

[liangfok]

I'm going to close this issue because I believe we've grown to accept the fact that RigidBodyTree is the world in Drake and will likely remain so in the near future.