Open mxgrey opened 9 years ago
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.
The Skeleton State will be split into three subclasses:
(1) Skeleton::JointState
contains a list of Eigen::VectorXd
s 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::vector
s which contain:
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.
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::VectorXd
s 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.
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.
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.
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::VectorXd
s. So here is what I would propose instead:
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.
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
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.
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 Addon
s and/or Node
s 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).
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.
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.
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.
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?
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.
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:
Skeleton
s in my world (thus I do not care about serializing the hard parts, because I always load exactly the same scene)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.
What are the skeleton variables that I have to keep in order to produce identical simulations?
Right, in theory the set of all Skeleton::State
s 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.
in theory the set of all
Skeleton::State
s 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.
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.
Oh, and the way to get the Skeleton state is Skeleton::getState()
.
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) whileProperties
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
andProperties
are managed by thedart::common::Aspect
class. Hypothetically it should be straightforward to add serialization toAspect::State
andAspect::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
andProperties
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 inState
accidentally landed inProperties
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.
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?
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.
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!
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).
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.
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.
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?
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.
I am coming back at this one for some questions (before start coding the serializer):
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..
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?
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 git@github.com:resibots/robot_dart.git
cd robot_dart
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.
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.
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.