Repository: WPI-MMR/gym_solo

Issue:
Integrate Reward Factory with Solo Env
It seems as if the Solo Env is still sending out a hard coded reward--this needs to be switched to the dynamic model asap: 

https://github.com/WPI-MMR/gym_solo/blob/88d9d645d1f27bd2754adfbcdd6c794daf818d08/gym_solo/envs/solo8v2vanilla.py#L77

^^ Here the 0.0 needs to be dynamically computed for the reward.

Discussion:

Patch:
diff --git a/gym_solo/core/rewards.py b/gym_solo/core/rewards.py
index d364f98..35284e6 100644
--- a/gym_solo/core/rewards.py
+++ b/gym_solo/core/rewards.py
@@ -55,9 +55,15 @@ class RewardFactory:
   def get_reward(self) -> float:
     """Evaluate the current state and get the combined reward.
 
+    Exceptions:
+      ValueError: If get_reward() is called with no registered rewards.
+
     Returns:
       float: The reward from the current state. Note that this reward is a 
       combination of multiple atomic sub-rewards, as explained by the 
       strategies earlier.
     """
+    if not self._rewards:
+      raise ValueError('Need to register at least one reward instance')
+
     return sum(wr.weight * wr.reward.compute() for wr in self._rewards)
\ No newline at end of file
diff --git a/gym_solo/envs/solo8v2vanilla.py b/gym_solo/envs/solo8v2vanilla.py
index bc0d4f6..4521e96 100644
--- a/gym_solo/envs/solo8v2vanilla.py
+++ b/gym_solo/envs/solo8v2vanilla.py
@@ -13,6 +13,8 @@ from gym import error, spaces
 
 from gym_solo.core.configs import Solo8BaseConfig
 from gym_solo.core import obs
+from gym_solo.core import rewards
+
 from gym_solo import solo_types
 
 
@@ -33,6 +35,7 @@ class Solo8VanillaEnv(gym.Env):
     self._config = config
 
     self.obs_factory = obs.ObservationFactory()
+    self.reward_factory = rewards.RewardFactory()
 
     self._client = p.connect(p.GUI if use_gui else p.DIRECT)
     p.setAdditionalSearchPath(pbd.getDataPath())
@@ -72,9 +75,10 @@ class Solo8VanillaEnv(gym.Env):
     if self._realtime:
       time.sleep(self._config.dt)
 
-    # TODO: Fix rewards
     obs_values, obs_labels = self.obs_factory.get_obs()
-    return obs_values, 0.0, False, {'labels': obs_labels}
+    reward = self.reward_factory.get_reward()
+
+    return obs_values, reward, False, {'labels': obs_labels}
 
   def reset(self) -> solo_types.obs:
     """Reset the state of the environment and returns an initial observation.
@@ -85,9 +89,14 @@ class Solo8VanillaEnv(gym.Env):
     p.removeBody(self.robot)
     self.robot, _ = self._load_robot()
 
-    # Let gravity do it's thing and reset the environment
+    # Let gravity do it's thing and reset the environment deterministically
     for i in range(1000):
-      self.step(self._zero_gains)
+      p.setJointMotorControlArray(self.robot, 
+                                  np.arange(self.action_space.shape[0]),
+                                  p.TORQUE_CONTROL, forces=self._zero_gains,
+                                  positionGains=self._zero_gains, 
+                                  velocityGains=self._zero_gains)
+      p.stepSimulation()
     
     obs_values, _ = self.obs_factory.get_obs()
     return obs_values


Test patch:
diff --git a/gym_solo/core/test_rewards_factory.py b/gym_solo/core/test_rewards_factory.py
index fd70bea..c29bc5a 100644
--- a/gym_solo/core/test_rewards_factory.py
+++ b/gym_solo/core/test_rewards_factory.py
@@ -17,6 +17,9 @@ class TestRewardsFactory(unittest.TestCase):
     rf = rewards.RewardFactory()
     self.assertListEqual(rf._rewards, [])
 
+    with self.assertRaises(ValueError):
+      rf.get_reward()
+
   @parameterized.expand([
     ('single', {1: 2.5}, 2.5),
     ('two_happy', {1: 1, 2: 2}, 5),
diff --git a/gym_solo/envs/test_solo8v2vanilla.py b/gym_solo/envs/test_solo8v2vanilla.py
index 5219151..59a4044 100644
--- a/gym_solo/envs/test_solo8v2vanilla.py
+++ b/gym_solo/envs/test_solo8v2vanilla.py
@@ -1,6 +1,9 @@
 import unittest
 from gym_solo.envs import solo8v2vanilla as solo_env
 
+from gym_solo.core.test_obs_factory import CompliantObs
+from gym_solo.core import rewards
+
 from gym import error, spaces
 from parameterized import parameterized
 from unittest import mock
@@ -11,9 +14,15 @@ import os
 import pybullet as p
 
 
+class SimpleReward(rewards.Reward):
+  def compute(self):
+    return 1
+
+
 class TestSolo8v2VanillaEnv(unittest.TestCase):
   def setUp(self):
     self.env = solo_env.Solo8VanillaEnv(config=solo_env.Solo8VanillaConfig())
+    self.env.reward_factory.register_reward(1, SimpleReward())
 
   def tearDown(self):
     self.env._close()
@@ -29,6 +38,9 @@ class TestSolo8v2VanillaEnv(unittest.TestCase):
   def test_realtime(self, mock_time):
     env = solo_env.Solo8VanillaEnv(config=solo_env.Solo8VanillaConfig(),
                                    realtime=True)
+    env.reward_factory.register_reward(1, SimpleReward())
+
+    env.step(env.action_space.sample())
     self.assertTrue(mock_time.called)
     
   def test_seed(self):
@@ -123,6 +135,19 @@ class TestSolo8v2VanillaEnv(unittest.TestCase):
     np.testing.assert_array_almost_equal(base_pos, new_pos)
     np.testing.assert_array_almost_equal(base_or, new_or)
 
+  def test_step_no_rewards(self):
+    env = solo_env.Solo8VanillaEnv(config=solo_env.Solo8VanillaConfig())
+    with self.assertRaises(ValueError):
+      env.step(np.zeros(self.env.action_space.shape[0]))
+
+  def test_step_simple_reward(self):
+    obs, reward, done, info = self.env.step(self.env.action_space.sample())
+    self.assertEqual(reward, 1)
+
+  def test_observation_space(self):
+    o = CompliantObs(None)
+    self.env.obs_factory.register_observation(o)
+    self.assertEqual(o.observation_space, self.env.observation_space)
 
 if __name__ == '__main__':
   unittest.main()
\ No newline at end of file