Cleans up reset method

albertReacher/envs/albertReacherEnv.py

@@ -1,43 +1,21 @@
 import numpy as np
-import pybullet as p
-from abc import abstractmethod
 
 from albertReacher.resources.albertRobot import AlbertRobot
-from albertReacher.resources.plane import Plane
 from urdfCommon.urdfEnv import UrdfEnv
 
 
 class AlbertReacherEnv(UrdfEnv):
 
     def __init__(self, render=False, dt=0.01):
         super().__init__(AlbertRobot(), render=render, dt=dt)
-        self._n = self.robot.n()
         self.setSpaces()
 
-    @abstractmethod
-    def setSpaces(self):
-        pass
-
-    @abstractmethod
-    def applyAction(self, action):
-        pass
-
-    def reset(self, initialSet=False, pos=None, vel=None):
-        if not isinstance(pos, np.ndarray) or not pos.size == self._n+1:
-            pos = np.zeros(self._n+1)
-        if not isinstance(vel, np.ndarray) or not vel.size == self._n:
-            vel = np.zeros(self._n)
-        if not initialSet:
-            print("Run " + str(self._nSteps) + " steps in this run")
-            self._nSteps = 0
-            p.resetSimulation()
-        p.setPhysicsEngineParameter(
-            fixedTimeStep=self._dt, numSubSteps=self._numSubSteps
-        )
-        self.plane = Plane()
-        self.robot.reset(pos=pos, vel=vel)
-        p.setGravity(0, 0, -10)
-        p.stepSimulation()
-
-        # Get observation to return
-        return self.robot.get_observation()
+    def checkInitialState(self, pos, vel):
+        if not isinstance(pos, np.ndarray) or not pos.size == self.robot.n()+1:
+            pos = np.zeros(self.robot.n()+1)
+            pos[6] = -1.501
+            pos[8] = 1.8675
+            pos[9] = np.pi/4
+        if not isinstance(vel, np.ndarray) or not vel.size == self.robot.n():
+            vel = np.zeros(self.robot.n())
+        return pos, vel

albertReacher/resources/albert.urdf

@@ -660,12 +660,11 @@ in the right frame w.r.t the ground. -->
   </link>
   <joint name="mmrobot_joint4" type="revolute">
     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.50100" soft_upper_limit="1.500996"/>
-    <!-- changed joint init position to be closer to home position-->
-    <origin rpy="1.57079632679 1.57079632679 0" xyz="0.0825 0 0"/>
+    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
     <parent link="mmrobot_link3"/>
     <child link="mmrobot_link4"/>
     <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-1.50100" upper="1.500996" velocity="2.1750"/>
+    <limit effort="87" lower="-3.0718" upper="0.0698" velocity="2.1750"/>
     <!-- for simulation -->
     <dynamics damping="1.0"/>
     <!-- end for simulation -->
@@ -721,12 +720,11 @@ in the right frame w.r.t the ground. -->
   </link>
   <joint name="mmrobot_joint6" type="revolute">
     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.58830" soft_upper_limit="2.1817"/>
-    <!-- changed values to be out of collision at start of simulation-->
-    <origin rpy="1.57079632679 -1.57079632679 0" xyz="0 0 0"/>
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
     <parent link="mmrobot_link5"/>
     <child link="mmrobot_link6"/>
     <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-1.58830" upper="2.1817" velocity="2.6100"/>
+    <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
     <!-- for simulation -->
     <dynamics damping="1.0"/>
     <!-- end for simulation -->

mobileReacher/envs/mobileReacherEnv.py

@@ -1,9 +1,6 @@
-import numpy as np
-import pybullet as p
 from abc import abstractmethod
 
 from mobileReacher.resources.mobileRobot import MobileRobot
-from mobileReacher.resources.plane import Plane
 from urdfCommon.urdfEnv import UrdfEnv
 
 
@@ -22,22 +19,3 @@ def setSpaces(self):
     def applyAction(self, action):
         pass
 
-    def reset(self, initialSet=False, pos=None, vel=None):
-        if not isinstance(pos, np.ndarray) or not pos.size == self._n:
-            pos = np.zeros(self._n)
-        if not isinstance(vel, np.ndarray) or not vel.size == self._n:
-            vel = np.zeros(self._n)
-        if not initialSet:
-            print("Run " + str(self._nSteps) + " steps in this run")
-            self._nSteps = 0
-            p.resetSimulation()
-        p.setPhysicsEngineParameter(
-            fixedTimeStep=self._dt, numSubSteps=self._numSubSteps
-        )
-        self.plane = Plane()
-        self.robot.reset(pos=pos, vel=vel)
-        p.setGravity(0, 0, -10)
-        p.stepSimulation()
-
-        # Get observation to return
-        return self.robot.get_observation()

nLinkUrdfReacher/envs/nLinkUrdfReacherEnv.py

@@ -1,36 +1,9 @@
-import numpy as np
-import pybullet as p
 from nLinkUrdfReacher.resources.nLinkRobot import NLinkRobot
-from nLinkUrdfReacher.resources.plane import Plane
 from urdfCommon.urdfEnv import UrdfEnv
 
 
 class NLinkUrdfReacherEnv(UrdfEnv):
 
     def __init__(self, render=False, dt=0.01, n=3):
         super().__init__(NLinkRobot(n), render=render, dt=dt)
-        self._n = n
         self.setSpaces()
-
-    def n(self):
-        return self._n
-
-    def reset(self, initialSet=False, pos=None, vel=None):
-        if not isinstance(pos, np.ndarray) or not pos.size == self._n:
-            pos = np.zeros(self._n)
-        if not isinstance(vel, np.ndarray) or not vel.size == self._n:
-            vel = np.zeros(self._n)
-        if not initialSet:
-            print("Run " + str(self._nSteps) + " steps in this run")
-            self._nSteps = 0
-            p.resetSimulation()
-        p.setPhysicsEngineParameter(
-            fixedTimeStep=self._dt, numSubSteps=self._numSubSteps
-        )
-        self.plane = Plane()
-        self.robot.reset(pos=pos, vel=vel)
-        p.setGravity(0, 0, -10)
-        p.stepSimulation()
-
-        # Get observation to return
-        return self.robot.get_observation()

pandaReacher/envs/pandaReacherEnv.py

@@ -1,38 +1,20 @@
 import numpy as np
-import pybullet as p
 from pandaReacher.resources.pandaRobot import PandaRobot
-from pandaReacher.resources.plane import Plane
 from urdfCommon.urdfEnv import UrdfEnv
 
 
 class PandaReacherEnv(UrdfEnv):
 
     def __init__(self, render=False, dt=0.01, friction=0.0, gripper=False, n=7):
         super().__init__(PandaRobot(gripper=gripper, friction=friction), render=render, dt=dt)
-        self._n = self.robot.n()
         self.setSpaces()
 
-    def n(self):
-        return self._n
-
-    def reset(self, initialSet=False, pos=None, vel=None):
-        if not isinstance(pos, np.ndarray) or not pos.size == self._n:
-            pos = np.zeros(self._n)
+    def checkInitialState(self, pos, vel):
+        if not isinstance(pos, np.ndarray) or not pos.size == self.robot.n():
+            pos = np.zeros(self.robot.n())
             pos[3] = -1.501
             pos[5] = 1.8675
-        if not isinstance(vel, np.ndarray) or not vel.size == self._n:
-            vel = np.zeros(self._n)
-        if not initialSet:
-            print("Run " + str(self._nSteps) + " steps in this run")
-            self._nSteps = 0
-            p.resetSimulation()
-        p.setPhysicsEngineParameter(
-            fixedTimeStep=self._dt, numSubSteps=self._numSubSteps
-        )
-        self.plane = Plane()
-        self.robot.reset(pos=pos, vel=vel)
-        p.setGravity(0, 0, -10)
-        p.stepSimulation()
-
-        # Get observation to return
-        return self.robot.get_observation()
+            pos[6] = np.pi/4
+        if not isinstance(vel, np.ndarray) or not vel.size == self.robot.n():
+            vel = np.zeros(self.robot.n())
+        return pos, vel

pointRobotUrdf/envs/pointRobotEnv.py

@@ -1,40 +1,10 @@
-import numpy as np
-import pybullet as p
-from abc import abstractmethod
-
 from pointRobotUrdf.resources.pointRobot import PointRobot
-from pointRobotUrdf.resources.plane import Plane
 from urdfCommon.urdfEnv import UrdfEnv
 
 
 class PointRobotEnv(UrdfEnv):
-    metadata = {"render.modes": ["human"]}
 
     def __init__(self, render=False, dt=0.01):
         super().__init__(PointRobot(), render=render, dt=dt)
         self.setSpaces()
         self.reset(initialSet=True)
-
-    @abstractmethod
-    def setSpaces(self):
-        pass
-
-    @abstractmethod
-    def applyAction(self, action):
-        pass
-
-    def reset(self, initialSet=False, pos=np.zeros(2), vel=np.zeros(2)):
-        if not initialSet:
-            print("Run " + str(self._nSteps) + " steps in this run")
-            self._nSteps = 0
-            p.resetSimulation()
-        p.setPhysicsEngineParameter(
-            fixedTimeStep=self._dt, numSubSteps=self._numSubSteps
-        )
-        self.plane = Plane()
-        self.robot.reset(pos=pos, vel=vel)
-        p.setGravity(0, 0, -10)
-        p.stepSimulation()
-
-        # Get observation to return
-        return self.robot.get_observation()

tiagoReacher/envs/tiagoReacherEnv.py

@@ -1,46 +1,18 @@
 import numpy as np
-import pybullet as p
-from abc import abstractmethod
 
 from tiagoReacher.resources.tiagoRobot import TiagoRobot
-from albertReacher.resources.plane import Plane
 from urdfCommon.urdfEnv import UrdfEnv
 
 
 class TiagoReacherEnv(UrdfEnv):
 
     def __init__(self, render=False, dt=0.01, n=19):
         super().__init__(TiagoRobot(), render=render, dt=dt)
-        self._n = self.robot.n()
         self.setSpaces()
 
-    def n(self):
-        return self._n
-
-    @abstractmethod
-    def setSpaces(self):
-        pass
-
-    @abstractmethod
-    def applyAction(self, action):
-        pass
-
-    def reset(self, initialSet=False, pos=None, vel=None):
-        if not isinstance(pos, np.ndarray) or not pos.size == self._n + 1:
-            pos = np.zeros(self._n+1)
-        if not isinstance(vel, np.ndarray) or not vel.size == self._n:
-            vel = np.zeros(self._n)
-        if not initialSet:
-            print("Run " + str(self._nSteps) + " steps in this run")
-            self._nSteps = 0
-            p.resetSimulation()
-        p.setPhysicsEngineParameter(
-            fixedTimeStep=self._dt, numSubSteps=self._numSubSteps
-        )
-        self.plane = Plane()
-        self.robot.reset(pos=pos, vel=vel)
-        p.setGravity(0, 0, -10)
-        p.stepSimulation()
-
-        # Get observation to return
-        return self.robot.get_observation()
+    def checkInitialState(self, pos, vel):
+        if not isinstance(pos, np.ndarray) or not pos.size == self.robot.n() + 1:
+            pos = np.zeros(self.robot.n()+1)
+        if not isinstance(vel, np.ndarray) or not vel.size == self.robot.n():
+            vel = np.zeros(self.robot.n())
+        return pos, vel