Add 'set_qpos', 'set_dofs_velocity' to accessor benchmark.

Author: duburcqa

tests/test_rigid_benchmarks.py

@@ -422,12 +422,23 @@ def _batched_franka(solver, n_envs, gjk, is_collision_free, accessors):
     if is_collision_free:
         franka.control_dofs_position(ctrl)
 
+    vel = torch.zeros((franka.n_qs,), dtype=gs.tc_float, device=gs.device)
+    if n_envs > 0:
+        n_reset_envs = int(0.02 * n_envs)
+        reset_envs_idx = torch.randperm(n_envs)[:n_reset_envs]
+        vel = torch.tile(vel, (n_reset_envs, 1))
+    else:
+        reset_envs_idx = None
+    qpos = franka.get_qpos(envs_idx=reset_envs_idx)
+
     num_steps = 0
     is_recording = False
     time_start = time.time()
     while True:
         scene.step()
         if accessors:
+            franka.set_qpos(qpos, envs_idx=reset_envs_idx, zero_velocity=False, skip_forward=True)
+            franka.set_dofs_velocity(vel, envs_idx=reset_envs_idx, skip_forward=True)
             franka.get_ang()
             franka.get_vel()
             franka.get_dofs_position()