Source code for base.stabilized_quadrirotor_testing

#! /usr/bin/env python
"""
This script tests the 'data stream' oriented feature of the socket interface.
"""

from morse.testing.testing import MorseTestCase

try:
    # Include this import to be able to use your test file as a regular 
    # builder script, ie, usable with: 'morse [run|exec] <your test>.py
    from morse.builder import *
except ImportError:
    pass

import sys
from pymorse import Morse

[docs]def send_ctrl(s, theta, phi, psi, h): s.publish({'theta_c' : theta, 'phi_c' : phi , 'psi_c' : psi, 'h_c' : h})
[docs]class StabilizedQuadrirotorTest(MorseTestCase):
[docs] def setUpEnv(self): robot = QUAD2012('robot') pose = Pose() pose.add_stream('socket') robot.append(pose) motion = StabilizedQuadrotor('motion') robot.append(motion) motion.add_stream('socket') env = Environment('empty', fastmode = True) env.add_service('socket')
[docs] def test_theta_c_control(self): with Morse() as morse: pose_stream = morse.robot.pose cmd_client = morse.robot.motion pose = pose_stream.get() send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(3.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose['z'], 10.0, delta = 0.2) # theta_c permits to control the acceleration on x send_ctrl(cmd_client, 0.1, 0.0, 0.0, 10.0) morse.sleep(1.0) pose1 = pose_stream.get() dx1 = 0.4 self.assertAlmostEqual(pose1['x'], dx1, delta=0.2) self.assertAlmostEqual(pose1['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose1['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose1['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose1['pitch'], 0.1, delta=0.1) self.assertAlmostEqual(pose1['roll'], 0.0, delta=0.0) # acceleration is constant as long ass we waintain theta_c morse.sleep(1.0) pose2 = pose_stream.get() dx2 = 1.4 self.assertAlmostEqual(pose2['x'], dx1 + dx2, delta=0.2) self.assertAlmostEqual(pose2['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose2['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose2['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose2['pitch'], 0.1, delta=0.1) self.assertAlmostEqual(pose2['roll'], 0.0, delta=0.0) # if we setup theta_c to 0, acceleration on x is now null, # so v_x is constant (no friction) send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(1.0) pose3 = pose_stream.get() dx3 = 2.2 self.assertAlmostEqual(pose3['x'], dx1 + dx2 + dx3 , delta=0.2) self.assertAlmostEqual(pose3['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose3['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose3['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose3['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose3['roll'], 0.0, delta=0.0) morse.sleep(1.0) pose4 = pose_stream.get() self.assertAlmostEqual(pose4['x'], dx1 + dx2 + 2 * dx3, delta=0.2) self.assertAlmostEqual(pose4['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose4['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose4['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose4['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose4['roll'], 0.0, delta=0.0) # adding a negative value to theta_c gives a negative # acceleration on x, leading to some stabilization at some # point t send_ctrl(cmd_client, -0.1, 0.0, 0.0, 10.0) last_x = 0.0 pose = pose_stream.get() cur_x = pose['x'] while cur_x - last_x > 0.005: morse.sleep(0.01) last_x = cur_x pose = pose_stream.get() cur_x = pose['x'] send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(0.2) pose = pose_stream.get() morse.sleep(1.0) pose1 = pose_stream.get() self.assertAlmostEqual(pose['x'] - pose1['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose['y'] - pose1['y'], 0.0, delta=0.05) self.assertAlmostEqual(pose['z'] - pose1['z'], 0.0, delta=0.05) self.assertAlmostEqual(pose['yaw'] - pose1['yaw'], 0.0, delta=0.05) self.assertAlmostEqual(pose['pitch'] - pose1['pitch'], 0.0, delta=0.05) self.assertAlmostEqual(pose['roll'] - pose1['roll'], 0.0, delta=0.05)
[docs] def test_phi_c_control(self): with Morse() as morse: pose_stream = morse.robot.pose cmd_client = morse.robot.motion pose = pose_stream.get() send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(3.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose['z'], 10.0, delta = 0.2) # phi_c permits to control the acceleration on y send_ctrl(cmd_client, 0.0, 0.1, 0.0, 10.0) morse.sleep(1.0) pose1 = pose_stream.get() dy1 = -0.4 self.assertAlmostEqual(pose1['y'], dy1, delta=0.2) self.assertAlmostEqual(pose1['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose1['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose1['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose1['roll'], 0.1, delta=0.1) self.assertAlmostEqual(pose1['pitch'], 0.0, delta=0.0) # acceleration is constant as long ass we waintain theta_c morse.sleep(1.0) pose2 = pose_stream.get() dy2 = -1.4 self.assertAlmostEqual(pose2['y'], dy1 + dy2, delta=0.2) self.assertAlmostEqual(pose2['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose2['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose2['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose2['roll'], 0.1, delta=0.1) self.assertAlmostEqual(pose2['pitch'], 0.0, delta=0.0) # if we setup phi_c to 0, acceleration on y is now null, # so v_y is constant (no friction) send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(1.0) pose3 = pose_stream.get() dy3 = -2.2 self.assertAlmostEqual(pose3['y'], dy1 + dy2 + dy3 , delta=0.2) self.assertAlmostEqual(pose3['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose3['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose3['yaw'], 0.0, delta=0.02) self.assertAlmostEqual(pose3['pitch'], 0.0, delta=0.0) self.assertAlmostEqual(pose3['roll'], 0.0, delta=0.1) morse.sleep(1.0) pose4 = pose_stream.get() self.assertAlmostEqual(pose4['y'], dy1 + dy2 + 2 * dy3, delta=0.2) self.assertAlmostEqual(pose4['x'], 0.0, delta=0.2) self.assertAlmostEqual(pose4['z'], 10.0, delta=0.2) self.assertAlmostEqual(pose4['yaw'], 0.0, delta=0.0) self.assertAlmostEqual(pose4['pitch'], 0.0, delta=0.0) self.assertAlmostEqual(pose4['roll'], 0.0, delta=0.1) # adding a negative value to theta_c gives a negative # acceleration on x, leading to some stabilization at some # point t send_ctrl(cmd_client, 0.0, -0.1, 0.0, 10.0) last_y = 0.0 pose = pose_stream.get() cur_y = pose['y'] while cur_y - last_y < -0.005: morse.sleep(0.01) last_y = cur_y pose = pose_stream.get() cur_y = pose['y'] send_ctrl(cmd_client, 0.0, 0.0, 0.0, 10.0) morse.sleep(0.2) pose = pose_stream.get() morse.sleep(1.0) pose1 = pose_stream.get() self.assertAlmostEqual(pose['x'] - pose1['x'], 0.0, delta=0.05) self.assertAlmostEqual(pose['y'] - pose1['y'], 0.0, delta=0.2) self.assertAlmostEqual(pose['z'] - pose1['z'], 0.0, delta=0.05) self.assertAlmostEqual(pose['yaw'] - pose1['yaw'], 0.0, delta=0.05) self.assertAlmostEqual(pose['pitch'] - pose1['pitch'], 0.0, delta=0.05) self.assertAlmostEqual(pose['roll'] - pose1['roll'], 0.0, delta=0.05)
[docs] def test_psi_c_control(self): with Morse() as morse: pose_stream = morse.robot.pose cmd_client = morse.robot.motion pose = pose_stream.get() z = 12.0 send_ctrl(cmd_client, 0.0, 0.0, 0.0, z) morse.sleep(3.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=0.1) self.assertAlmostEqual(pose['yaw'], 0.0, delta=0.1) self.assertAlmostEqual(pose['z'], z, delta = 0.2) # psi correspond to a delta yaw cmd send_ctrl(cmd_client, 0.0, 0.0, 0.1, z) morse.sleep(1.0) pose = pose_stream.get() delta_yaw = -0.69 self.assertAlmostEqual(pose['x'], 0.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=0.1) self.assertAlmostEqual(pose['z'], z, delta=0.2) self.assertAlmostEqual(pose['yaw'], delta_yaw, delta=0.1) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1) morse.sleep(1.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=0.1) self.assertAlmostEqual(pose['z'], z, delta=0.2) self.assertAlmostEqual(pose['yaw'], 2 * delta_yaw, delta=0.1) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1) send_ctrl(cmd_client, 0.0, 0.0, 0.0, z) morse.sleep(1.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=0.1) self.assertAlmostEqual(pose['z'], z, delta=0.2) self.assertAlmostEqual(pose['yaw'], 2 * delta_yaw, delta=0.2) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1) send_ctrl(cmd_client, 0.0, 0.0, -0.1, z) morse.sleep(1.0) pose = pose_stream.get() self.assertAlmostEqual(pose['x'], 0.0, delta=0.1) self.assertAlmostEqual(pose['y'], 0.0, delta=0.1) self.assertAlmostEqual(pose['z'], z, delta=0.2) self.assertAlmostEqual(pose['yaw'], delta_yaw, delta=0.2) self.assertAlmostEqual(pose['pitch'], 0.0, delta=0.1) self.assertAlmostEqual(pose['roll'], 0.0, delta=0.1) ########################## Run these tests ##########################
if __name__ == "__main__": from morse.testing.testing import main main(StabilizedQuadrirotorTest)