Merge pull request #40 from dfki-ric/fix/hand-on-arms-envs

Fix UR+hand envs

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]

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

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
     ----------
@@ -33,33 +33,39 @@ class UR10ShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv):
 
     def __init__(
             self,
-            gui=True,
-            real_time=False,
             object_name="insole",
-            verbose=False,
-            horizon=100,
             object_scale=1.0,
-            verbose_dt=10.0
+            observable_object_pos: bool = False,
+            **kwargs
     ):
         self.insole = None
         self.velocity_commands = False
         self.object_name = object_name
         self.object_scale = object_scale
-        super().__init__(gui=gui, real_time=real_time, horizon=horizon,
-                         soft=True, verbose=verbose,
-                         verbose_dt=verbose_dt)
+        self._observable_object_pos = observable_object_pos
+
+        super().__init__(
+            soft=True,
+            **kwargs
+        )
 
         self.robot = self._create_robot()
 
         limits = pbh.get_limit_array(self.robot.motors.values())
 
         lower_observations = np.concatenate([
             np.array([-2, -2, 0]), -np.ones(4), limits[0][6:],
-            np.array([-5, -5, -5])], axis=0)
+            ], axis=0)
 
         upper_observations = np.concatenate([
             np.array([2, 2, 2]), np.ones(4), limits[1][6:],
-            np.array([5, 5, 5])], axis=0)
+            ], 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)
@@ -115,11 +121,16 @@ def reset(self, seed=None, options=None):
         return super().reset(seed, options)
 
     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()
+        joint_pos = self.robot.get_joint_positions()
+        #ee_pose = self.robot.get_ee_pose()
+
+        state = np.concatenate([joint_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):
         """

deformable_gym/envs/ur5_mia_grasp_env.py

@@ -45,16 +45,14 @@ class UR5MiaGraspEnv(GraspDeformableMixin, BaseBulletEnv):
     - (5) normal force at middle finger.
 
     **Action space:**
-    The action space has 11 dimensions.
+    The action space has 10 dimensions.
     End-effector pose (7 values) and joint angles of the hand (3 values).
     End-effector commands are defined by position (x, y, z) and scalar-last
     quaternion (qx, qy, qz, qw).
     Joint commands are ordered as follows:
     - (0) j_index_fle
     - (1) j_mrl_fle
     - (3) j_thumb_fle
-    At the end there is a flag to indicate that the episode is finished (1)
-    or not (0).
 
     Parameters
     ----------
@@ -84,31 +82,26 @@ class UR5MiaGraspEnv(GraspDeformableMixin, BaseBulletEnv):
 
     def __init__(
             self,
-            gui: bool = True,
-            real_time: bool = False,
             object_name: str = "insole",
-            verbose: bool = False,
-            horizon: int = 100,
-            train: bool = True,
             thumb_adducted: bool = True,
-            compute_reward: bool = True,
             object_scale: float = 1.0,
-            verbose_dt: float = 10.0,
-            pybullet_options: str = ""):
-        self.train = train
+            observable_object_pos: bool = False,
+            **kwargs
+    ):
+
         self.velocity_commands = False
         self.object_name = object_name
         self.randomised = False
         self.thumb_adducted = thumb_adducted
-        self.compute_reward = compute_reward
         self.object_scale = object_scale
 
-        super().__init__(gui=gui, real_time=real_time, horizon=horizon,
-                         soft=True, verbose=verbose,
-                         verbose_dt=verbose_dt,
-                         pybullet_options=pybullet_options)
+        super().__init__(
+            soft=True,
+            **kwargs
+        )
 
         self.robot = self._create_robot()
+        self._observable_object_pos = observable_object_pos
 
         limits = pbh.get_limit_array(self.robot.motors.values())
 
@@ -120,6 +113,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)
 
@@ -159,7 +158,6 @@ def _create_robot(self):
         self.simulation.add_robot(robot)
 
         robot.set_initial_joint_positions(INITIAL_JOINT_ANGLES)
-
         robot.set_thumb_opp(self.thumb_adducted)
 
         return robot
@@ -174,10 +172,8 @@ def _load_objects(self):
             scale=self.object_scale)
 
     def reset(self, seed=None, options=None):
-
         self.object_to_grasp.reset()
         self.robot.activate_motors()
-
         return super().reset(seed, options)
 
     def _get_observation(self):
@@ -186,7 +182,13 @@ def _get_observation(self):
         ee_pose = self.robot.get_ee_pose()
         sensor_readings = self.robot.get_sensor_readings()
 
-        return np.concatenate([ee_pose, joint_pos, sensor_readings], axis=0)
+        obs = np.concatenate([ee_pose, joint_pos, sensor_readings], axis=0)
+
+        if self._observable_object_pos:
+            obj_pos = self.object_to_grasp.get_pose()[:3]
+            obs = np.append(obs, obj_pos)
+
+        return obs
 
     def calculate_reward(self, state, action, next_state, terminated):
         """

deformable_gym/objects/bullet_object.py

@@ -1,6 +1,5 @@
 import abc
 import os
-import sys
 import warnings
 from pathlib import Path
 from typing import List, Sequence, Tuple, Union
@@ -11,8 +10,8 @@
 import pytransform3d.rotations as pr
 import pytransform3d.transformations as pt
 
-from deformable_gym.helpers import pybullet_helper as pbh
-from deformable_gym.robots.bullet_utils import draw_pose
+from ..helpers import pybullet_helper as pbh
+from ..robots.bullet_utils import draw_pose
 
 from pybullet_utils import bullet_client as bc
 

deformable_gym/objects/conveyor.py

@@ -1,4 +1,4 @@
-from deformable_gym.objects.bullet_object import BoxObject, ObjectFactory
+from ..objects.bullet_object import BoxObject, ObjectFactory
 from pybullet_utils import bullet_client as bc
 
 

deformable_gym/robots/bullet_robot.py

@@ -7,10 +7,10 @@
 from pybullet_utils import bullet_client as bc
 from gymnasium.spaces import Box
 
-from deformable_gym.helpers import pybullet_helper as pbh
-from deformable_gym.objects.bullet_object import Pose
-from deformable_gym.robots.bullet_utils import draw_limits
-from deformable_gym.robots.inverse_kinematics import PyBulletSolver
+from ..helpers import pybullet_helper as pbh
+from ..objects.bullet_object import Pose
+from ..robots.bullet_utils import draw_limits
+from ..robots.inverse_kinematics import PyBulletSolver
 
 
 class BulletRobot(abc.ABC):
@@ -53,7 +53,7 @@ def __init__(
         self.pb_client = pb_client
 
         if self.task_space_limit is not None and verbose:
-            draw_limits(self.task_space_limit)
+            draw_limits(self.task_space_limit, self.pb_client)
 
         if world_pos is None:
             self.init_pos = [0, 0, 0]
@@ -129,7 +129,9 @@ def __init__(
             print(self.all_joints)
 
         self.multibody_pose = pbh.MultibodyPose(
-            self.get_id(), self.init_pos, self.init_rot,
+            self.get_id(),
+            self.init_pos,
+            self.init_rot,
             pb_client=self.pb_client)
 
         # create observation and action spaces
@@ -187,7 +189,7 @@ def set_endeffector(self, link_id: int) -> None:
         self.end_effector = link_id
         if self.inverse_kinematics_solver is None:  # default to PyBullet IK
             self.inverse_kinematics_solver = PyBulletSolver(
-                self._id, self.end_effector)
+                self._id, self.end_effector, self.pb_client)
 
     def get_joint_positions(
             self, keys: Union[Iterable[str], None] = None) -> np.ndarray:
@@ -341,7 +343,7 @@ def get_ee_pose(self) -> np.ndarray:
         """
         if self.end_effector is not None:
             current_pos, current_orn = pbh.link_pose(
-                self._id, self.end_effector)
+                self._id, self.end_effector, self.pb_client)
         else:
             current_pos, current_orn = (
                 self.pb_client.getBasePositionAndOrientation(self._id))

deformable_gym/robots/bullet_utils.py

@@ -1,5 +1,4 @@
 import numpy as np
-import pybullet as pb
 import pytransform3d.rotations as pr
 import pytransform3d.transformations as pt
 

deformable_gym/robots/inverse_kinematics.py

@@ -1,21 +1,23 @@
 import numpy as np
-import pybullet as pb
 
-from deformable_gym.robots.ur_kinematics import (analytical_ik,
-                                                 ee_kin2ee_options,
-                                                 robot_params,
-                                                 urdf_base2kin_base)
+from ..robots.ur_kinematics import (analytical_ik,
+                                    ee_kin2ee_options,
+                                    robot_params,
+                                    urdf_base2kin_base)
+from pybullet_utils import bullet_client as bc
 
 
 class PyBulletSolver:
     def __init__(
             self,
             robot,
             end_effector,
+            pb_client: bc.BulletClient,
             n_iter: int = 200,
             threshold: float = 1e-6,
     ):
         self.robot = robot
+        self.pb_client = pb_client
         self.end_effector = end_effector
         self.n_iter = n_iter
         self.threshold = threshold
@@ -26,14 +28,14 @@ def __call__(self, target_pos, target_orn, last_joint_angles):
         if last_joint_angles is None:  # HACK
             last_joint_angles = np.zeros(6)
 
-        out = np.array(pb.calculateInverseKinematics(
-            self.robot,
-            self.end_effector,
-            targetPosition=target_pos,
-            targetOrientation=target_orn,
-            maxNumIterations=self.n_iter,
-            residualThreshold=self.threshold,
-            physicsClientId=self.robot.physics_client_id))
+        out = np.array(
+            self.pb_client.calculateInverseKinematics(
+                self.robot,
+                self.end_effector,
+                targetPosition=target_pos,
+                targetOrientation=target_orn,
+                maxNumIterations=self.n_iter,
+                residualThreshold=self.threshold))
         return out[:6]