def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy,
period):
# Initialise encoder
Encoders.__init__(self,
BigEncoder,
SmallEncoders,
small_encoders_required=False)
# Initialise robot
Robot.__init__(self,
values,
positions,
ALProxy,
masses=False,
acc_required=False,
gyro_required=False)
self.order = [{
'algo': Nothing,
'duration': 20
}, {
'algo': Start,
'duration': 30
}, {
'algo': IncreaseQuarterPeriod,
'increasing': True,
'max_angle': 5.0
}, {
'algo': MaintainGoodBadKick,
'maintain_angle': 5,
'duration': 60
}, {
'algo': IncreaseQuarterPeriod,
'max_angle': 7.5
}, {
'algo': MaintainGoodBadKick,
'maintain_angle': 7.5,
'duration': 60
}, {
'algo': IncreaseQuarterPeriod,
'min_angle': 5,
'increasing': False
}, {
'algo': MaintainGoodBadKick,
'duration': 230,
'maintain_angle': 5
}]
#self.order = [{
#'algo': Nothing,
#'duration': 10
#},{
#'algo': MaintainFeedback,
#'maintain_angle': 7.5
#}]
for dictionary in self.order:
dictionary['period'] = period
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy, period):
# Initialise encoder
Encoders.__init__(self, BigEncoder, SmallEncoders)
# Initialise robot
Robot.__init__(self, values, positions, ALProxy, masses=False, acc_required=False, gyro_required=False)
self.order = [{
'algo': Nothing,
'duration': 5
},{
'algo': IncreaseQuarterPeriod,
'duration': 60}]
for dictionary in self.order:
dictionary['period'] = period
def move_torso(angle=1, percent_max_speed=0.4):
torso = {
'RHP': 1.0,
'LHP': 1.0,
'RSP': 0.480417754569,
'LSP': 0.0496083550914,
'RSR': 1.12532637076,
'LSR': -1.10966057441,
'RER': -2.13838120104,
'LER': 2.18263145891,
'REY': -0.258485639687,
'LEY': 0.853785900783,
'RWY': 0.167101827676,
'LWY': -0.180156657963
}
for joint in torso:
Robot.move_limbs(joint, angle * torso[joint] * 0.0174533,
percent_max_speed)
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy, period):
# Initialise encoder
Encoders.__init__(self, BigEncoder, SmallEncoders)
# Initialise robot
Robot.__init__(self, values, positions, ALProxy, masses=False, acc_required=False, gyro_required=False)
self.order = [{
'algo': Nothing,
'duration': 5.0
},{
'algo': MaintainGoodBadKick,
'maintain_angle': 10.0
}]
for dictionary in self.order:
dictionary['period'] = period
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy, period):
Encoders.__init__(self, BigEncoder, SmallEncoders, small_encoders_required=False)
Robot.__init__(self, values, positions, ALProxy, masses=False, acc_required=False, gyro_required=False)
self.order = [
{
'algo': Nothing,
'duration': 10
},{
'algo': Triple,
'duration': 30
},{
'algo': TripleIncrease,
'duration' : 30
},{
'algo': Main,
'duration': 30
}
]
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy,
period):
Encoders.__init__(self,
BigEncoder,
SmallEncoders,
small_encoders_required=False)
Robot.__init__(self,
values,
positions,
ALProxy,
masses=False,
acc_required=True,
gyro_required=False)
self.order = [{
'algo': Nothing,
'duration': 40
}, {
'algo': Acc,
'duration': 600
}]
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy,
period):
Encoders.__init__(self,
BigEncoder,
SmallEncoders,
small_encoders_required=False)
Robot.__init__(self,
values,
positions,
ALProxy,
masses=False,
acc_required=False,
gyro_required=False)
self.order = [{
'algo': Nothing_ML,
'duration': 10
}, {
'algo': Machine_Learning_Bool,
'duration': 600
}]
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy):
# Initialise encoder
Encoders.__init__(self, BigEncoder, SmallEncoders)
# Initialise robot
Robot.__init__(self, values, positions, ALProxy)
# These are the classes that all containing the function algorithm that will be run,
# this classes will be initialised one cycle before switching to the algorithm
self.increase1 = IncreaseQuarterPeriod
self.increase2 = Increase
self.start = Start
self.start = Nothing
self.maintain = MaintainConstant
self.order = [{
'algo': self.start,
'duration': 5.0
}, {
'algo': self.increase2,
'duration': 35.0,
'max_angle': 10.0,
'decrease': False
}]
def move_legs(angle, percent_max_speed=0.5):
legs = ['RKP', 'LKP']
for joint in legs:
Robot.move_limbs(joint, angle * 0.0174533, percent_max_speed)
def move_torso(angle, percent_max_speed=0.5):
for joint in torso_dict:
Robot.move_limbs(joint, angle * torso_dict[joint] * 0.0174533,
percent_max_speed)
import top_encoder.encoder_functions as BigEncoder
import pandas as pd
from robot_interface import Robot
from encoder_interface import Encoders
from naoqi import ALProxy
from positions_sym import positions
from limb_data_2020 import values
import time
path.insert(0, "../Training_functions")
import SmallEncoders
from ml_old import ML
ml = ML()
Robot = Robot(values, positions, ALProxy)
Encoders = Encoders(BigEncoder, SmallEncoders)
#creates dictionary of ratios required to move whole torso relative to hip movements
crunched_pos = positions['crunched']
extended_pos = positions['extended']
torso_list = {
'RHP', 'LHP', 'RSP', 'LSP', 'RSR', 'LSR', 'RER', 'LER', 'REY', 'LEY',
'RWY', 'LWY'
}
hips = extended_pos['RHP'] - crunched_pos['RHP']
torso_dict = {}
for joint in torso_list:
value = (extended_pos[joint] - crunched_pos[joint]) / hips
torso_dict[joint] = value
from sys import path
path.insert(0, "../Interface")
from robot_interface import Robot
from naoqi import ALProxy
from positions_sym import positions
from limb_data_2020 import values
Robot = Robot(values, positions, ALProxy, "192.168.1.3", 9559)
Robot.get_posture()
print Robot.positions['current']
def move_torso(angle=1, percent_max_speed=0.4):
torso = {
'RHP': 1.0,
'LHP': 1.0,
'RSP': 0.480417754569,
'LSP': 0.0496083550914,
'RSR': 1.12532637076,
'LSR': -1.10966057441,
'RER': -2.13838120104,
'LER': 2.18263145891,
'REY': -0.258485639687,
'LEY': 0.853785900783,
'RWY': 0.167101827676,
'LWY': -0.180156657963
}
for joint in torso:
Robot.move_limbs(joint, angle * torso[joint] * 0.0174533,
percent_max_speed)
import pandas as pd
import time
import numpy as np
from sys import path
path.insert(0, "../Interface")
from robot_interface import Robot
from naoqi import ALProxy
from positions_sym import positions
from limb_data_2020 import values
from torso_and_legs import torso_dict, torso_speed, legs_dict, legs_speed
Robot = Robot(values, positions, ALProxy)
calls = np.linspace(100, 1000, 10)
joint_calls = np.linspace(10, 100, 10)
##########
#Joint movement timing test
##########
times_JM = []
start_time_JM = time.time()
for i in range(10):
for j in range(10):
Robot.move_limbs('LKP', 0.0, 0.5)
times_JM.append(time.time() - start_time_JM)
print "joint movements done"
##########
#Get Posture timing test
##########
times_GP = []
start_time_GP = time.time()
def __init__(self, BigEncoder, SmallEncoders, values, positions, ALProxy,
period):
# Initialise encoder
Encoders.__init__(self, BigEncoder, SmallEncoders)
# Initialise robot
Robot.__init__(self,
values,
positions,
ALProxy,
masses=False,
acc_required=False,
gyro_required=False)
self.order = [{'algo': Nothing, 'duration': 30}]
#self.order = [{
#'algo': Nothing,
#'duration': 10
#},{
#'algo': MaintainFeedback,
#'duration': 90,
#'masses': True
#}]
#self.order = [{
#'algo': Nothing,
#'duration': 90
#}]
#self.order = [{
#'algo': Nothing,
#'duration': 10
#},
#self.order = [{
#'algo': Start,
#'duration': 25
#},{
#'algo': IncreaseQuarterPeriod,
#'max_angle': 15
#},
##'algo': DecreaseQuarterPeriod,
##'increasing': False,
##'min_angle': 10
##},{
##'algo': MaintainConstant,
##'duration': 45
#{
#'algo': IncreaseParametric,
#'duration': 60,
#'max_angle': 40
#},{
#'algo': DecreaseParametric,
#'duration': 60,
#'increasing': False,
#'min_angle': 5
#},{
#'algo': Nothing,
#'duration': 10
#}]
for dictionary in self.order:
dictionary['period'] = period
from sys import path
path.insert(0, "../Interface")
from robot_interface import Robot
from naoqi import ALProxy
from positions_sym import positions
from limb_data_2020 import values
Robot = Robot(values, positions, ALProxy, "192.168.1.3", 9559)
def move_torso(angle=1, percent_max_speed=0.4):
torso = {'RHP': 1.0, 'LHP': 1.0, 'RSP': 0.480417754569, 'LSP': 0.0496083550914, 'RSR': 1.12532637076, 'LSR': -1.10966057441, 'RER': -2.13838120104, 'LER': 2.18263145891, 'REY': -0.258485639687, 'LEY': 0.853785900783, 'RWY': 0.167101827676, 'LWY': -0.180156657963}
for joint in torso:
Robot.move_limbs(joint, angle*torso[joint]*0.0174533, percent_max_speed)
def move_legs(angle, percent_max_speed=0.4):
legs = ['RKP', 'LKP']
for joint in legs:
Robot.move_limbs(joint, angle*0.0174533, percent_max_speed)
while True:
key = raw_input("q = torso out\tw = torso in\to = legs in\tp = legs out\n")
if key == "q":
move_torso(5)
print "Torso Out\n"
elif key == "w":
move_torso(-5)
print "Torso In\n"
elif key == "o":
