Webots: How to restore robot state in webots?

Question

I want to restore the state(including position and velocity of each solid node in robot).I have tried using supervisor controller API about getting and setting field of the node to set position, and using wb_supervisor_node_get_velocity and wb_supervisor_node_set_velocity to set velocity. But I found the results between using control loop and setting the torque after copying the intermediate state to the robot through above API are different. What should I do to correct this?

Answer 1

There are many ways to reset the state of the simulation, you will find some complete presentation of these various possibilities here: https://cyberbotics.com/doc/guide/using-numerical-optimization-methods#resetting-the-robot

In particular, the first solution looks exactly like what you are trying to do:

// get handles to the robot's translation and rotation fields
WbNodeRef robot_node = wb_supervisor_node_get_from_def("MY_ROBOT");
WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");
WbFieldRef rot_field = wb_supervisor_node_get_field(robot_node, "rotation");

// reset the robot
const double INITIAL_TRANS[3] = { 0, 0.5, 0 };
const double INITIAL_ROT[4] = { 0, 1, 0, 1.5708 };
wb_supervisor_field_set_sf_vec3f(trans_field, INITIAL_TRANS);
wb_supervisor_field_set_sf_rotation(rot_field, INITIAL_ROT);
wb_supervisor_node_reset_physics(robot_node);