Merge remote-tracking branch 'origin/Add-ProDMP-envs' into mujoco_binding

# Conflicts:
#	fancy_gym/black_box/factory/basis_generator_factory.py

fancy_gym/black_box/controller/base_controller.py

@@ -2,3 +2,6 @@ class BaseController:
 
     def get_action(self, des_pos, des_vel, c_pos, c_vel):
         raise NotImplementedError
+
+    def __call__(self, des_pos, des_vel, c_pos, c_vel):
+        return self.get_action(des_pos, des_vel, c_pos, c_vel)

fancy_gym/black_box/controller/meta_world_controller.py

@@ -18,7 +18,8 @@ class MetaWorldController(BaseController):
         cur_pos = c_pos[:-1]
         xyz_pos = des_pos[:-1]
 
-        assert xyz_pos.shape == cur_pos.shape, \
-            f"Mismatch in dimension between desired position {xyz_pos.shape} and current position {cur_pos.shape}"
+        if xyz_pos.shape != cur_pos.shape:
+            raise ValueError(f"Mismatch in dimension between desired position"
+                             f" {xyz_pos.shape} and current position {cur_pos.shape}")
         trq = np.hstack([(xyz_pos - cur_pos), gripper_pos])
         return trq

fancy_gym/black_box/controller/pd_controller.py

@@ -8,7 +8,6 @@ class PDController(BaseController):
     A PD-Controller. Using position and velocity information from a provided environment,
     the tracking_controller calculates a response based on the desired position and velocity
 
-    :param env: A position environment
     :param p_gains: Factors for the proportional gains
     :param d_gains: Factors for the differential gains
     """
@@ -20,9 +19,11 @@ class PDController(BaseController):
         self.d_gains = d_gains
 
     def get_action(self, des_pos, des_vel, c_pos, c_vel):
-        assert des_pos.shape == c_pos.shape, \
-            f"Mismatch in dimension between desired position {des_pos.shape} and current position {c_pos.shape}"
-        assert des_vel.shape == c_vel.shape, \
-            f"Mismatch in dimension between desired velocity {des_vel.shape} and current velocity {c_vel.shape}"
+        if des_pos.shape != c_pos.shape:
+            raise ValueError(f"Mismatch in dimension between desired position "
+                             f"{des_pos.shape} and current position {c_pos.shape}")
+        if des_vel.shape != c_vel.shape:
+            raise ValueError(f"Mismatch in dimension between desired velocity"
+                             f" {des_vel.shape} and current velocity {c_vel.shape}")
         trq = self.p_gains * (des_pos - c_pos) + self.d_gains * (des_vel - c_vel)
         return trq

fancy_gym/black_box/factory/basis_generator_factory.py

@@ -1,4 +1,5 @@
-from mp_pytorch.basis_gn import NormalizedRBFBasisGenerator, ZeroPaddingNormalizedRBFBasisGenerator, ProDMPBasisGenerator
+from mp_pytorch.basis_gn import NormalizedRBFBasisGenerator, ZeroPaddingNormalizedRBFBasisGenerator, \
+    ProDMPBasisGenerator
 from mp_pytorch.phase_gn import PhaseGenerator
 
 ALL_TYPES = ["rbf", "zero_rbf", "rhythmic"]
@@ -11,6 +12,8 @@ def get_basis_generator(basis_generator_type: str, phase_generator: PhaseGenerat
     elif basis_generator_type == "zero_rbf":
         return ZeroPaddingNormalizedRBFBasisGenerator(phase_generator, **kwargs)
     elif basis_generator_type == "prodmp":
+        from mp_pytorch.phase_gn import ExpDecayPhaseGenerator
+        assert isinstance(phase_generator, ExpDecayPhaseGenerator)
         return ProDMPBasisGenerator(phase_generator, **kwargs)
     elif basis_generator_type == "rhythmic":
         raise NotImplementedError()