-
Notifications
You must be signed in to change notification settings - Fork 287
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
State class and scene recording #386
Comments
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:
Skeleton::StateThe Skeleton State will be split into three subclasses: (1) (2) (3)
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::StateI'm thinking there should be two forms of the BodyNode::StateI'm thinking the BodyNode state can be constructed like this:
Then the Depending on how the ShapeNode is implemented, it seems likely that we will want some kind of World::StateIdeally I would very very much like for the World::State to be nothing more than a |
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 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 Skeleton::ConfigurationI imagine the configuration class would simply look like this:
I don't know whether to include joint accelerations, forces, or commands in the configuration. Opinions on this would be greatly appreciated. Skeleton::StateThe We would no longer have
Then If anyone has opinions on this proposal, please don't hesitate to share. Edit: Added |
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 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
|
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 I think the most sensible approach we could take would be to make the |
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 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. |
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:
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. |
Right, in theory the set of all If that doesn't work, then you may be encountering an issue related to #1089 / #1086. Let us know if |
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?
I will try it out (without saving to file) to check and let you know. |
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,
While I think 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 |
OK. Thanks a lot! I will try saving all the forces as well and see what I get..
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 #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 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 |
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 |
I think the key thing would be to add something like After that we would probably want the functions to be implemented somehow by 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 |
In all my use cases, the 2nd option (i.e., programmatically constructing the world, or defining the world in something like a
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 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 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):
So the file 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 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.
The text was updated successfully, but these errors were encountered: