#! /usr/bin/env python
"""
This script tests the KUKA LWR arm, both the data and service api
"""
import sys
from time import sleep
from morse.testing.testing import MorseTestCase
from pymorse import Morse
# 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]def send_pose(s, x, y, yaw):
s.publish({'x' : x, 'y' : y, 'z' : 0.0, \
'yaw' : yaw, 'pitch' : 0.0, 'roll' : 0.0})
sleep(0.1)
[docs]class gripperTest(MorseTestCase):
[docs] def setUpEnv(self):
""" Defines the test scenario, using the Builder API.
"""
robot = ATRV('robot')
arm = KukaLWR()
robot.append(arm)
arm.translate(x=0.5, z=0.9)
arm.add_stream('socket')
arm.add_service('socket')
gripper = Gripper('gripper')
gripper.translate(z=1.28)
arm.append(gripper)
gripper.properties(Angle = 180.0, Distance=2.0)
gripper.add_service('socket')
teleport = Teleport()
robot.append(teleport)
teleport.add_stream('socket')
tape1 = PassiveObject(prefix='BlackVideotape')
tape1.properties(Object = True, Graspable = True, Label = "BlackTape")
tape1.translate(x=5, y=5, z=0)
tape2 = PassiveObject(prefix='WhiteVideotape')
tape2.properties(Object = True, Graspable = True, Label = "WhiteTapee")
tape2.translate(x=5, y=-5, z=0)
env = Environment('empty', fastmode=True)
env.add_service('socket')
[docs] def test_gripper(self):
""" This test is guaranteed to be started only when the simulator
is ready.
"""
with Morse() as morse:
# kuka controller socket
kuka_client = morse.robot.arm
teleport_stream = morse.robot.teleport
kuka_client.set_rotation('kuka_2', -1.57)
sleep(0.1)
obj = morse.robot.arm.gripper.grab().result()
self.assertEqual(obj, None)
obj = morse.robot.arm.gripper.release().result()
self.assertEqual(obj, None)
send_pose(teleport_stream, 3.0, 5, 0.0)
sleep(0.1)
obj = morse.robot.arm.gripper.grab().result()
self.assertEqual(obj, 'tape1')
send_pose(teleport_stream, 3.0, -5.0, 0.0)
obj = morse.robot.arm.gripper.grab().result()
self.assertEqual(obj, 'tape1')
send_pose(teleport_stream, 3.0, 8.0, 0.0)
obj = morse.robot.arm.gripper.release().result()
self.assertEqual(obj, True)
send_pose(teleport_stream, 3.0, -5.0, 0.0)
obj = morse.robot.arm.gripper.grab().result()
self.assertEqual(obj, 'tape2')
obj = morse.robot.arm.gripper.release().result()
self.assertEqual(obj, True)
send_pose(teleport_stream, 3.0, 8.0, 0.0)
obj = morse.robot.arm.gripper.grab().result()
self.assertEqual(obj, 'tape1')
########################## Run these tests ##########################
if __name__ == "__main__":
import unittest
from morse.testing.testing import MorseTestRunner
suite = unittest.TestLoader().loadTestsFromTestCase(gripperTest)
sys.exit(not MorseTestRunner().run(suite).wasSuccessful())