Adding delay to robot controller in webots

Question

I'm doing a pick and place task using UR3 in webots. I need the robot to go to Pick position, grasp the object (close its fingers), and go to Place position. Something like following code:

  while (wb_robot_step(TIME_STEP) != -1) {
   // 1. Go to Pick position
   for (i = 0; i < 6; ++i)
     wb_motor_set_position(ur_motors[i], pick_position[i]);                  

   // 2. Grasp the object
   for (i = 0; i < 2; ++i)
      wb_motor_set_position(finger_motors[i], 0.5);
        
   // 3. Go to Place position
   for (i = 0; i < 6; ++i)
     wb_motor_set_position(ur_motors[i], place_position[i]);                     
} 

However, when I run the code, I see that while robot is going to Pick position, fingers also get closed. I read in documentations that "the wb_motor_set_position function stores the new position, but it does not immediately actuate the motor". So I want to know how I can add a delay between step 1 and 2, i.e., fingers get closed only after motor actuation reached the desired position. I used sleep but didn't help.

Answer 1

There are different possibilities to achieve what you want. A simple one would be to introduce a time counter variable (you may need to adjust the 100 and 200 values depending on your TIME_STEP):

  int counter = 0;
  while (wb_robot_step(TIME_STEP) != -1) {
   // 1. Go to Pick position
   if (counter == 0)
     for (i = 0; i < 6; ++i)
       wb_motor_set_position(ur_motors[i], pick_position[i]);                  

   // 2. Grasp the object
   else if (counter == 100)
     for (i = 0; i < 2; ++i)
        wb_motor_set_position(finger_motors[i], 0.5);
        
   // 3. Go to Place position
   else if (counter == 200)
     for (i = 0; i < 6; ++i)
       wb_motor_set_position(ur_motors[i], place_position[i]);                     
   counter++;
}

Another possibility is to use wb_robot_get_time() to retrieve the current simulation time (instead of the counter variable) and to perform some action only when a certain time is reached.

Finally, another possibility would be to use the position feedback to step to the next motion (closing the gripper) only when the target position is reached (and not after some fixed time delay).