Source code for base.armature_pose_testing

#! /usr/bin/env python
"""
This script tests the KUKA LWR arm, both the data and service api
"""

import sys
import math
from morse.testing.testing import MorseTestCase
from pymorse import Morse, MorseServiceFailed

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

[docs]class ArmaturePoseTest(MorseTestCase):
[docs] def setUpEnv(self): """ Defines the test scenario, using the Builder API. """ robot = ATRV('robot') kuka_lwr = KukaLWR('arm') robot.append(kuka_lwr) kuka_lwr.translate(z=0.9) kuka_lwr.add_service('socket') kuka_posture = ArmaturePose('arm_pose') kuka_lwr.append(kuka_posture) kuka_posture.add_stream('socket') kuka_posture.add_service('socket') env = Environment('empty', fastmode = True) env.add_service('socket')
[docs] def test_pose_services(self): precision = 0.02 JOINTS = ['kuka_1', 'kuka_2', 'kuka_3', 'kuka_4', 'kuka_5', 'kuka_6', 'kuka_7'] with Morse() as simu: self.assertEqual(simu.robot.arm.arm_pose.get_joints(), JOINTS) #res = simu.robot.arm.get_rotations() #for joint in joints: # for i in range(3): # self.assertAlmostEqual(res[joint][i], 0.0, delta=precision) #res = simu.robot.arm.get_rotation('kuka_5').result() #for i in range(3): # self.assertAlmostEqual(res[i], 0.0, delta=precision) #res = simu.robot.arm.get_rotation('pipo') #self.assertEqual(type(res.exception(2)), MorseServiceFailed) res = simu.robot.arm.arm_pose.get_joints_length().result() self.assertAlmostEqual(res['kuka_1'], 0.31, delta=precision) self.assertAlmostEqual(res['kuka_2'], 0.20, delta=precision) self.assertAlmostEqual(res['kuka_3'], 0.20, delta=precision) self.assertAlmostEqual(res['kuka_4'], 0.20, delta=precision) self.assertAlmostEqual(res['kuka_5'], 0.19, delta=precision) self.assertAlmostEqual(res['kuka_6'], 0.08, delta=precision) self.assertAlmostEqual(res['kuka_7'], 0.13, delta=precision) # Move the arm now, and get the measure angles = [1.57, 2.0, 1.0, -1.28, 1.1, -2.0, 1.0] simu.robot.arm.set_rotations(angles) simu.sleep(0.1) pose = simu.robot.arm.arm_pose.get_state().result() target = dict(zip(JOINTS, angles)) for j, v in pose.items(): self.assertAlmostEqual(v, target[j], delta=precision)
[docs] def test_pose_stream(self): precision = 0.02 JOINTS = ['kuka_1', 'kuka_2', 'kuka_3', 'kuka_4', 'kuka_5', 'kuka_6', 'kuka_7'] with Morse() as simu: pose = simu.robot.arm.arm_pose.get() # 7 joints + timestamp self.assertEqual(len(pose), 7 + 1) self.assertEqual(set(pose.keys()), set(JOINTS).union(set(['timestamp']))) for j, v in pose.items(): if j != 'timestamp': self.assertAlmostEqual(v, 0.0, delta=precision) simu.robot.arm.set_rotation("kuka_2", 1).result() simu.sleep(0.1) pose = simu.robot.arm.arm_pose.get() self.assertAlmostEqual(simu.robot.arm.arm_pose.get()["kuka_2"], 1.0, delta = precision) for j, v in pose.items(): if j != "kuka_2" and j != 'timestamp': self.assertAlmostEqual(v, 0.0, delta=precision) # Move the arm now, and get the measure angles = [1.57, 2.0, 1.0, -1.28, 1.0, -2.0, 1.0] simu.robot.arm.set_rotations(angles) simu.sleep(0.1) target = dict(zip(JOINTS, angles)) pose = simu.robot.arm.arm_pose.get() for j, v in pose.items(): if j != 'timestamp': self.assertAlmostEqual(v, target[j], delta=precision) angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] simu.robot.arm.set_rotations(angles) simu.sleep(0.1) target = dict(zip(JOINTS, angles)) pose = simu.robot.arm.arm_pose.get() for j, v in pose.items(): if j != 'timestamp': self.assertAlmostEqual(v, target[j], delta=precision) ########################## Run these tests ##########################
if __name__ == "__main__": from morse.testing.testing import main main(ArmaturePoseTest)