made specific envs for motor and angle commands

reworked motorcommand test

examples/random.py

@@ -4,7 +4,7 @@
 import gym_quadrotor
 
 
-env = gym.make("QuadrotorStabilizeAttitude-v0")
+env = gym.make("QuadrotorStabilizeAttitude-MotorCommands-v0")
 env.reset()
 
 for i in range(1000):

gym_quadrotor/__init__.py

@@ -17,11 +17,17 @@
 #)
 
 register(
-    id="QuadrotorStabilizeAttitude-v0",
+    id="QuadrotorStabilizeAttitude-MotorCommands-v0",
     entry_point='gym_quadrotor.envs:CopterStabilizeAttitudeEnv',
     max_episode_steps=500
 )
 
+register(
+    id="QuadrotorStabilizeAttitude-Angular-v0",
+    entry_point='gym_quadrotor.envs:CopterStabilizeAttitudeEnvAngular',
+    max_episode_steps=500
+)
+
 register(
     id="QuadrotorStabilizeAttitude2D-v0",
     entry_point='gym_quadrotor.envs:CopterStabilizeAttitude2DEnv',

gym_quadrotor/envs/__init__.py

@@ -1,3 +1,3 @@
 from .base import QuadRotorEnvBase
-from .attitude_env import CopterStabilizeAttitudeEnv
+from .attitude_env import CopterStabilizeAttitudeEnv, CopterStabilizeAttitudeEnvAngular
 from .minimal import CopterStabilizeAttitude2DEnv

gym_quadrotor/envs/attitude_env.py

@@ -51,3 +51,8 @@ def _reset_copter(self):
         self._state.attitude.yaw = self.random_state.uniform(low=-0.3, high=0.3)
         self._state.position[2] = 1
 
+
+def CopterStabilizeAttitudeEnvAngular():
+    from gym_quadrotor.wrappers.angular_control import AngularControlWrapper
+    return AngularControlWrapper(CopterStabilizeAttitudeEnv(), fixed_total=3.0)
+

gym_quadrotor/wrappers/test/motorcommands_test.py

@@ -0,0 +1,37 @@
+from gym_quadrotor.dynamics import Euler
+from gym_quadrotor.wrappers.angular_control import attitude_to_motor_control
+from gym_quadrotor.dynamics.copter import DynamicsState, CopterParams
+from gym_quadrotor.dynamics.dynamics import simulate_quadrotor
+import numpy as np
+import pytest
+
+
+@pytest.mark.parametrize("yaw", np.linspace(0, 2*np.pi))
+def test_angular_motor_commands(yaw):
+    startatt = [0.0, 0.0, yaw]
+    check_couple_direction(0, startatt)
+    check_couple_direction(1, startatt)
+    check_couple_direction(2, startatt)
+
+
+def check_couple_direction(index, startat = None):
+    setup = CopterParams()
+    copterstatus = DynamicsState()
+    if startat is not None:
+        copterstatus._attitude = Euler.from_numpy_array(startat)
+    base = np.zeros(3)
+    base[index] = 0.25
+    control = attitude_to_motor_control(3.0, *base)
+    copterstatus.desired_rotor_speeds = control * setup.max_rotor_speed
+    copterstatus._rotorspeeds = copterstatus.desired_rotor_speeds
+    start_attitude = np.copy(copterstatus.attitude._euler)
+    for i in range(10):
+        simulate_quadrotor(setup, copterstatus, 0.001)
+    delta = copterstatus.attitude._euler - start_attitude
+    #assert(delta[index] > 0.0)
+    nd = delta / delta[index]
+    ref = np.zeros(3)
+    ref[index] = 1
+    assert abs(nd[0] - ref[0]) < 1e-2
+    assert abs(nd[1] - ref[1]) < 1e-2
+    assert abs(nd[2] - ref[2]) < 1e-2