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)
