State class and scene recording

[mxgrey]

We need to have a well-defined state class to go along with the new cloning abilities in DART. I'd like to open up a discussion about what exactly belongs in the State class. I think we should have two levels of State class: Skeleton::State and World::State.

At a minimum, the Skeleton::State should contain everything that is intrinsic to a Skeleton’s state. This would include generalized positions, generalized velocities, generalized accelerations, generalized forces, and any other kinds of internal forces (like the forces on the PointMasses for SoftBodyNodes). Is there anything else that anyone can think of that the Skeleton::State should include? For example, should external forces (a.k.a. applied forces) be included in the Skeleton::State? What about collisions?

Then for the World::State, this must include at least all the information needed to get identical simulation results if you were to run a simulation forward from the saved World::State. I imagine this would include the Skeleton::State of each Skeleton currently inside the World. Additionally, it probably needs to include detailed information about collisions between Skeletons. Is there anything else that the World::State should include?

Finally, I think it would be a good idea to have a class dedicated to recording Worlds / Scenes. The World class has a “state baking” feature, but this feature fails badly whenever Skeletons are added/removed or the number of BodyNodes in a Skeleton has changed. It also doesn’t account for changes in BodyNode/Joint properties (like the transform from the parent BodyNode), which can result in playbacks that do not accurately represent what happened. Also, the World is primarily used for simulation and might not be used by people who are only concerned with kinematic planning. So I propose an independent class for recording World::States and Skeleton::States.

The Recording class would take advantage of the new cloning features: whenever an observable structural change to a Skeleton property is observed, the Recording class can split off a clone of the Skeleton and refer to the clone while it records states, up until another structural change gets made to the Skeleton; then it can split off yet another clone. This way, a user can arbitrarily make structural changes to their Skeleton, and the Recording class will keep a unique preserved copy of the Skeleton for each time a change is made. This should allow 100% fidelity in the recording of Worlds and scenes.

[mxgrey]

I've given it a lot of thought, and I believe I've come up with a good approach for State classes.

To begin with: all the data members of every first (and second) class citizen will be split into three categories: Properties, Cache, and State.

Properties will contain any members that are not generally affected by time and therefore do not change often. Properties will also exhibit lazy copying during cloning by using a std::shared_ptr scheme.

Cache will contain any data that gets auto-updated, like what was proposed by @psigen in issue #337.

State will contain practically everything else.

Skeleton::State

The Skeleton State will be split into three subclasses:

(1) Skeleton::JointState contains a list of Eigen::VectorXds for joint positions, joint velocities, joint accelerations, joint forces, and joint commands.

(2) Skeleton::BodyState just contains a std::vector<BodyNode::State>.

(3) Skeleton::MetaState will be used to save the current layout of a Skeleton, and will have std::vectors which contain:

• BodyNode names
• parent BodyNode names
• BodyNode types (we'll use strings, similar to Joint types)
• Joint names
• Joint types
• Version number (which is incremented each time a structural change occurs)

The full Skeleton::State class will just be a composition of these three classes. By splitting them into separate classes, we allow the user to decide on the trade off between completeness and conciseness. For most purposes, JointState will likely be sufficient. To save the results of an entire simulation, the user may want both JointState and BodyState, but if it's a simulation where the Skeletons fall apart or have any kind of structural changes, then they'll want the full State class to ensure that everything can be replayed correctly.

Joint::State

I'm thinking there should be two forms of the Joint::State class: one which is dynamically sized and one which is statically sized. So the vanilla Joint::State class will contain a set of Eigen::VectorXds for the positions, velocities, accelerations, forces, and commands. Then there will be a SingleDofJoint::State class that contains a double instead of an Eigen::VectorXd for each of those. And there will also be a templated MultiDofJoint::State class that uses fixed size Eigen vectors. I'm thinking we can just leave out a ZeroDofJoint::State, because I imagine it would just be empty.

BodyNode::State

I'm thinking the BodyNode state can be constructed like this:

BodyNode::StandardState contains a bunch of fixed size Eigen vectors for things like external forces and moments.

BodyNode::Context is a base class which does nothing except provide a virtual clone() function which returns a std::unique_ptr<BodyNode::Context>. By default, the clone() function will just return a nullptr, but classes that inherit BodyNode can create their own Context class which will use the clone() function to provide a copy of its data. As an obvious example, the SoftBodyNode class will have a SoftBodyNode::Context class that contains the positions, velocities, etc, of all its point masses.

Then the BodyNode::State class will inherit the BodyNode::StandardState class, plus it will contain a std::unique_ptr<BodyNode::Context> instance. The BodyNode::State class will be copy-safe by copying the BodyNode::StandardState portion as normal but calling the clone() function on its context (unless the context is a nullptr, in which case it just passes along a nullptr). Move semantics will work perfectly fine as normal.

Depending on how the ShapeNode is implemented, it seems likely that we will want some kind of std::vector<ShapeNode::State> as part of the BodyNode::StandardState. The main thing I'd like to see in the ShapeNode::State would be stuff like current collision information (i.e. Is the ShapeNode in a collision? If so, then with what, and where?). I think we can also do something similar to the std::unique_ptr<BodyNode::Context> for ShapeNodes where we can give them a std::unique_ptr<ShapeNode::Context> that derived classes can use for any non-standard stateful data.

World::State

Ideally I would very very much like for the World::State to be nothing more than a std::vector<Skeleton::State> with some non-crucial meta data like a list of the current Skeleton names (in the order of their current indexes) and maybe the current time step.

[mxgrey]

I'd like to revise my proposal a bit.

@jturner65 and I were discussing the possibility of information like gearing, transmission, and servo motor properties being introduced to DART. It seems like the Addon concept would suit this well, where we'd have classes like Gearing and ServoMotor be Addons for the Joint class.

This implies that Joints may need an extensible State, which conflicts with my previous proposal where the joint positions, velocities, accelerations, and forces would all be consolidated into a few Eigen::VectorXds. So here is what I would propose instead:

Skeleton::Configuration

I imagine the configuration class would simply look like this:

struct Configuration
{
  Eigen::VectorXd mPositions;
  Eigen::VectorXd mVelocities;
  // Eigen::VectorXd mAccelerations?
  // Eigen::VectorXd mForces?
  // Eigen::VectorXd mCommands?
}

I don't know whether to include joint accelerations, forces, or commands in the configuration. Opinions on this would be greatly appreciated.

Skeleton::State

The Skeleton::State class would be the extended alternative to Skeleton::Configuration. You would use Skeleton::Configuration if you just want to store basic information about a Skeleton's configuration (and therefore have less overhead), but you would use Skeleton::State if you want to store additional state-related information, such as the external forces on each BodyNode or the positions of all the points in a SoftBodyNode. The critical goal of Skeleton::State is to guarantee that if you set a pair of identical Skeleton clones to the same Skeleton::State and run them through the same controller, then the forward simulation results of each will be identical, all the way to full machine precision.

We would no longer have Skeleton::JointState and Skeleton::BodyState. Instead we would have:

struct Skeleton::StandardState // or should this be called BasicState? Or any other name?
{
  std::vector<BodyNode::State> mBodyNodeStates;
  std::vector<Joint::State> mJointStates;
  common::AddonManager::State mAddonStates;
}

Then Skeleton::MetaState would remain the same as in the original proposal, and Skeleton::State would be a composition of Skeleton::StandardState and Skeleton::MetaState.

If anyone has opinions on this proposal, please don't hesitate to share.

Edit: Added mAddonStates to the Skeleton::StandardState struct

[jslee02]

It's not easy to decide which information should be included to a certain state class. I think we can avoid this problem by letting the user to determine it themselves by introducing two kinds of X::State depending on its flexibility.

The first kind is inflexible state classes whose members are not extendable just like Skeleton::Configuration and Skeleton::State. These inflexible state classes would be used for specific purpose. For example, Skeleton::State would be used to reproduce identical forward simulation to its original clone's one with same controller as @mxgrey said.

The other kind is flexible state class so that the members can be determined by the user as they want. The flexible state class should be able to store the information of which data are required to be stored in it. I haven't thought how to implement this clearly but it wouldn't be too hard to.

[mxgrey]

Your idea for a flexible State class should be covered by the State class that I mentioned, as long as the user achieves their "flexibility" by introducing Addons and/or Nodes to the Skeleton or to the BodyNodes and Joints in the Skeleton.

BodyNode::State will contain the State of every Node and Addon attached to the BodyNode. Joint::State will contain the State of every Addon attached to the Joint. Skeleton::StandardState will contain the State of every Addon attached to the Skeleton (I just edited my previous post to reflect this, because I accidentally left it out).

[mxgrey]

Although I guess when you talk about a flexible state class, you're saying that the user can cherry pick what kind of information gets stored in it instead of assuming that all the states of all the current Nodes and Addons should be stored.

At that point, I feel like maybe it should just be left up to the user to create their own state class that will poll whatever information it needs from the Skeleton.

[mxgrey]

I thought about it some more, and I realized why we would want a more flexible version of the state class. Suppose someone wants to use the machinery provided by the Recorder class, but they want very specific information to be recorded (perhaps they don't want to waste space on all the information needed to record a scene, or perhaps they want to record some information that they are processing themselves during the simulation).

I think the most sensible approach we could take would be to make the Recorder class templated. It would take in two template arguments: a data structure that holds a single timeslice of the information that they want, and some kind of functor that grabs the information that it needs from either the World or the specific Skeleton that the Recorder is recording.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed in 7 days if no further activity occurs. Thank you for your contributions.

[costashatz]

Any updates on this one? I would really like to be able to save the simulation state in order to produce identical simulations. Is there something I can help with?

[mxgrey]

When you say "save the simulation state", do you mean save it to memory or save it to disk? Because saving to memory should already be very easy, like in the osgSoftBodies example. Basically you save the Skeleton::State of each skeleton in your simulation at each time step. You might also need to save control signals separately, I'm less sure about that. It wouldn't be hard to make a Recorder class to do these things.

If you mean saving the simulation state to disk, then it's much much much harder because that requires serialization, which we don't currently have any support for.

[costashatz]

When you say "save the simulation state", do you mean save it to memory or save it to disk?

What I mean is more generic: What are the skeleton variables that I have to keep in order to produce identical simulations?

More precisely, here's my setup:

• I always have the same Skeletons in my world (thus I do not care about serializing the hard parts, because I always load exactly the same scene)
• I would like the following:
  • Run a simulation (with some controllers) from T_start = 0. to T_end = 10.
  • At each time step, T_i, save the state of my robots; e.g. the states at T_4 = 4. should be S_4
  • Run another simulation from T_4' = 4. to T_12 = 12., where the state in T_4 is identical to what it was in the initial simulation (i.e., S_4' = S_4). I would still save all the states.
  • Now I would like to "replay" the command sequence from T_start to T_4 and from T_4' to T_12 and get exactly the same results.

Is this possible? At the moment I am saving generalized positions, generalized velocities, generalized accelerations and the commands that produced them. When I replay the command sequence I am getting very similar results but not identical. I checked all the possible bugs that I could have (and solved a few of them), but the difference happens also when "replaying" motions that are not stitched up (i.e., replaying a sequence of commands in the same world), so I guess there's something that cannot be replicated exactly. One thing to notice is that if there are no collisions, then I do not get any difference (the difference is actual exactly 0). When there are collisions, things add up: the difference starts at 1e-16 and can get up to 0.2-0.3 (after chaining a few command sequences that come from different runs).

Sorry for the long post and please do not feel obliged to response in details. I would just like your opinion on whether I could store something additional in order to make the simulations identical. The main difference from the example that you showed me is that I want to replay (in open-loop) a sequence of commands from the same starting state and get the same results.

Thanks a lot in advance.

[mxgrey]

What are the skeleton variables that I have to keep in order to produce identical simulations?

Right, in theory the set of all Skeleton::States from each Skeleton in your simulation should contain everything needed to replay a simulation exactly (combined with Skeleton::Properties if the properties, [e.g. kinematic properties, friction properties] ever change over time).

If that doesn't work, then you may be encountering an issue related to #1089 / #1086. Let us know if Skeleton::State is failing you, and if you can provide test code for the failure then we can try to figure out what needs to be fixed to make the the Skeleton::State comprehensive.

[costashatz]

in theory the set of all Skeleton::States from each Skeleton in your simulation should contain everything needed to replay a simulation exactly

What is exactly saved in State? I need to save some things in a file and I guess serialization is not an option yet. So I guess that positions, velocities and forces? Or is it saving something else?

Let us know if Skeleton::State is failing you

I will try it out (without saving to file) to check and let you know.

[mxgrey]

What is exactly saved in State?

For better or worse, this is defined implicitly, not explicitly. What I mean by that is we did a redesign of DART a few years ago where dynamics data was split into three categories: state, properties, and cache. In theory, State should be any data that is normally time-varying (i.e. it might change each time step) while Properties should be any data that is typically held constant between timesteps. Cache data should never impact simulation because it should be updated internally as needed.

State and Properties are managed by the dart::common::Aspect class. Hypothetically it should be straightforward to add serialization to Aspect::State and Aspect::Properties to save states and properties to disk. However, serialization is never an easy undertaking and will probably require an external serialization library if we want to do it decently.

While I think State and Properties were tremendously helpful for getting a more robust mechanism for saving/recovering simulation data, I can't rule out the possibility that some data fields which belonged in State accidentally landed in Properties or a cache, which could lead to inconsistent replays.

Typically I would expect generalized positions, generalized velocities, and applied forces (both internal and external) to be sufficient to replay a simulation (if there are no soft bodies involved). But there may be more complicated situations that I'm not thinking of.

[mxgrey]

Oh, and the way to get the Skeleton state is Skeleton::getState().

[costashatz]

Typically I would expect generalized positions, generalized velocities, and applied forces (both internal and external) to be sufficient to replay a simulation (if there are no soft bodies involved). But there may be more complicated situations that I'm not thinking of.

OK. Thanks a lot! I will try saving all the forces as well and see what I get..

For better or worse, this is defined implicitly, not explicitly. What I mean by that is we did a redesign of DART a few years ago where dynamics data was split into three categories: state, properties, and cache. In theory, State should be any data that is normally time-varying (i.e. it might change each time step) while Properties should be any data that is typically held constant between timesteps. Cache data should never impact simulation because it should be updated internally as needed.

State and Properties are managed by the dart::common::Aspect class. Hypothetically it should be straightforward to add serialization to Aspect::State and Aspect::Properties to save states and properties to disk. However, serialization is never an easy undertaking and will probably require an external serialization library if we want to do it decently.

While I think State and Properties were tremendously helpful for getting a more robust mechanism for saving/recovering simulation data, I can't rule out the possibility that some data fields which belonged in State accidentally landed in Properties or a cache, which could lead to inconsistent replays.

Thanks a lot for the detailed explanation. Now things are clearer in my mind. I could help with the serialization. I am putting it on my TO-DO list and will let you know if I start working on it.

[costashatz]

Either I didn't get how getState() works or you are actually not saving any state information into State. Here's an example:

#include <iostream>

#include <dart/dart.hpp>

dart::dynamics::SkeletonPtr create_box(const Eigen::Vector3d& dims, double mass, const Eigen::Vector4d& color, const std::string& box_name)
{
    dart::dynamics::SkeletonPtr box_skel = dart::dynamics::Skeleton::create(box_name);

    // Give the box a body
    dart::dynamics::BodyNodePtr body;
    body = box_skel->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(nullptr).second;
    body->setName(box_name);

    // Give the body a shape
    auto box = std::make_shared<dart::dynamics::BoxShape>(dims);
    auto box_node = body->createShapeNodeWith<dart::dynamics::VisualAspect, dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(box);
    box_node->getVisualAspect()->setColor(color);
    // Set up inertia
    dart::dynamics::Inertia inertia;
    inertia.setMass(mass);
    inertia.setMoment(box->computeInertia(mass));
    body->setInertia(inertia);

    return box_skel;
}

dart::dynamics::SkeletonPtr create_floor()
{
    double floor_width = 10.;
    double floor_height = 0.1;

    dart::dynamics::SkeletonPtr floor_skel = dart::dynamics::Skeleton::create("floor");

    // Give the floor a body
    dart::dynamics::BodyNodePtr body = floor_skel->createJointAndBodyNodePair<dart::dynamics::WeldJoint>(nullptr).second;

    // Give the body a shape
    auto box = std::make_shared<dart::dynamics::BoxShape>(Eigen::Vector3d(floor_width, floor_width, floor_height));
    auto box_node = body->createShapeNodeWith<dart::dynamics::VisualAspect, dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(box);
    box_node->getVisualAspect()->setColor(dart::Color::Gray());

    // Put the body into position
    Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
    tf.translation()[2] -= floor_height / 2.0;
    body->getParentJoint()->setTransformFromParentBodyNode(tf);

    return floor_skel;
}

int main()
{
    auto world = std::make_shared<dart::simulation::World>();

    auto box_skel = create_box({0.1, 0.1, 0.1}, 1., {1., 0., 0., 1.}, "box");
    auto floor_skel = create_floor();

    box_skel->setPosition(5, 0.2);

    world->addSkeleton(box_skel);
    world->addSkeleton(floor_skel);

    for (size_t i = 0; i < 20; i++)
        world->step();

    std::cout << box_skel->getPositions().transpose() << std::endl;
    auto box_skel2 = box_skel->cloneSkeleton();
    box_skel2->setState(box_skel->getState());
    std::cout << box_skel2->getPositions().transpose() << std::endl;
    return 0;
}

Output:

0 0 0 0 0 0.19794
0 0 0 0 0 0

I guess the output should be the same, right? Or not?

[mxgrey]

Thanks for writing this example. I've confirmed that your code should be fine, and this is not working the way it's supposed to. I think this is some kind of regression, because I've successfully used Skeleton::State for this kind of application in the past, but it doesn't seem to be working properly anymore.

I'm going to create a regression test for this and see if I can find a fix.

[costashatz]

I'm going to create a regression test for this and see if I can find a fix.

Great! I am waiting for it. Keep me updated.. Thanks!

[mxgrey]

I'm not sure how it got past me in the first place, but the issue should be fixed now in PR #1244 .

I think I was overconfident in the State concept working because I thought Skeleton::State was working the osgSoftBodies example, but that example was actually written before Skeleton::State existed, so it was using BodyNode::State instead. I've changed that example to use Skeleton::State instead, and I've added a regression test (but it would be good to write more tests at some point).

[costashatz]

I'm not sure how it got past me in the first place, but the issue should be fixed now in PR #1244

Great! Thanks for the fix.. I will test it as soon as possible.

Now that we are at it. Could you give me some hints on what should be done to save Skeleton::State in disk? I am thinking of working on the serialization part and I would like some hints before I dive in.

[mxgrey]

I think the key thing would be to add something like std::string serialize() const and bool deserialize(const std::string&) virtual functions to the dart::common::Cloneable class. I don't know if std::string is the best type to use for inputs/outputs, but it's probably the most generic one.

After that we would probably want the functions to be implemented somehow by MakeCloneable and ProxyCloneable. I would very much discourage any attempt to write a serialization implementation yourself, because it's very ticky and extremely time-consuming. It would be better to try to find an external library that can help with implementing it (for example, I've heard good things about cereal so that might be worth looking at).

Personally I'd be perfectly happy to add a new dependency to the core of dart if it can help us serialize to disk. I don't know if @jslee02 would have any objections to that, though.

Perhaps the biggest challenge would be determining if/how we want to be able to reconstruct worlds just from saved files (as opposed to programmatically constructing the world, or defining the world in something like a .skel file, and then loading the states and properties seperately). I think it's feasible to have Skeleton/BodyNode/Joint/Node factories that could look at a saved file and reconstruct the necessary objects automatically, but it would be more of a matter of convenience than a critical feature.

[costashatz]

Perhaps the biggest challenge would be determining if/how we want to be able to reconstruct worlds just from saved files (as opposed to programmatically constructing the world, or defining the world in something like a .skel file, and then loading the states and properties seperately)

In all my use cases, the 2nd option (i.e., programmatically constructing the world, or defining the world in something like a .skel file, and then loading the states and properties seperately) is more than enough and most of the times preferable.

I think it's feasible to have Skeleton/BodyNode/Joint/Node factories that could look at a saved file and reconstruct the necessary objects automatically, but it would be more of a matter of convenience than a critical feature.

One additional problem with this would be the 3D mesh shapes loading. Or will we save this too?

[mxgrey]

One additional problem with this would be the 3D mesh shapes loading. Or will we save this too?

Ah, great point! I forgot that the one big hole currently in the State/Properties design is that it doesn't really touch the geometries. It's still not 100% clear to me at the moment what the right way to handle this is.

We could have the Shape class inherit dart::common::Composite and then require the Shape classes to store their states and properties as Aspects the way we currently do for BodyNode, Joints, and other Node types. Then a member of the ShapeNode::State and ShapeNode::Properties would be a proxy for the Shape::State and Shape::Properties.

That being said, if you're programmatically constructing/generating the Skeletons, then that shape data probably isn't crucial for you. So for the first iteration it would be fine to just ignore the Shape properties (none of the Shapes should really have any State currently) and then we could tag on features to save Shape properties to disk later.

[costashatz]

I am coming back at this one for some questions (before start coding the serializer):

• If I save positions, velocities, accelerations of all skeletons in one world, then I should be able to reproduce exactly the same simulations, right?
• Or should I also save some state of the collision detector?
• Or maybe even from the constraint solver?

Because in my mind, if I set all the objects in the world with the previous positions, velocities and accelerations, then this should be enough to compute everything: collisions, external forces etc. I would say that I could even skip the accelerations.

I am not getting this in the latest master of DART (neither in release-6.7) when the simulation involves collisions (sometimes I do, sometimes not; it is not consistent). Am I missing something?

Thanks in advance..

[mxgrey]

If you're not using any soft bodies, then in theory your first bullet point should be correct. But we've seen cases where things don't work out that way because of what I would call "hidden variables". The State and Properties concepts were introduced to try to root out hidden variables, although there are some corners of the code that haven't been fully refactored into State and Properties, so that could be causing some issues.

Are you able to show us an example case where the simulation is being inconsistent?

[costashatz]

Are you able to show us an example case where the simulation is being inconsistent?

So investigating it a bit more, the problem comes from the collision detectors. Here's a minimal example (utilizing my DART wrapper robot_dart):

#include <iostream>
#include <robot_dart/robot_dart_simu.hpp>

#include <robot_dart/control/pd_control.hpp>

#ifdef GRAPHIC
#include <robot_dart/graphics/graphics.hpp>
#endif

#include <dart/collision/bullet/BulletCollisionDetector.hpp>
#include <dart/collision/fcl/FCLCollisionDetector.hpp>
#include <dart/collision/ode/OdeCollisionDetector.hpp>
#include <dart/constraint/ConstraintSolver.hpp>

std::shared_ptr<robot_dart::Robot> random_box(size_t num = 0)
{
    // random pose
    Eigen::Vector6d pose = Eigen::Vector6d::Random();
    // make sure it is above the ground
    pose(5) += 1.5;
    // random size
    Eigen::Vector3d size = Eigen::Vector3d::Random().array() * Eigen::Vector3d(0.1, 0.2, 0.1).array() + 0.3;
    std::cout << "Creating box of size: " << size.transpose() << std::endl;
    return robot_dart::Robot::create_box(size, pose, "free", 1., dart::Color::Red(1.0), "box_" + std::to_string(num));
}

std::shared_ptr<robot_dart::Robot> random_sphere(size_t num = 0)
{
    // random pose
    Eigen::Vector6d pose = Eigen::Vector6d::Random();
    // make sure it is above the ground
    pose(5) += 1.5;
    // random size
    Eigen::Vector3d size = Eigen::Vector3d::Random()[0] * Eigen::Vector3d(0.2, 0.2, 0.2).array() + 0.3;
    std::cout << "Creating sphere of size: " << size.transpose() << std::endl;
    return robot_dart::Robot::create_ellipsoid(size, pose, "free", 1., dart::Color::Blue(1.0), "sphere_" + std::to_string(num));
}

struct StateDesc : public robot_dart::descriptor::BaseDescriptor {
    StateDesc(robot_dart::RobotDARTSimu& simu, size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(simu, desc_dump) {}

    void operator()()
    {
        if (states.size() != _simu.robots().size())
            states.resize(_simu.robots().size());

        for (size_t i = 0; i < _simu.robots().size(); i++) {
            auto robot = _simu.robots()[i];
            int D = robot->skeleton()->getPositions().size();
            Eigen::VectorXd s = Eigen::VectorXd::Zero(3 * D);
            s.head(D) = robot->skeleton()->getPositions();
            s.segment(D, D) = robot->skeleton()->getVelocities();
            s.tail(D) = robot->skeleton()->getAccelerations();
            states[i].push_back(s);
        }

        dts.push_back(_simu.world()->getTime());
    }

    std::vector<std::vector<Eigen::VectorXd>> states;
    std::vector<double> dts;
};

robot_dart::RobotDARTSimu get_world()
{
    std::srand(20);
    // choose time step of 0.001 seconds
    robot_dart::RobotDARTSimu simu(0.001); // by default the DARTCollisionDetector is used
    // simu.world()->getConstraintSolver()->setCollisionDetector(dart::collision::OdeCollisionDetector::create());
    // simu.world()->getConstraintSolver()->setCollisionDetector(dart::collision::BulletCollisionDetector::create());
    // simu.world()->getConstraintSolver()->setCollisionDetector(dart::collision::FCLCollisionDetector::create());
#ifdef GRAPHIC
    simu.set_graphics(std::make_shared<robot_dart::graphics::Graphics>(simu.world()));
    // set the camera at position (0, 3, 1) looking at the center (0, 0, 0)
    std::static_pointer_cast<robot_dart::graphics::Graphics>(simu.graphics())->look_at({0., 3., 1.}, {0., 0., 0.});
#endif

    // add floor of square size of 10 meters and height of 0.2 meters
    simu.add_floor(10., 0.2);

    // add random objects to the world
    for (size_t i = 0; i < 10; i++) {
        simu.add_robot(random_box(i));
        simu.add_robot(random_sphere(i));
    }

    // add a simple arm
    auto arm_robot = std::make_shared<robot_dart::Robot>("res/models/arm.urdf");
    // pin the arm to world
    arm_robot->fix_to_world();
    arm_robot->set_position_enforced(true);

    // add a PD-controller to the arm
    // set desired positions
    std::vector<double> ctrl = {0.0, 1.0, -1.5, 1.0};
    // add the controller to the robot
    arm_robot->add_controller(std::make_shared<robot_dart::control::PDControl>(ctrl));

    // add the arm to the simulator
    simu.add_robot(arm_robot);

    return simu;
}

int main()
{
    robot_dart::RobotDARTSimu simu = get_world();
    simu.add_descriptor(std::make_shared<StateDesc>(simu));

    // run the simulator for 5 seconds
    simu.run(5.);

    auto states = std::static_pointer_cast<StateDesc>(simu.descriptor(0))->states;
    auto dts = std::static_pointer_cast<StateDesc>(simu.descriptor(0))->dts;

    std::cout << "-----------------------------" << std::endl;

    robot_dart::RobotDARTSimu simu2 = get_world();
    simu2.add_descriptor(std::make_shared<StateDesc>(simu2));

    size_t index = 800;

    for (size_t i = 0; i < simu2.robots().size(); i++) {
        auto robot = simu2.robots()[i];
        int D = robot->skeleton()->getPositions().size();

        robot->skeleton()->setPositions(states[i][index].head(D));
        robot->skeleton()->setVelocities(states[i][index].segment(D, D));
        robot->skeleton()->setAccelerations(states[i][index].tail(D));
    }

    simu2.run(5. - dts[index]);

    auto states2 = std::static_pointer_cast<StateDesc>(simu2.descriptor(0))->states;

    for (size_t i = index; i < states[0].size() - 1; i++) {
        std::cout << (i - index) << ": " << std::endl;
        for (size_t j = 0; j < simu2.robots().size(); j++) {
            auto robot = simu2.robots()[j];

            std::cout << "   " << j << ": " << (states[j][i + 1] - states2[j][i - index]).norm() << std::endl;
        }
    }

    return 0;
}

To run the code and reproduce the bug/behavior, do the following (assume you have already installed dart):

• cd /path/to/tmp/dir
• git clone [email protected]:resibots/robot_dart.git
• cd robot_dart
• copy the code above into src/examples/tutorial.cpp (replace whatever is inside)
• ./waf configure (optionally --dart=/path/to/installation/of/dart)
• ./waf
• ./build/tutorial_plain > out.log

So the file out.log should contain differences (after a small header for debugging purposes) in states between the original simulation and the one started from a random point in time (with setting positions, velocities and accelerations). What I observe is that with DARTCollisionDetector I am getting identical simulations, whereas with FCL and Bullet I am getting different ones (especially with FCL). With ODE collision this example is not working as it complains for ellipsoid objects.

So my guess is that we need to save somehow the state of the collision detector. This now makes more sense since I am sure that DARTCollisionDetector has no state; I guess FCL and Bullet have? I hope this helps to clarify things.

[mxgrey]

That's a very interesting outcome. I vaguely remember an issue with FCL where we originally used a map or set of pointers to store the collision objects that would get iterated over, and FCL would produce different results depending on how those objects were ordered because its contact point selection was sensitive to the ordering of objects. Since the objects were stored in a set (or map, I can't remember which) their order was based on their memory addresses, which is going to be a semi-random value.

We started to store them in a std::vector after that, so the objects would be traversed in the order they were added to the collision detector. That should've taken care of that source of non-determinism, but it's possible that there are more sources. This might be a very major thing to try to investigate.

I found some very significant non-deterministic behavior in ODE, as reported here: #1235, but it's plausible that ODE isn't meant to support the way it's being used in that test.