Fix: more precise grasp in grasp_demo.py

Author: pkrack

examples/fr3/grasp_demo.py

@@ -61,10 +61,11 @@ def approach(self, geom_name: str):
 
     def grasp(self, geom_name: str):
 
-        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.01, num_waypoints=60)
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=-0.005, num_waypoints=60)
         self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
 
-        self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED))
+        for _ in range(4):
+            self.step(self._action(self.unwrapped.robot.get_cartesian_position(), GripperWrapper.BINARY_GRIPPER_CLOSED))
 
         waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
         self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)

python/rcs/envs/sim.py

@@ -104,10 +104,12 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo
         return obs, 0.0, False, bool(truncated), info
 
     def reset(
-        self, seed: dict[str, int | None] | None = None, options: dict[str, Any] | None = None  # type: ignore
+        self,
+        seed: dict[str, int | None] | None = None,
+        options: dict[str, Any] | None = None,  # type: ignore
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         if seed is None:
-            seed = {key: None for key in self.env.envs}
+            seed = dict.fromkeys(self.env.envs)
         if options is None:
             options = {key: {} for key in self.env.envs}
         obs = {}
@@ -196,7 +198,6 @@ def __init__(
             self.sim.open_gui()
 
     def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict[str, Any]]:
-
         self.collision_env.get_wrapper_attr("robot").set_joints_hard(self.unwrapped.robot.get_joint_position())
         _, _, _, _, info = self.collision_env.step(action)
 
@@ -219,7 +220,7 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, b
         return obs, reward, done, truncated, info
 
     def reset(
-        self, seed: int | None = None, options: dict[str, Any] | None = None
+        self, *, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         # check if move to home is collision free
         if self.check_home_collision:
@@ -333,7 +334,7 @@ def __init__(
         self.y_offset = y_offset
 
     def reset(
-        self, seed: int | None = None, options: dict[str, Any] | None = None
+        self, *, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         if options is not None and "RandomObjectPos.init_object_pose" in options:
             assert isinstance(
@@ -381,7 +382,7 @@ def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = F
         self.cube_joint_name = cube_joint_name
 
     def reset(
-        self, seed: int | None = None, options: dict[str, Any] | None = None
+        self, *, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         obs, info = super().reset(seed=seed, options=options)
         self.sim.step(1)
@@ -456,11 +457,7 @@ def step(self, action: dict[str, Any]):
         reward /= 3  # type: ignore
         return obs, reward, success, truncated, info
 
-    def reset(
-        self,
-        seed: dict[str, int | None] | None = None,
-        options: dict[str, Any] | None = None,  # type: ignore
-    ):
+    def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None):
         obs, info = super().reset()
         self.home_pose = self.unwrapped.robot.get_cartesian_position()
         return obs, info