00001
00002
00003 import time
00004 from random import *
00005
00006 import roslib
00007 roslib.load_manifest('cob_script_server')
00008 import rospy
00009
00010 from simple_script_server import script
00011
00012 class RandomMoves(script):
00013
00014 def Initialize(self):
00015 self.sss.init("sdh")
00016 self.sss.init("tray")
00017 self.sss.init("torso")
00018 self.sss.init("arm")
00019
00020 def Run(self):
00021 seed()
00022 maxVal = 0.3
00023 print "start"
00024 self.sss.move("sdh","home",False)
00025 self.sss.move("torso","home",False)
00026 handle01 = self.sss.move("arm","folded",False)
00027 self.sss.move("tray","up")
00028 handle01.wait()
00029 for i in range(1,3):
00030 r1 = (random()-0.5)*2*maxVal;
00031 r2 = (random()-0.5)*2*maxVal;
00032 self.sss.move("torso",[[0.5*r1,0.5*r2,r1,r2]])
00033 self.sss.move("arm","pregrasp")
00034 self.sss.move_cart_rel("arm",[[0.0, 0.0, -0.1], [0.0, 0.0, 0.0]])
00035 self.sss.move_cart_rel("arm",[[0.0, 0.1, 0.0], [0.0, 0.0, 0.0]])
00036 self.sss.move("sdh","cylopen")
00037 self.sss.move("sdh","cylclosed")
00038 self.sss.move("tray","down")
00039 self.sss.move("tray","up")
00040 self.sss.move("arm","folded")
00041 self.sss.sleep(1)
00042 self.sss.move("sdh","home",False)
00043 self.sss.move("torso","home",False)
00044 self.sss.move("arm","folded",False)
00045 self.sss.move("tray","up")
00046
00047 if __name__ == "__main__":
00048 SCRIPT = RandomMoves()
00049 SCRIPT.Start()
