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/base.py

@@ -255,12 +255,9 @@ def get_obs(self) -> ArmObsType:
     def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bool, dict]:
         action_dict = cast(dict, action)
         if (
-            self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat
-            and self.tquat_key not in action_dict
-            or self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY
-            and self.trpy_key not in action_dict
-            or self.get_base_control_mode() == ControlMode.JOINTS
-            and self.joints_key not in action_dict
+            (self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat and self.tquat_key not in action_dict)
+            or (self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY and self.trpy_key not in action_dict)
+            or (self.get_base_control_mode() == ControlMode.JOINTS and self.joints_key not in action_dict)
         ):
             msg = "Given type is not matching control mode!"
             raise RuntimeError(msg)
@@ -336,8 +333,8 @@ def reset(
         obs = {}
         info = {}
 
-        seed_ = seed if seed is not None else {key: None for key in self.envs}  # type: ignore
-        options_ = options if options is not None else {key: None for key in self.envs}  # type: ignore
+        seed_ = seed if seed is not None else dict.fromkeys(self.envs)  # type: ignore
+        options_ = options if options is not None else dict.fromkeys(self.envs)  # type: ignore
         for key, env in self.envs.items():
             obs[key], info[key] = env.reset(seed=seed_[key], options=options_[key])
         return obs, info
@@ -459,8 +456,10 @@ def set_origin_to_current(self):
         else:
             self._origin = self.unwrapped.robot.get_cartesian_position()
 
-    def reset(self, **kwargs) -> tuple[dict, dict[str, Any]]:
-        obs, info = super().reset(**kwargs)
+    def reset(
+        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.initial_obs = obs
         self.set_origin_to_current()
         self._last_action = None
@@ -629,7 +628,9 @@ def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False):
         )
         self.camera_key = get_space_keys(CameraDictType)[0]
 
-    def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[dict, dict[str, Any]]:
+    def reset(
+        self, *, seed: int | None = None, options: dict[str, Any] | None = None
+    ) -> tuple[dict[str, Any], dict[str, Any]]:
         self.camera_set.clear_buffer()
         return super().reset(seed=seed, options=options)
 

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