forked from morse-simulator/morse
-
Notifications
You must be signed in to change notification settings - Fork 0
/
armature_pose_testing.py
134 lines (96 loc) · 4.51 KB
/
armature_pose_testing.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#! /usr/bin/env python
"""
This script tests the KUKA LWR arm, both the data and service api
"""
import sys
import math
from time import sleep
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
class ArmaturePoseTest(MorseTestCase):
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.configure_service('socket')
kuka_posture = ArmaturePose('arm_pose')
kuka_lwr.append(kuka_posture)
kuka_posture.add_stream('socket')
kuka_posture.configure_service('socket')
env = Environment('empty', fastmode = True)
env.configure_service('socket')
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)
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)
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()
self.assertEqual(len(pose), 7)
self.assertEqual(set(pose.keys()), set(JOINTS))
for j,v in pose.items():
self.assertAlmostEqual(v, 0.0, delta=precision)
simu.robot.arm.set_rotation("kuka_2", 1).result()
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":
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)
sleep(0.1)
target = dict(zip(JOINTS, angles))
pose = simu.robot.arm.arm_pose.get()
for j, v in pose.items():
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)
sleep(0.1)
target = dict(zip(JOINTS, angles))
pose = simu.robot.arm.arm_pose.get()
for j, v in pose.items():
self.assertAlmostEqual(v, target[j], delta=precision)
########################## Run these tests ##########################
if __name__ == "__main__":
import unittest
from morse.testing.testing import MorseTestRunner
suite = unittest.TestLoader().loadTestsFromTestCase(ArmaturePoseTest)
sys.exit(not MorseTestRunner().run(suite).wasSuccessful())