OpenRAVE "fast-forward" physics simulation?

Question

I am working with OpenRAVE simulator and I am using ODE Physics (OpenRAVE plugin to support ODE).

I have a robot in an environment and multiple other objects that are subject to the physics engine. I would like to send velocities to the robot for the duration of 1 second (in real time). I am able to achieve something like this with the following logic:

penv->StartSimulation(0.001);
while(true) {    
    {
        EnvironmentMutex::scoped_lock lock(penv->GetMutex());
        robot->SetVelocity(OpenRAVE::Vector(0.1, 0, 0), OpenRAVE::Vector(0, 0, 0));
    }
}
penv->StopSimulation();

Note that although not included in the code above, I keep a timer since the start of the simulation and I am constantly checking it until I reach 1 second of difference, in which case I break out of the while loop.

Now, what I really want to do, is to simulate the real time into simulation time. So the same result to happen in faster time (i.e fast-forward) the result of applying the robot velocities for 1 real time second but let's say in 10, 100ms or any other time less than a second. I am basically looking on how to get the final result of applying the velocities (including how the environment will react to the robot-to-object interactions using the physics engine).

I have tried few things but nothing worked. Here is the closest I can get:

penv->StartSimulation(0.001, false);
double sim = 0.000001 * penv->GetSimulationTime();
while(true) {
    if (0.000001*penv->GetSimulationTime() > sim+1)
    {
        break;
    }

    {
        EnvironmentMutex::scoped_lock lock(penv->GetMutex());
        robot->SetVelocity(OpenRAVE::Vector(0.1, 0, 0), OpenRAVE::Vector(0, 0, 0));
    }
}
penv->StopSimulation();

The false at the StartSimulation indicates that the simulation does not happen in real time and the SimulationStep is called as fast as possible.

The problem I am getting is that the robot (and probably the physics) are not keeping the robot at the right position. When I execute the above solution by the end of it, the robot is fall down from the ground for some reason. I have also tried calling penv->StepSimulation(0.001); but this did nothing more.

Any ideas?

OpenRAVE "fast-forward" physics simulation?

0 Answers0