adaptable observatons spaces

dfki-ric · May 27, 2024 · 521ab2b · 521ab2b
1 parent 2ba7315
commit 521ab2b
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 14 deletions.
diff --git a/deformable_gym/envs/floating_mia_grasp_env.py b/deformable_gym/envs/floating_mia_grasp_env.py
@@ -157,7 +157,7 @@ def _get_observation(self):
         ee_pose = self.robot.get_ee_pose()
         sensor_readings = self.robot.get_sensor_readings()
 
-        state = np.concatenate([ee_pose, joint_pos, sensor_readings])
+        state = np.concatenate([ee_pose, joint_pos, sensor_readings], axis=0)
 
         if self._observable_object_pos:
             obj_pos = self.object_to_grasp.get_pose()[:3]

diff --git a/deformable_gym/envs/floating_shadow_grasp_env.py b/deformable_gym/envs/floating_shadow_grasp_env.py
@@ -16,7 +16,7 @@ class FloatingShadowGraspEnv(GraspEnv):
 
     **Action space:**
     - End-effector pose: (x, y, z, qx, qy, qz, qw)
-    - Finger joint angle: 3 values
+    - Finger joint angle: 24 values
 
     Parameters
     ----------
@@ -37,10 +37,11 @@ def __init__(
             **kwargs):
         self.velocity_commands = False
 
-        super().__init__(object_name=object_name,
-                         object_scale=object_scale,
-                         observable_object_pos=observable_object_pos,
-                         **kwargs)
+        super().__init__(
+            object_name=object_name,
+            object_scale=object_scale,
+            observable_object_pos=observable_object_pos,
+            **kwargs)
 
         self.hand_world_pose = self.INITIAL_POSE
         self.robot = self._create_robot()
@@ -71,10 +72,10 @@ def __init__(
             dtype=np.float64)
 
         lower_actions = np.concatenate([
-            -np.ones(7)*0.01, limits[0], [0]], axis=0)
+            -np.ones(7)*0.01, limits[0]], axis=0)
 
         upper_actions = np.concatenate([
-            np.ones(7)*0.01, limits[1], [1]], axis=0)
+            np.ones(7)*0.01, limits[1]], axis=0)
 
         self.action_space = spaces.Box(
             low=lower_actions, high=upper_actions, dtype=np.float64)
@@ -108,4 +109,12 @@ def _create_robot(self):
 
     def _get_observation(self):
         finger_pos = self.robot.get_joint_positions()[6:]
-        return np.concatenate([finger_pos], axis=0)
+        ee_pose = self.robot.get_ee_pose()
+
+        state = np.concatenate([ee_pose, finger_pos], axis=0)
+
+        if self._observable_object_pos:
+            obj_pos = self.object_to_grasp.get_pose()[:3]
+            state = np.append(state, obj_pos)
+
+        return state
diff --git a/deformable_gym/envs/ur10_shadow_grasp_env.py b/deformable_gym/envs/ur10_shadow_grasp_env.py
@@ -13,11 +13,11 @@ class UR10ShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv):
 
     **State space:**
     - End-effector pose: (x, y, z, qx, qy, qz, qw)
-    - Finger joint angles: 6 values TODO order
+    - Finger joint angles: 24 values TODO order
 
     **Action space:**
     - End-effector pose offset: (x, y, z, qx, qy, qz, qw)
-    - Finger joint angle offsets: 3 values
+    - Finger joint angle offsets: 24 values
 
     Parameters
     ----------
@@ -35,12 +35,14 @@ def __init__(
             self,
             object_name="insole",
             object_scale=1.0,
+            observable_object_pos: bool = False,
             **kwargs
     ):
         self.insole = None
         self.velocity_commands = False
         self.object_name = object_name
         self.object_scale = object_scale
+        self._observable_object_pos = observable_object_pos
 
         super().__init__(
             soft=True,
@@ -59,6 +61,12 @@ def __init__(
             np.array([2, 2, 2]), np.ones(4), limits[1][6:],
             np.array([5, 5, 5])], axis=0)
 
+        if self._observable_object_pos:
+            lower_observations = np.append(
+                lower_observations, -np.full(3, 2.))
+            upper_observations = np.append(
+                upper_observations, np.full(3, 2.))
+
         self.observation_space = spaces.Box(
             low=lower_observations, high=upper_observations)
 
@@ -114,10 +122,15 @@ def reset(self, seed=None, options=None):
 
     def _get_observation(self):
         finger_pos = self.robot.get_joint_positions()[6:]
-        ee_pose = self.robot.get_ee_pose()
-        sensor_readings = self.robot.get_sensor_readings()
+        #ee_pose = self.robot.get_ee_pose()
+
+        state = np.concatenate([finger_pos], axis=0)
+
+        if self._observable_object_pos:
+            obj_pos = self.object_to_grasp.get_pose()[:3]
+            state = np.append(state, obj_pos)
 
-        return np.concatenate([ee_pose, finger_pos, sensor_readings], axis=0)
+        return state
 
     def calculate_reward(self, state, action, next_state, terminated):
         """