def __init__(self, arm1Len = 3, arm2Len = 2, arm3Len = 1, goalPoint = {'x' :1, 'y' : 1 , 'z' : 1 }):
self.exactitudeParams = (0.05)
self.arm = Arm(arm1Len, arm2Len, arm3Len)
self.goalPoint = goalPoint
self.gs = GeneticSolver(arm1Len, arm2Len, arm3Len, goalPoint)
if not self.isAPossibleShot():
print('Goal point is too far ')
def __init__(self):
"""
Build all of Inmoov's parts.
"""
#open the file json file
with open(INMOOV_FILE) as json_file:
json.load(json_file, object_hook=parse)
self.head = Head(
filter(lambda x: x.name == "head_x", servos)[0],
filter(lambda x: x.name == "head_y", servos)[0])
#Right side
self.right_wrist = Wrist(
filter(lambda x: x.name == "left_wrist", servos)[0])
self.right_hand = Hand(
Finger(filter(lambda x: x.name == "right_pinky", servos)[0]),
Finger(filter(lambda x: x.name == "right_ring", servos)[0]),
Finger(filter(lambda x: x.name == "right_mid", servos)[0]),
Finger(filter(lambda x: x.name == "right_index", servos)[0]),
Finger(filter(lambda x: x.name == "right_thumb", servos)[0]))
self.right_forearm = Forearm(self.right_hand, self.right_wrist)
self.right_shoulder = Shoulder(
filter(lambda x: x.name == "right_shoulder_flexion", servos)[0],
filter(lambda x: x.name == "right_shoulder_abduction", servos)[0],
filter(lambda x: x.name == "right_shoulder_rotation_x", servos)[0],
filter(lambda x: x.name == "right_shoulder_rotation_y", servos)[0])
self.right_arm = Arm(self.right_forearm, self.right_shoulder)
#Left side
self.left_wrist = Wrist(
filter(lambda x: x.name == "left_wrist", servos)[0])
self.left_hand = Hand(
Finger(filter(lambda x: x.name == "left_pinky", servos)[0]),
Finger(filter(lambda x: x.name == "left_ring", servos)[0]),
Finger(filter(lambda x: x.name == "left_middle", servos)[0]),
Finger(filter(lambda x: x.name == "left_index", servos)[0]),
Finger(filter(lambda x: x.name == "left_thumb", servos)[0]))
self.left_forearm = Forearm(self.left_hand, self.left_wrist)
self.left_shoulder = Shoulder(
filter(lambda x: x.name == "left_shoulder_flexion", servos)[0],
filter(lambda x: x.name == "left_shoulder_abduction", servos)[0],
filter(lambda x: x.name == "left_shoulder_rotation_x", servos)[0],
filter(lambda x: x.name == "left_shoulder_rotation_y", servos)[0])
self.left_arm = Arm(self.left_forearm, self.left_shoulder)
self.initialize()
def get_available_arms(self, t):
# Construct a list of Arm objects
arm_list = []
for _, row in self.df.loc[t].iterrows():
arm_list.append(
Arm(len(arm_list), row['context'], row['true_mean']))
return arm_list
def _get_time_bw_poses(pose0, pose1, velocity=0.2):
'''Determines how much time should be allowed for
moving between two poses'''
dist = Arm.get_distance_bw_poses(pose0, pose1)
duration = dist / velocity
if duration < 0.5:
duration = 0.5
return duration
def _get_time_bw_poses(pose0, pose1, velocity=0.2):
'''Determines how much time should be allowed for
moving between two poses'''
dist = Arm.get_distance_bw_poses(pose0, pose1)
duration = dist / velocity
if duration < 0.5:
duration = 0.5
return duration
def __init__(self):
r_arm = Arm(Side.RIGHT)
l_arm = Arm(Side.LEFT)
Arms.arms = [r_arm, l_arm]
self.attended_arm = -1
self.action = None
self.preempt = False
rospy.loginfo('Arms have been initialized.')
Arms.arms[0].set_mode(ArmMode.HOLD)
Arms.arms[1].set_mode(ArmMode.HOLD)
Arms.arms[0].check_gripper_state()
Arms.arms[1].check_gripper_state()
Arms.arms[Side.RIGHT].open_gripper()
Arms.arms[Side.LEFT].close_gripper()
self.status = ExecutionStatus.NOT_EXECUTING
def __init__(self, data, num_arms, epsilon=0.9, decay=0.95):
self.epsilon = epsilon
self.decay = decay
self.data = data
self.num_arms = num_arms
self.indices = self.generate_indices()
self.sampled_priors = np.zeros(self.num_arms)
self.arms = [Arm() for i in range(num_arms)]
self.final_values = {}
def __init__(self, position=(0,0), name="simple", bTwoArms=True, collisionGroup=None, bOppositeArms=False):
"""Init body and arms of the robot."""
global nao
nao = self
self.ini_pos = position
x, y = position[0], position[1]
self.salient = []
self.bTwoArms = bTwoArms
self.body_pos = (x, y)
w = 0.4
if(bOppositeArms):
w = 0.5
if(not bOppositeArms and bTwoArms):
createBox((x - w / 2.0, y), w=w / 2.0, h=w / 1.8, bDynamic=False, collisionGroup=-1)
createBox((x + w / 2.0, y), w=w / 2.0, h=w / 1.8, bDynamic=False, collisionGroup=-1)
bShrink = False
length = 1
self.nparts = 2
if(name == "human"):
self.nparts = 3
length = 1.0 / 1.5
bShrink = True
self.arms = []
if(not bOppositeArms):
self.arms.append(Arm(bLateralize=1, hdiv=1, nparts=self.nparts, position=(x - w, y), length=length, name=name, bShrink=bShrink, collisionGroup=collisionGroup))
if(bTwoArms):
self.arms.append(Arm(bLateralize=2, hdiv=1, nparts=self.nparts, position=(x+w,y), length=length, name=name, bShrink=bShrink, collisionGroup=collisionGroup))
else:
arm1 = Arm(position=(x, y), bLateralize=0, hdiv=1, nparts=self.nparts, length = length, name=name, bShrink=bShrink, collisionGroup=collisionGroup)
arm2 = Arm(position=(x, y + 3), signDir=-1, bLateralize=0, hdiv=1, nparts=self.nparts, length=length, name=name, bShrink=bShrink, collisionGroup=collisionGroup)
self.arms.append(arm1)
self.arms.append(arm2)
def main():
if len(sys.argv) < 3:
print_usage()
exit()
num_pulls = int(sys.argv[1])
goal = int(sys.argv[2])
arms = [Arm(arm.split(",")) for arm in sys.argv[3:]]
bandit = Bandit(arms)
alg = IncrementalUniformBandit(num_pulls, bandit)
print "NOTE: cumulative regret is incorrect right now"
print "uniform simple regret", alg.get_simple_regret()
print "uniform cumulative regret", alg.get_cum_regret()
print "uniform best arm", alg.get_best_arm()
alg = UCBBandit(num_pulls, bandit)
print "ucb simple regret", alg.get_simple_regret()
print "ucb cumulative regret", alg.get_cum_regret()
print "ucb best arm", alg.get_best_arm()
def _test():
#Testing:
print('Testing:')
arm = Arm('/dev/ttyACM0', True)
g = TTTGame(Arm)
while True:
letter = input('What character: X(1) or O(2) ')
if letter in ('1', '2'):
break
g.setPLetter(int(letter))
print(g.getPLetter())
while not g.board.isBoardFull():
g.board.draw()
playerMove = input('Player move: ')
print(g.makeMove(int(playerMove)))
g.board.draw()
g.quit()
def play(gym_mode):
BUFFER_SIZE = 500000
BATCH_SIZE = 32
GAMMA = 0.99
TAU = 0.001
LRA = 0.0001
LRC = 0.001
ACTION_DIM = 1
STATE_DIM = 3
NUM_EPISODES = 3000
INITIAL_REPLAY_SIZE = 20000
BATCH_SIZE = 32
np.random.seed(1234)
agent = Agent(ACTION_DIM, STATE_DIM, TAU, GAMMA, LRA, LRC,
INITIAL_REPLAY_SIZE, BATCH_SIZE)
if gym_mode:
env = gym.make("Pendulum-v0")
else:
from Arm import Arm
import rospy
env = Arm()
for _ in xrange(NUM_EPISODES):
terminal = False
state = env.reset()
t = 0
print _
while not terminal:
env.render()
action = agent.get_action(state.reshape(1, state.shape[0]))
next_state, reward, terminal, _ = env.step(action)
agent.run(state, action, reward, terminal, next_state)
t += 1
if t == 300:
break
if not gym_mode:
if rospy.is_shutdown():
break
def simulate_ts():
arms = [Arm(0.3) for i in range(4)]
arms.append(Arm(0.5))
return thompson_sampling(arms=arms, T=10**3)
A : Motor 0 : 0
B : Motor 1 : 1
X : Motor 2 : 2
Y : Motor 3 : 3
LB : Motor 4 : 4
RB :Servo Motor : 5
START : 7
BACK : 6
Servo moter
Threashould Duty cycle 345 ~ 525
"""
t1 = TB6560(9, 11) #Bottom
t2 = TB6560(14, 15) #Second1
t3 = TB6560(17, 27) #Second2
t4 = TB6560(25, 8) #Thread
t5 = TB6560(23, 24) #Forth
t6 = TB6560(16, 20) #Arm
if __name__ == '__main__':
Robo_Arm = Arm([t1, t2, t3, t4, t5, t6])
flag = True
while flag:
Robo_Arm.Get_Data()
flag = False if Robo_Arm.Get_Button() == "6" else True
Robo_Arm.Decide_Move()
time.sleep(0.02)
Robo_Arm.Stop()
GPIO.cleanup()
def simulate_ucb():
arms = [Arm(0.3) for i in range(4)]
arms.append(Arm(0.5))
return UCB(arms=arms, T=10**3)
import time from Arm import Arm arm = Arm() #arm.backwd() #time.sleep(3) arm.fwd()
import time
from Arm import Arm
arm = Arm()
status = 0
while True:
if status == 0:
print "Rauf fahren"
arm.up()
time.sleep(8)
print "Oben"
arm.upDownStop()
status = 1
elif status == 1:
#arm.release()
time.sleep(1.2)
arm.holdItem()
arm.fwd()
time.sleep(5)
status = 2
elif status == 2:
time.sleep(3)
status = 3
elif status == 3:
arm.grap()
time.sleep(1.3)
arm.holdItem()
time.sleep(1)
arm.up()
from Arm import Arm
def __calc_success_ratio(arm):
if arm.success + arm.fail == 0:
return 0
return arm.success / (arm.success + arm.fail)
def epsilon_greedy(arms, T, epsilon):
reward = 0
hist = []
for i in range(1, T + 1):
if binomial(n=1, p=epsilon) == 1:
# 探索ステップ : アームを一様ランダムに選ぶ
index = randint(0, len(arms))
else:
# 活用ステップ : 今までで一番成功確率の高いアームを選ぶ
avgs = [__calc_success_ratio(arm) for arm in arms]
index = avgs.index(max(avgs))
tmp = arms[index].play()
reward += tmp
hist.append(tmp)
return reward, hist
if __name__ == "__main__":
arms = [Arm(0.3) for _ in range(5)] + [Arm(0.1)
for _ in range(4)] + [Arm(0.5)]
print(epsilon_greedy(arms=arms, T=10**3, epsilon=0.3))
import time from Arm import Arm arm = Arm() arm.down() time.sleep(8) arm.upDownStop()
def TestMoveServo():
#Setup
curServo = m_arm.GetCurrentServo()
startingAngle = 80
curServo.SetAngle(startingAngle)
TestMoveServoForward()
TestMoveServoBack()
bigAngle = curServo.MaxAllowedAngle
curServo.SetAngle(bigAngle)
TestMoveServoForward()
TestMoveServoBack()
smallAngle = curServo.MinAllowedAngle
curServo.SetAngle(smallAngle)
TestMoveServoBack()
TestMoveServoForward()
def TestMethods():
OutputServoIndex()
TestCycleUp()
TestCycleDown()
TestMoveServo()
if __name__ == '__main__':
m_arm = Arm()
TestMethods()
import time from Arm import Arm arm = Arm() arm.turnLeft() time.sleep(2) #arm.turnStop()
def simulate_eg(epsilon):
arms = [Arm(0.3) for i in range(4)]
arms.append(Arm(0.5))
return epsilon_greedy(arms=arms, T=10**3, epsilon=epsilon)
class Inmoov(object):
"""
This class are Inmoov!
"""
def __init__(self):
"""
Build all of Inmoov's parts.
"""
#open the file json file
with open(INMOOV_FILE) as json_file:
json.load(json_file, object_hook=parse)
self.head = Head(
filter(lambda x: x.name == "head_x", servos)[0],
filter(lambda x: x.name == "head_y", servos)[0])
#Right side
self.right_wrist = Wrist(
filter(lambda x: x.name == "left_wrist", servos)[0])
self.right_hand = Hand(
Finger(filter(lambda x: x.name == "right_pinky", servos)[0]),
Finger(filter(lambda x: x.name == "right_ring", servos)[0]),
Finger(filter(lambda x: x.name == "right_mid", servos)[0]),
Finger(filter(lambda x: x.name == "right_index", servos)[0]),
Finger(filter(lambda x: x.name == "right_thumb", servos)[0]))
self.right_forearm = Forearm(self.right_hand, self.right_wrist)
self.right_shoulder = Shoulder(
filter(lambda x: x.name == "right_shoulder_flexion", servos)[0],
filter(lambda x: x.name == "right_shoulder_abduction", servos)[0],
filter(lambda x: x.name == "right_shoulder_rotation_x", servos)[0],
filter(lambda x: x.name == "right_shoulder_rotation_y", servos)[0])
self.right_arm = Arm(self.right_forearm, self.right_shoulder)
#Left side
self.left_wrist = Wrist(
filter(lambda x: x.name == "left_wrist", servos)[0])
self.left_hand = Hand(
Finger(filter(lambda x: x.name == "left_pinky", servos)[0]),
Finger(filter(lambda x: x.name == "left_ring", servos)[0]),
Finger(filter(lambda x: x.name == "left_middle", servos)[0]),
Finger(filter(lambda x: x.name == "left_index", servos)[0]),
Finger(filter(lambda x: x.name == "left_thumb", servos)[0]))
self.left_forearm = Forearm(self.left_hand, self.left_wrist)
self.left_shoulder = Shoulder(
filter(lambda x: x.name == "left_shoulder_flexion", servos)[0],
filter(lambda x: x.name == "left_shoulder_abduction", servos)[0],
filter(lambda x: x.name == "left_shoulder_rotation_x", servos)[0],
filter(lambda x: x.name == "left_shoulder_rotation_y", servos)[0])
self.left_arm = Arm(self.left_forearm, self.left_shoulder)
self.initialize()
def do_motion(self, motion_id):
"""
Make InMoov do one of these motions
"""
if motion_id == 0:
self.wave()
elif motion_id == 1:
self.point()
time.sleep(5)
self.wave()
elif motion_id == 2:
self.initialize()
def wave(self):
self.left_arm.forearm.hand.off()
self.left_arm.shoulder.rotation_up(-20)
self.left_arm.shoulder.rotation_internal(60)
self.left_arm.shoulder.abduction_up(-90)
time.sleep(2)
self.left_arm.shoulder.abduction_up(60)
time.sleep(0.5)
time.sleep(2)
self.left_arm.shoulder.abduction_up(90)
time.sleep(1.5)
self.left_arm.shoulder.abduction_up(0)
time.sleep(2)
self.left_arm.shoulder.abduction_up(90)
time.sleep(1.5)
self.left_arm.shoulder.abduction_up(0)
self.left_arm.shoulder.rotation_up(-20)
self.left_arm.shoulder.flex(90)
self.left_arm.shoulder.rotation_internal(60)
def point(self):
self.left_arm.shoulder.rotation_up(-20)
self.left_arm.shoulder.rotation_internal(60)
time.sleep(3)
self.left_arm.shoulder.rotation_internal(90)
self.left_arm.forearm.hand.make_fist()
self.left_arm.shoulder.rotation_up(60)
self.left_arm.forearm.hand.straighten_all_fingers()
def thumbs_down(self):
pass
def goodbye(self):
pass
def initialize(self):
"""initializes InMoov"""
self.head.initialize()
self.left_arm.initialize()
def off(self):
"""Turns InMoov off"""
self.head.off()
self.left_arm.off()
self.right_arm.off()
def __init__(self, seed=0, n=10, spread=2):
self.n = n
self.spread = spread
self.arms = []
for s in range(n):
self.arms.append(Arm(seed + s, spread))
def main():
status_ui = aiy.voicehat.get_status_ui()
status_ui.status('starting')
assistant = aiy.assistant.grpc.get_assistant()
button = aiy.voicehat.get_button()
arm = Arm('/dev/ttyACM0', False)
ttt = TTTGame(arm)
grqd = GRQDGame(arm)
guesses = 4
time.sleep(2)
ttt.arm.aWrite('N\n')
game = 'none'
with aiy.audio.get_recorder():
while True:
status_ui.status('ready')
print('Press the button and speak')
#button.wait_for_press()
input("Press Enter to continue...")
status_ui.status('listening')
print('Listening...')
text, audio = assistant.recognize()
#text = input('Input:\n')
if text:
print('You said "', text, '"')
# New game prompt:
if 'start a' in text and 'game' in text:
if 'tic tac toe' in text or 'TTT' in text or 'tic-tac-toe' in text:
ttt = TTTGame(arm)
aiy.audio.say(
'Ok, starting a new tic tac toe game. Would you like to be X or O?'
)
ttt.drawBoard()
game = 'ttt'
elif 'reverse Google Quick Draw' in text or 'rgqd' in text:
aiy.audio.say(
'Ok, starting a new reverse Google Quick Draw game.'
)
#aiy.audio.say('This game reverses the role of Google Quick Draw.')
#aiy.audio.say('You have to decide which object the drawing best represents.')
grqd.chooseObjects()
grqd.generateCoordinates()
message = 'Is it a ' + str(
GRQDGame.choices[0]) + ', a ' + str(
GRQDGame.choices[1]) + ', a ' + str(
GRQDGame.choices[2]) + ', or a ' + str(
GRQDGame.choices[3]) + '.'
print(message)
#aiy,audio.say(message.decode("utf-8"))
grqd.drawObject(grqd.xCoords, grqd.yCoords)
game = 'rgqd'
else:
aiy.audio.say('Sorry, I dont support that game yet!')
audio = False
# TTT prompts:
elif game == 'ttt':
# Choose letter:
if 'set my letter' in text:
setResult = 'none'
if 'ex' in text or 'X' in text:
aiy.audio.say(
'Ok, setting your letter to ex. X will go first'
)
setResult = ttt.setPLetter(1)
audio = False
elif 'oh' in text or '200' in text:
setResult = ttt.setPLetter(2)
aiy.audio.say(
'Ok, setting your letter to oh. X will go first'
)
audio = False
if setResult == 'Done':
aiy.audio.say('You have already set your letter')
audio = False
# Choose position:
elif 'move to' in text or 'position' in text:
moveResult = 'none'
if '1' in text or 'one' in text:
aiy.audio.say('Ok, moving to position 1')
moveResult = ttt.makeMove(1)
elif '2' in text or 'position to' in text:
aiy.audio.say('Ok, moving to position 2')
moveResult = ttt.makeMove(2)
elif '3' in text or 'three' in text:
aiy.audio.say('Ok, moving to position 3')
moveResult = ttt.makeMove(3)
elif '4' in text or 'four' in text:
aiy.audio.say('Ok, moving to position 4')
moveResult = ttt.makeMove(4)
elif '5' in text or 'five' in text:
aiy.audio.say('Ok, moving to position 5')
moveResult = ttt.makeMove(5)
elif '6' in text or 'six' in text:
aiy.audio.say('Ok, moving to position 6')
moveResult = ttt.makeMove(6)
elif '7' in text or 'seven' in text:
aiy.audio.say('Ok, moving to position 7')
moveResult = ttt.makeMove(7)
elif '8' in text or 'eight' in text:
aiy.audio.say('Ok, moving to position 8')
moveResult = ttt.makeMove(8)
elif '9' in text or 'nine' in text:
aiy.audio.say('Ok, moving to position 9')
moveResult = ttt.makeMove(9)
else:
aiy.audio.say(
'Sorry, that is not a valid position')
print(moveResult)
if moveResult == 'Failure':
aiy.audio.say(
'Sorry, that position is already occupied')
audio = False
elif moveResult == '1W':
aiy.audio.say('Looks like X has won!')
aiy.audio.say('Thanks for playing!')
game = 'none'
audio = False
elif moveResult == '2W':
aiy.audio.say('Looks like O has won!')
aiy.audio.say('Thanks for playing!')
game = 'none'
audio = False
elif moveResult == 'Tie':
aiy.audio.say('Looks like its a tie!')
aiy.audio.say('Thanks for playing!')
game = 'none'
audio = False
audio = False
elif game == 'rgqd':
if grqd.item in text:
aiy.audio.say('Correct!')
game == 'none'
else:
guesses = guesses - 1
if guesses == 0:
aiy.audio.say(
'Sorry, thats not correct. The object was a ' +
GRQDGame.item + '.')
game == 'none'
guesses = 4
else:
aiy.audio.say(
'Sorry, thats not correct. You have ' +
str(guesses) + ' guesses left.')
audio = False
# Exit prompt:
elif text == 'goodbye':
status_ui.status('stopping')
print('Bye!')
break
if game == 'ttt':
ttt.board.draw()
if audio:
aiy.audio.play_audio(audio)
audio = True
## -*- coding: utf-8 -*- from Arm import Arm #from TouchSensorArduino import TouchSensorArduino #import matplotlib.pyplot as plt #ts = TouchSensorArduino() robo = Arm()
#Creación de la jerarquía de Bones del MoCap
#cada objeto se inicializa con un ID (nombre identificador) y las coordenadas
#dentro de la sección MOTION
#miembros inferiores
LeftUpLeg = UpLeg('Izquierda', 132, 133, 134)
RightUpLeg = UpLeg('Derecha', 144, 145, 146)
LeftLeg = Leg('Izquierda', 135, 136, 137)
RightLeg = Leg('Derecha', 147, 148, 149)
Leftfoot = Foot('Izquierdo', 138, 139, 140)
Rightfoot = Foot('Derecho', 150, 151, 152)
#tronco superior
LeftArm = Arm('Izquierdo', 21, 22, 23)
RightArm = Arm('Derecho', 78, 79, 80)
LeftForeArm = ForeArm('Izquierdo', 24, 25, 26)
RightForeArm = ForeArm('Derecho', 81, 82, 83)
LeftHand = Wrist('Izquierda', 27, 28, 29)
RightHand = Wrist('Derecha', 84, 85, 86)
Spine1 = Spine('lumbar-toracica', 9, 10, 11)
Neck = Spine('Cervical', 12, 13, 14)
#-------------------FIN DE ESTRUCTURAS DE DATOS Y OBJETOS------------------
#Archivos de entrada (BVHfile) y salida (outputBVH)
BVHfile = open(sys.argv[1], 'r')
outputBVH = open(sys.argv[2], 'w+')
from Arm import Arm arm = Arm() arm.release() #arm.grap()
from Arm import Arm
from TTTArmV3 import TTTGame
from GRQDARMV2 import GRQDGame
from google.cloud import texttospeech
from pygame import mixer
from google.assistant.library import Assistant
from google.assistant.library.event import EventType
from google.assistant.library.file_helpers import existing_file
DEVICE_API_URL = 'https://embeddedassistant.googleapis.com/v1alpha2'
client = texttospeech.TextToSpeechClient()
arm = Arm("/dev/ttyACM0", True)
arm.aWrite("N")
ttt = TTTGame(arm)
grqd = GRQDGame(arm)
guesses = 4
game = "none"
voice = texttospeech.types.VoiceSelectionParams(language_code='en-US', ssml_gender=texttospeech.enums.SsmlVoiceGender.FEMALE)
audio_config = texttospeech.types.AudioConfig(audio_encoding=texttospeech.enums.AudioEncoding.MP3)
mixer.init()
def play_audio(toSay):
response = client.synthesize_speech(toSay, voice, audio_config)
mixer.music.load(response.audio_content)
mixer.music.play()
class ArmSolver():
def __init__(self, arm1Len = 3, arm2Len = 2, arm3Len = 1, goalPoint = {'x' :1, 'y' : 1 , 'z' : 1 }):
self.exactitudeParams = (0.05)
self.arm = Arm(arm1Len, arm2Len, arm3Len)
self.goalPoint = goalPoint
self.gs = GeneticSolver(arm1Len, arm2Len, arm3Len, goalPoint)
if not self.isAPossibleShot():
print('Goal point is too far ')
# This is the method that creates all population and try to get a result
#Family1 have a bias
def getGeneticResult(self, generations, familyPopulation = 10):
family1 = self.getNewGeneration(familyPopulation)
family2 = self.getNewGeneration(familyPopulation)
#New generation combined can have mutations in a grade of 10% of cases
for generation in range(generations):
print('Generation : {}'.format(generation))
solution = self.existASolution( family1, family2 ) #TOOD
if not solution:
family1 = self.newGenerationCombined(family1, family2, familyPopulation)
#print('Family 2 : {}'.format(family2))
family2 = self.mutantAddition(family2, family1)
#print('Mutation! : {}'.format(family2))
if ( generation % 5 ) == 0 :
#print(' $$$$$$$$$$$$$ New generation random addition')
family2 = self.newIndividualsAddition(family2, familyPopulation)
else:
print(solution)
return solution
def newIndividualsAddition(self, family, familyPopulation):
keys = sorted(family.keys())
preservedMembers = int(familyPopulation / 2)
bestResults = keys[:preservedMembers]
newMembers = self.getNewGeneration(familyPopulation)
return self.newGenerationCombined(family, newMembers, familyPopulation)
##Exactitude params for a satisfactory solution
def existASolution(self, family1, family2):
listResults = list(family1.keys()) + list(family2.keys())
#print(listResults)
results = sorted(listResults)
print('Aproximacion : {}'.format(results[0]))
if results[0] < self.exactitudeParams and results[0] > self.exactitudeParams * -1:
if results[0] in family1:
return family1[results[0]]
else :
return family2[results[0]]
return False
#MutantAddition generates mutants by each family and only choose the
#the most capable mutants, probably some mutants are superior of
# current families or worst ...
def mutantAddition(self, familyReceiber, familyColaborative):
mutantGeneration = {}
#print('Mutant Addition')
#print('family Receiber : {}'.format(familyReceiber))
#print('family Colaborative : {}'.format(familyColaborative))
for familyMember1, familyMember2 in zip(familyReceiber, familyColaborative): ##Moving on arm evaluations
mutant1 = {}
mutant2 = {}
#{'arm1' : {'gama' : <>, 'theta' : <>}}
for armM1, armM2 in zip(familyReceiber[familyMember1][0], familyColaborative[familyMember2][0]):
mutantArm1 = self.mutantArm(familyReceiber[familyMember1][0][armM1])
mutantArm2 = self.mutantArm(familyColaborative[familyMember2][0][armM2])
mutant1.update({ armM1 : mutantArm1 })
mutant2.update({ armM2 : mutantArm2 })
#print('mutant1 : {}'.format(mutant1))
eval1 = self.evaluateAnglesGroup(mutant1['arm1'], mutant1['arm2'], mutant1['arm2'])
eval2 = self.evaluateAnglesGroup(mutant2['arm1'], mutant2['arm2'], mutant2['arm2'])
mutantGeneration.update({ eval1 : mutant1 })
mutantGeneration.update({ eval2 : mutant2 })
bestMutants = sorted(mutantGeneration.keys())
bestFamilyOriginals = sorted(familyReceiber.keys())
receiberFamilyNumMembers = len(familyReceiber.keys())
familyPreservedMembers = int(receiberFamilyNumMembers / 2)
newReceptorFamily = {}
familyMemberIndex = 0
mutantElementIndex = 0
for member in range(0, receiberFamilyNumMembers):
if member < familyPreservedMembers:
#print('Best familie members {}'.format(familyReceiber[bestFamilyOriginals[familyMemberIndex]]))
newReceptorFamily.update({bestFamilyOriginals[familyMemberIndex] : familyReceiber[bestFamilyOriginals[familyMemberIndex]]})
familyMemberIndex = familyMemberIndex + 1
else :
#print('Best mutants : {}'.format(bestMutants))
newReceptorFamily.update({bestMutants[mutantElementIndex] : [mutantGeneration[bestMutants[mutantElementIndex]]]})
#print('Best Mutant additions : {}'.format({bestMutants[mutantElementIndex] : [mutantGeneration[bestMutants[mutantElementIndex]]]}))
mutantElementIndex = mutantElementIndex + 1
familyReceiber = newReceptorFamily
return familyReceiber
def mutantArm(self, armObject):
nArm = {}
nGama = self.gs.mutantAngle(armObject['gama'], 180)## Z angle
nTheta = self.gs.mutantAngle(armObject['theta'])
nArm.update( { 'gama' : nGama , 'theta' : nTheta } )
return nArm
def newGenerationCombined(self, family1, family2, familyPopulation):
bestIndividuals = {}
allKeys = list(family1.keys()) + list(family2.keys())
sortedBestKeys = sorted(allKeys)
familyMembers = 0
for bestKey in sortedBestKeys:
if bestKey in family1:
bestIndividuals.update( { bestKey : family1[bestKey] } )
else:
bestIndividuals.update( { bestKey : family2[bestKey] } )
if familyMembers >= familyPopulation:
break
return bestIndividuals
def getPossibleSolution(self, family):
bestFamilySolution = sorted(family.keys())[1]
if bestFamilySolution == 0 :
return family[bestFamilySolution]
else:
return False
#generationColl {<evaluation> : [{'arm1': {'gama' :<gamaAngle>, 'theta' : <theta> }, 'arm2' : ...}]}
def getNewGeneration(self, generationZise = 10):
nGenerationAnglesL1 = self.gs.generateNewGenGeneration(generationZise)
nGenerationAnglesL2 = self.gs.generateNewGenGeneration(generationZise)
nGenerationAnglesL3 = self.gs.generateNewGenGeneration(generationZise)
generationColl = {}
for anglesArm1, anglesArm2, anglesArm3 in zip(nGenerationAnglesL1, nGenerationAnglesL2, nGenerationAnglesL3):
evaluation = self.evaluateAnglesGroup(anglesArm1, anglesArm2, anglesArm3)
angles = {'arm1' : anglesArm1, 'arm2' : anglesArm2, 'arm3' : anglesArm3}
if evaluation in generationColl:
generationColl[evaluation].append(angles)
else:
generationColl.update( { evaluation : [angles] } )
return generationColl
def getCrosses(self, generationColl, generationColl2):
bestGen1 = sorted(generationColl.keys())
bestGen2 = sorted(generationColl2.keys())
nChilds = {}
for parent, child in bestGen1, bestGen2:
for pArm, cArm in bestGen1[parent], bestGen2[child]:
nArm = self.combineArmStatus(pArm, cArm)
evaluation = self.evaluateAnglesGroup(nArm['arm1'], nArm['arm2'], nArm['arm3'])
if evaluation in nChilds:
nChilds[evaluation].append(nArm)
else:
nChilds.update({evaluation : [nArm] } )
return nChilds
# pArm {'arm1' : {'alpha' : <alpha>, 'theta' : <theta> }, 'arm2' : {'alpha' : <alpha>, 'theta' : <theta> } , 'arm3' : {'alpha' : <alpha>, 'theta' : <theta> } }
def combineArmStatus(self, pArm, cArm):
combinedResult = {}
for armKey in pArm:
for angleKey in pArm[pKeys]:
# { 'armX' : { 'alpha' : <crossAngles> } }
combinedResult.update( { armKey : { angleKey : gs.crossAngles( pArm[armKey][angleKey], pArm[armKey][angleKey] ) } } )
return combinedResult
def evaluateAnglesGroup(self, anglesArm1, anglesArm2, anglesArm3):
self.arm.actualizeArm(anglesArm1, anglesArm2, anglesArm3)
distanceToGoal = self.pointsDistance(self.goalPoint, self.arm.getArmFinalPoint())
return distanceToGoal
def pointsDistance(self, point1, point2):
pointDistance = (point1['x'] - point2['x']) ** 2 + (point1['y'] + point2['y']) ** 2 + (point1['z'] + point2['z']) ** 2
pointDistance = math.sqrt(pointDistance)
return pointDistance
def isAPossibleShot(self):
pointDistance = self.goalPoint['x'] ** 2 + self.goalPoint['y'] ** 2 + self.goalPoint['z'] ** 2
pointDistance = math.sqrt(pointDistance)
if pointDistance > self.arm.maximumReach :
return False
return True
GRQDGame.xCoords.append([int(x) for x in xStrokes[i].split(',')])
GRQDGame.yCoords.append([int(y) for y in yStrokes[i].split(',')])
def play(self):
GRQDGame.chooseObjects(self)
# Change to audio prompts
print('This game reverses the role of Google Quick Draw.')
print(
'You have 5 seconds to decide which option the drawing best represents.'
)
print('Options: ')
print(', '.join(GRQDGame.choices))
GRQDGame.generateCoordinates(self)
print(GRQDGame.xCoords)
print(GRQDGame.yCoords)
GRQDGame.drawObject(self, GRQDGame.xCoords, GRQDGame.yCoords)
answer = input('Which option is it?:\n')
if GRQDGame.choices[int(answer) - 1] == GRQDGame.item:
print('You are correct!')
else:
print('Sorry the correct answer is:' + GRQDGame.item)
if __name__ == '__main__':
arm = Arm('COM6', False)
time.sleep(2)
arm.aWrite('N')
g = GRQDGame(arm)
while True:
g.play()
pwm = PCA9685()
pwm.reset()
pwm.wakeup()
#########################
# setup the mech arm pwm channel
#########################
motor_set_dic = {
"stage": 15,
"shoulder": 14,
"elbow": 13,
"wrist": 12,
"wrist_twist": 11,
"finger": 10,
}
arm = Arm(pwm, motor_set_dic)
#########################
# setup the Joystick
#########################
joystick = joystick_usb()
###########################
# setup the LCM module
# print the motor position
###########################
mylcd = LCM1602(debug_mode=False)
#########################
from Arm import Arm import time # =========================================================================== # Test # =========================================================================== # Initialise the PWM device using the default address # bmp = Arm(debug=False) Arm = Arm() SERVO_MIN = 150 # Min pulse length out of 4096 SERVO_MAX = 650 # Max pulse length out of 4096 SERVO_CENTER = 375 # Center value for the servo, should be 0 degrees of rotation. while (True): Arm.setBase(0) time.sleep(1) Arm.setBase(90) time.sleep(1) Arm.setBase(180) time.sleep(1)
def __init__(self, art1Len, art2Len, art3Len, goalPoint):
self.arm = Arm(art1Len, art2Len, art3Len)
self.goalPoint = goalPoint
arm_close = False
arm_open = True
else:
arm_open = False
def callback_arm_claw_close(msg):
global arm_close, arm_open
if msg.data:
arm_close = True
arm_open = False
else:
arm_close = False
arm = Arm(ids, offsets)
controller = ArmPositionController(arm, rotation_motor)
reboot = False
torque = True
delta_x = 0
delta_y = 0
delta_rotation = 0
rotation = 180
position = (0, 0)
arm_open = False
arm_close = False
rospy.init_node('dynamixel_arm')
rospy.Subscriber("dynamixel_arm_estop", Bool, callback_arm_estop)
rospy.Subscriber("dynamixel_arm_x_joystick", Vector3, callback_arm_x_joystick)
def reset(self):
self.sampled_priors = np.zeros(self.num_arms)
self.arms = [Arm() for i in range(self.num_arms)]
self.final_values = {}
from numpy.random import binomial, randint
from Arm import Arm
def __calc_success_ratio(arm):
if arm.success + arm.fail == 0:
return 0
return arm.success / (arm.success + arm.fail)
def epsilon_greedy(arms, T, epsilon):
reward = 0
for i in range(1, T + 1):
if binomial(n=1, p=epsilon) == 1:
# 探索ステップ : アームを一様ランダムに選ぶ
index = randint(0, len(arms))
else:
# 活用ステップ : 今までで一番成功確率の高いアームを選ぶ
avgs = [__calc_success_ratio(arm) for arm in arms]
index = avgs.index(max(avgs))
reward += arms[index].play()
return reward
if __name__ == "__main__":
arms = [Arm(0.3) for i in range(4)]
arms.append(Arm(0.5))
epsilon_greedy(arms=arms, T=10**3, epsilon=0.3)
import google.auth.transport.requests
import google.oauth2.credentials
import time
from Arm import Arm
from TTT import TTTGame
from GRQD import GRQDGame
from pygame import mixer
from google.assistant.library import Assistant
from google.assistant.library.event import EventType
from google.assistant.library.file_helpers import existing_file
arm = Arm("/dev/ttyACM0", False) #Creates Arm object
time.sleep(5) #Sleeps for 5 seconds to verify start
arm.aWrite("N") #Sends the start serial command
ttt = TTTGame(arm)
grqd = GRQDGame(arm)
guesses = 4
game = "none" #Variable to hold type of game being played.
# Function: play_audio(toSay);
# Takes string "toSay" and parses with Google Cloud Text-to-Speech API and provides
# spoken result of string. FUNCTIONALITY TO BE IMPLEMENTED.
def play_audio(toSay):
#Function to control TextToSpeech (using new Google Cloud Text-to-Speech API)
print(toSay)