def populate(self, known_instrs):
addr = self.entry
while True:
known_instrs[addr] = self
if self.exe.architecture() == "x86":
opcode = self.exe.read(addr, 15)
result = X86.disassemble32(opcode, addr)
opcode = opcode[0:result.length]
instr = X86Instruction(opcode, addr, result, 4)
arch = X86
elif self.exe.architecture() == "x86_64":
opcode = self.exe.read(addr, 15)
result = X86.disassemble64(opcode, addr)
opcode = opcode[0:result.length]
instr = X86Instruction(opcode, addr, result, 8)
arch = X86
elif self.exe.architecture() == "ppc":
opcode = self.exe.read(addr, 4)
if len(opcode) == 4:
result = PPC.disassemble(struct.unpack(">I", opcode)[0], addr)
instr = PPCInstruction(opcode, addr, result)
else:
instr = PPCInstruction("", addr, PPC.Instruction())
arch = PPC
elif self.exe.architecture() == "arm":
opcode = self.exe.read(addr & (~1), 4)
if len(opcode) == 4:
result = Arm.disassemble(struct.unpack("<I", opcode)[0], addr)
instr = ArmInstruction(opcode, addr, result)
else:
instr = ArmInstruction("", addr, Arm.Instruction())
arch = Arm
else:
break
self.instrs += [instr]
instr.format_text(self, self.analysis.options)
if not instr.isValid():
break
if instr.isBlockEnding():
if instr.isConditionalBranch():
self.true_path = instr.target
self.false_path = addr + instr.length()
self.exits += [self.true_path, self.false_path]
elif instr.target != None:
self.exits += [instr.target]
break
addr += instr.length()
if addr in known_instrs:
self.exits += [addr]
break
def Stream():
while (1):
time.sleep(0.05)
a = arm.GetYoloOutput()
print(len(a))
if len(a) > 0:
arm.DrawTargetsOnVideo(a)
def main(IP, PORT, LH, LS, LV, UH, US, UV):
finish = False
Arm.Reset(IP, PORT)
Hand_flag = False
break_all = False
again_flag = False
Right_H = 0
Left_H = 0
tts.say("I am waiting!")
target = detection.target(IP, PORT, LH, LS, LV, UH, US, UV)
print("target", target)
while target == 1: # main loop
x, y = detection.getcenter(IP, PORT, LH, LS, LV, UH, US, UV) #
Left_H, Right_H = MoveArm.movement(IP, PORT, x, y)
print Left_H, Right_H
time.sleep(1)
Hand_flag = True
if Hand_flag == True:
target_hand = 0
pick_flag = False
CK = detection.check(IP, PORT, LH, LS, LV, UH, US, UV)
print("check", CK)
pick_flag, again_flag = detection.recenter(IP, PORT, CK, Left_H,
Right_H, x, y, LH, LS,
LV, UH, US, UV)
print pick_flag, again_flag
if again_flag == True: # try again
continue
while pick_flag == True:
target_hand = detection.hand(IP, PORT)
print("hand", target_hand)
if target_hand == 1:
tts.say("I will relesae it")
while Right_H == 1:
proxy.setAngles("RHand", 1, 0.2)
proxy.setAngles("RShoulderPitch", -0.087, 0.2)
tts.say("I finish the job")
Arm.Reset(IP, PORT)
Right_H = 0
pick_flag = False
target = 0
finish = True
continue
while Left_H == 1:
proxy.setAngles("LHand", 1, 0.2)
proxy.setAngles("LShoulderPitch", -0.087, 0.2)
tts.say("I finish the job")
Arm.Reset(IP, PORT)
Left_H = 0
pick_flag = False
target = 0
finish = True
continue
return finish
def populate(self, known_instrs):
addr = self.entry
while True:
known_instrs[addr] = self
if self.exe.architecture() == "x86":
opcode = self.exe.read(addr, 15)
result = X86.disassemble32(opcode, addr)
opcode = opcode[0:result.length]
instr = X86Instruction(opcode, addr, result, 4)
arch = X86
elif self.exe.architecture() == "x86_64":
opcode = self.exe.read(addr, 15)
result = X86.disassemble64(opcode, addr)
opcode = opcode[0:result.length]
instr = X86Instruction(opcode, addr, result, 8)
arch = X86
elif self.exe.architecture() == "ppc":
opcode = self.exe.read(addr, 4)
if len(opcode) == 4:
result = PPC.disassemble(
struct.unpack(">I", opcode)[0], addr)
instr = PPCInstruction(opcode, addr, result)
else:
instr = PPCInstruction("", addr, PPC.Instruction())
arch = PPC
elif self.exe.architecture() == "arm":
opcode = self.exe.read(addr & (~1), 4)
if len(opcode) == 4:
result = Arm.disassemble(
struct.unpack("<I", opcode)[0], addr)
instr = ArmInstruction(opcode, addr, result)
else:
instr = ArmInstruction("", addr, Arm.Instruction())
arch = Arm
else:
break
self.instrs += [instr]
instr.format_text(self, self.analysis.options)
if not instr.isValid():
break
if instr.isBlockEnding():
if instr.isConditionalBranch():
self.true_path = instr.target
self.false_path = addr + instr.length()
self.exits += [self.true_path, self.false_path]
elif instr.target != None:
self.exits += [instr.target]
break
addr += instr.length()
if addr in known_instrs:
self.exits += [addr]
break
def LookForObject():
global ismove
global lastpos
while (ismove == 0):
a = arm.GetYoloOutput()
if len(a) > 0:
arm.DrawTargetsOnVideo(a)
arm.StopMovement()
lastpos = arm.GetArmPosition()
GetObject()
def __init__(self):
self.level = 0
self.name = 'Default'
self.length = 100
self.resistance = 10
self.propulsion = 100
self.evasion = .1
self.armor = [50, 50, 50, 50, 50, 50, 50, 50]
self.health = [30, 30, 30, 30, 30, 30, 30, 30]
self.weapon = [[Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()],
[Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()],
[Arm.cannon127()], [Arm.cannon127()]]
self.color = white
self.block = [
block(),
block(),
block(),
block(),
block(),
block(),
block(),
block()
]
for i in range(0, 8):
self.block[i].number = i
self.block[i].init(self)
def __init__(self):
self.level = 0
self.name = 'Default'
self.length = 100
self.resistance = 10
self.propulsion = 100
self.evasion = .1
self.armor = [50, 50, 50, 50, 50, 50, 50, 50]
self.health = [30, 30, 30, 30, 30, 30, 30, 30]
self.weapon = [[Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()], [Arm.cannon127()]]
self.color = white
self.block = [block(), block(), block(), block(), block(), block(), block(), block()]
for i in range(0, 8):
self.block[i].number = i
self.block[i].init(self)
def choose(IP, PORT, LH, RH):
if LH == 1:
proxy.setAngles("LHand", 0.25, 0.2)
time.sleep(0.5)
Arm.Left_Arm(IP, PORT)
time.sleep(0.5)
pick_flag = True
elif RH == 1:
proxy.setAngles("RHand", 0.25, 0.2)
time.sleep(0.5)
Arm.Right_Arm(IP, PORT)
time.sleep(0.5)
pick_flag = True
else:
pick_flag = False
return pick_flag
def WaitForMove():
global ismove
ismove = 0
while (1):
a = arm.IsMoving()
if a == 0:
ismove = 1
break
time.sleep(0.5)
def reInit(self):
self.sta.name = 'Bismark'
self.sta.length = 250
self.sta.propulsion = 175
self.sta.evasion = 0.04
self.sta.health = [96, 96, 96, 96, 96, 96, 96, 96]
self.sta.armor = [100, 90, 90, 100, 100, 90, 90, 100]
self.sta.weapon = [[Arm.cannon380(), Arm.cannon150(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()],[Arm.cannon380(), Arm.cannon150(), Arm.cannon150(), Arm.cannon105(), Arm.cannon105()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (128, 128, 128)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def test_pair_arm(self):
idx_pair = (0, 0)
idx = 0
with open('../test_folder/pair_draw_1000.p', 'rb') as f:
draws_dict = p.load(f)
my_arm = Arm.PairArm(mu=0,
draws_in_advance=draws_dict['draws_in_advance'])
my_arm.set_idx_pair(idx_pair)
my_arm.draw(0)
def reInit(self):
self.sta.name = 'Fletcher'
self.sta.length = 115
self.sta.propulsion = 180
self.sta.evasion = 0.1
self.sta.health = [30, 30, 30, 30, 30, 30, 30, 30]
self.sta.armor = [50, 50, 50, 50, 50, 50, 50, 50]
self.sta.weapon = [[Arm.cannon460(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610()], [Arm.cannon460()], [], [Arm.cannon460()], [Arm.cannon460(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610()], [Arm.cannon460()], [], [Arm.cannon460()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (128, 128, 255)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def reInit(self):
self.sta.name = 'Shimakaze'
self.sta.length = 130
self.sta.propulsion = 205
self.sta.evasion = 0.08
self.sta.health = [36, 36, 36, 36, 36, 36, 36, 36]
self.sta.armor = [59, 59, 59, 59, 59, 59, 59, 59]
self.sta.weapon = [[Arm.cannon460(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610()], [Arm.cannon460()], [], [Arm.cannon460()], [Arm.cannon460(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610(), Arm.torpedo610()], [Arm.cannon460()], [], [Arm.cannon460()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (255, 200, 200)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def reInit(self):
self.sta.name = 'Richlieu'
self.sta.length = 248
self.sta.propulsion = 150
self.sta.evasion = 0.04
self.sta.health = [100, 100, 100, 100, 100, 100, 100, 100]
self.sta.armor = [100, 100, 100, 100, 100, 100, 100, 100]
self.sta.weapon = [[Arm.cannon380(), Arm.cannon152(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()],[Arm.cannon380(), Arm.cannon152(), Arm.cannon100(), Arm.cannon100(), Arm.cannon100()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (0, 0, 0)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def reInit(self):
self.sta.name = 'G3'
self.sta.length = 261
self.sta.propulsion = 160
self.sta.evasion = 0.04
self.sta.health = [115, 115, 115, 115, 115, 115, 115, 115]
self.sta.armor = [115, 115, 115, 115, 115, 115, 115, 115]
self.sta.weapon =[[Arm.cannon406(), Arm.cannon406(), Arm.cannon152(), Arm.cannon152()],[Arm.cannon406(), Arm.cannon152(), Arm.cannon152(), Arm.cannon120()],[Arm.cannon406(), Arm.cannon152(), Arm.cannon152(), Arm.cannon120()],[Arm.cannon406(), Arm.cannon152(), Arm.cannon152(), Arm.cannon120()],[Arm.cannon406(), Arm.cannon406(), Arm.cannon152(), Arm.cannon152()],[Arm.cannon406(), Arm.cannon152(), Arm.cannon152(), Arm.cannon120()],[Arm.cannon406(), Arm.cannon152(), Arm.cannon152(), Arm.cannon120()],[Arm.cannon406(), Arm.cannon152(), Arm.cannon152(), Arm.cannon120()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (255, 255, 0)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def reInit(self):
self.sta.name = 'Yamato'
self.sta.length = 263
self.sta.propulsion = 135
self.sta.evasion = 0.04
self.sta.health = [130, 130, 130, 130, 130, 130, 130, 130]
self.sta.armor = [130, 130, 130, 130, 130, 130, 130, 130]
self.sta.weapon = [[Arm.cannon460(), Arm.cannon460(), Arm.cannon155(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon155(), Arm.cannon155(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon155(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon155(), Arm.cannon155(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon460(), Arm.cannon155(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon155(), Arm.cannon155(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon155(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon460(), Arm.cannon155(), Arm.cannon155(), Arm.cannon127(), Arm.cannon127()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (255, 200, 200)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def reInit(self):
self.sta.name = 'Iowa'
self.sta.length = 263
self.sta.propulsion = 163
self.sta.evasion = 0.04
self.sta.health = [100, 100, 100, 100, 100, 100, 100, 100]
self.sta.armor = [100, 100, 100, 100, 100, 100, 100, 100]
self.sta.weapon = [[Arm.cannon406(), Arm.cannon406(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon127(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon127(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon406(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon127(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon127(), Arm.cannon127()],[Arm.cannon406(), Arm.cannon127(), Arm.cannon127(), Arm.cannon127()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (128, 128, 255)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def reInit(self):
self.sta.name = 'NorthCarolina'
self.sta.length = 222
self.sta.propulsion = 140
self.sta.evasion = 0.04
self.sta.health = [80, 80, 80, 80, 80, 80, 80, 80]
self.sta.armor = [95, 95, 95, 95, 95, 95, 95, 95]
self.sta.weapon = [[Arm.cannon410(), Arm.cannon410(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon130(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon130(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon410(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon130(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon130(), Arm.cannon130()],[Arm.cannon410(), Arm.cannon130(), Arm.cannon130(), Arm.cannon130()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (128, 128, 255)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def __init__(self):
self.A = bot.Arm()
self.M0 = mag.Magnet(12,25)
self.M5 = mag.Magnet(33,25)
self.M0.TURN_ON()
self.Walkmode=Walk.Walk(A,M0,M5)
self.Forkmode=Fork.Fork(A,M0,M5)
self.Cam=Camera.Camera(Walkmode.max_distance)
self.way=[]
def test_draw_advance(self):
with open('../test_folder/single_draw_1000.p', 'rb') as f:
draws_dict = p.load(f)
my_arm = Arm.Arm(mu=0,
draws_in_advance=draws_dict['draws_in_advance'][0])
my_arm.idx = 0
cur_t = 0
reward = my_arm.draw(cur_t)
assert reward == 0 or reward == 1
def test_unreachable(self): arm = Arm.Arm(link1=link1, link2=link2) end_effector = position(1+link1+link2, 0) with self.assertRaises(ValueError): joints = arm.inverse_kinematics(end_effector) end_effector = position(0, 0) with self.assertRaises(ValueError): joints = arm.inverse_kinematics(end_effector)
def reInit(self):
self.sta.name = 'Kongo'
self.sta.length = 222
self.sta.propulsion = 150
self.sta.evasion = 0.04
self.sta.health = [82, 82, 82, 82, 82, 82, 82, 82]
self.sta.armor = [94, 94, 94, 94, 94, 94, 94, 94]
self.sta.weapon = [[Arm.cannon356(), Arm.cannon152(), Arm.cannon152(), Arm.torpedo553(), Arm.torpedo553(), Arm.torpedo553(), Arm.torpedo553()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152(), Arm.torpedo553(), Arm.torpedo553(), Arm.torpedo553(), Arm.torpedo553()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152()], [Arm.cannon356(), Arm.cannon152(), Arm.cannon152()]]
self.sta.block = [Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block(), Ship.block()]
self.sta.color = (255, 200, 200)
for i in range(0, 8):
self.sta.block[i].number = i
self.sta.block[i].init(self.sta)
for block in self.sta.block:
block.armor += self.sta.level
block.health += self.sta.level
for weapon in block.weapon:
weapon.harm += self.sta.level
def __init__(self):
import sys
import os
if sys.platform == 'linux':
port_handler = PortHandler('/dev/ttyACM0')
else:
port_handler = PortHandler('/dev/' + os.listdir('/dev')[-2])
packet_handler = PacketHandler(1.0)
try:
port_handler.openPort()
port_handler.setBaudRate(1000000)
except OSError:
_ = None
self.arm = Arm(port_handler, packet_handler)
self.apply_pressure = False
self.pid = PID(self.arm.get_xy())
def build_body():
pymelLogger.debug('Starting: build_body()...')
Torso.build()
# legs
CanidaeLeg.build(side = Names.prefixes['left'])
CanidaeLeg.build(side = Names.prefixes['right'])
# arms
Arm.build(side = Names.prefixes['left'])
Arm.build(side = Names.prefixes['right'])
# Hands and fingers
Hand.build( side = Names.prefixes['left'] )
Hand.build( side = Names.prefixes['right'] )
# main ctrl
main_cnt_name = Names.joints_torso[0] + '_' + Names.suffixes['control']
main_cnt = Control.create( name=main_cnt_name, offsets=0, shape='circle_4_arrow',
size=1, color='yellow',
pos=None, parent=None, typ='body' )
pm.parent(main_cnt, 'controls')
######################
######################
# hard coded! fix!
consGrp = [Names.torso_module, Names.legs_module]
for grp in consGrp:
try: pm.parentConstraint( main_cnt, grp, mo=1 )
except: print 'Could not constraint: ' + main_cnt + ' to ' + grp
# constraint main to reference_jnt
pm.parentConstraint(main_cnt, Names.joints_torso[0], mo=1)
######################
######################
pymelLogger.debug('End: build_body()...')
def choose(IP, PORT, LH, RH, proxy):
proxy.setAngles("HeadPitch", 0.157, 0.2)
if LH == 1: #Left Hand
proxy.setAngles("LHand", 0.25, 0.1)
time.sleep(0.5)
Arm.Left_Arm(IP, PORT, proxy)
time.sleep(0.5)
pick_flag = True
elif RH == 1: #Right Hand
proxy.setAngles("RHand", 0.25, 0.1)
time.sleep(0.5)
Arm.Right_Arm(IP, PORT, proxy)
time.sleep(0.5)
pick_flag = True
else:
pick_flag = False
return pick_flag
def test_case2(self): arm = Arm.Arm(link1=link1, link2=link2) end_effector = position(link2, -link1) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, -pi / 2) self.assertAlmostEqual(joints.theta2, pi / 2) end_effector = position(0, link1+link2) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, pi / 2) self.assertAlmostEqual(joints.theta2, 0.0)
def test_case3(self): arm = Arm.Arm(link1=link1, link2=link2, origin=makeVector(1,1)) end_effector = position(1+link2, 1-link1) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, -pi / 2) self.assertAlmostEqual(joints.theta2, pi / 2) end_effector = position(1, 1+link1+link2) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, pi / 2) self.assertAlmostEqual(joints.theta2, 0.0)
def GetObject():
global lastpos
base = float(lastpos[1])
compensation = float(lastpos[3])
pos = "w"
while (1):
a = arm.GetYoloOutput()
if len(a) > 0:
pos = a[0][3]
arm.DrawTargetsOnVideo(a)
lock = arm.TryLockAtObject(a[0], 3, 40, res)
if (lock == 1):
base = base - 3
compensation = compensation - 3
arm.SetPosition(-1, base, -1, compensation, -1, -1)
WaitForMove()
trigger = arm.GetTriggerStatus()
if (trigger == "1"):
break
GetAndThrowiT(pos)
def test_case3(self): arm = Arm.Arm(link1=link1, link2=link2, origin=makeVector(1,1)) joints = jointangle(pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, 1 - link2) self.assertAlmostEqual(end_effector.y, 1 + link1) joints = jointangle(-pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, 1 + link2) self.assertAlmostEqual(end_effector.y, 1 - link1)
def test_case2(self): arm = Arm.Arm(link1=link1, link2=link2) joints = jointangle(pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, -link2) self.assertAlmostEqual(end_effector.y, link1) joints = jointangle(-pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, link2) self.assertAlmostEqual(end_effector.y, -link1)
def Secondtic():
WaitForMove()
arm.SetPosition(91.19,77.97,65.9,65.64,93.34,32)
WaitForMove()
arm.SetPosition(91.62,42.35,48.6,63.81,171.11,32)
WaitForMove()
arm.SetPosition(91.62,36.08,63.07,35.64,171.11,32)
WaitForMove()
arm.Gripper(85)
WaitForMove()
arm.SetPosition(92.62,36.48,57.99,46.59,171.11,-1)
time.sleep(1)
WaitForMove()
arm.SetPosition(91.62,42.35,48.6,63.81,171.11,-1)
WaitForMove()
arm.SetPosition(91,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,64.81,77,40,93,-1)
WaitForMove()
arm.Gripper(34)
def SecondGum2():
time.sleep(0.5)
WaitForMove()
arm.SetPosition(90.19,77.97,65.9,65.64,93.34,35)
WaitForMove()
arm.SetPosition(90.88,28.56,30.45,63.08,177.49,35)
WaitForMove()
arm.SetPosition(90.88,28.0,37.1,52.13,177.49,35)
WaitForMove()
arm.Gripper(85)
WaitForMove()
arm.SetPosition(90.88,29.22,30.85,64.64,177.49,-1)
WaitForMove()
arm.SetPosition(90.67,38.51,29.02,65.84,171.6,-1)
WaitForMove()
arm.SetPosition(91,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,110,79,45,170,-1)
WaitForMove()
arm.SetPosition(15,64.81,77,40,93,-1)
WaitForMove()
arm.Gripper(34)
from Arm import * arm = Arm() print arm.kinematicAnglesToArmAngles([0, 0.446, -pi/2, 0])
pygame.draw.circle(display, black, joints[1], 20)
pygame.draw.circle(display, arm_color, joints[1], 7)
if __name__ == '__main__':
pygame.init()
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()
origin = (width / 2, (height / 4)*3)
arm = Arm(origin[0], origin[1], 100, 60)
goalx = 100.0
goaly = -120.0
startx = 30.0
starty = 80.0
K = 1000.0
D = 40.0
xseq = np.linspace(startx, goalx, num=100)
yseq = np.linspace(starty, goaly, num=100)
step = 0
step_change = -1
