def main():
controller_knee = Controller(controller_ID=1)
controller_hip = Controller(controller_ID=2)
controller_abad = Controller(controller_ID=3)
kinematics = Kinematics()
freq = 20
while True:
freq_measure_time = time.time()
response_data_knee = controller_knee.get_data()
robot_knee_rot_org = response_data_knee[MoteusReg.MOTEUS_REG_POSITION]
knee_deg = kinematics.rad_to_deg(
kinematics.robot_to_rad_for_knee(robot_knee_rot_org))
response_data_hip = controller_hip.get_data()
robot_hip_rot_org = response_data_hip[MoteusReg.MOTEUS_REG_POSITION]
hip_deg = kinematics.rad_to_deg(
kinematics.robot_to_rad_for_hip(robot_hip_rot_org))
response_data_abad = controller_abad.get_data()
robot_abad_rot_org = response_data_abad[MoteusReg.MOTEUS_REG_POSITION]
abad_deg = kinematics.rad_to_deg(
kinematics.robot_to_rad_for_abad(robot_abad_rot_org))
x, y, z = kinematics.fk(robot_knee_rot_org, robot_hip_rot_org,
robot_abad_rot_org)
robot_knee_rot_proc, robot_hip_rot_proc, robot_abad_rot_proc = kinematics.ik(
x, y, z)
print(f'X: {x:.2f}, Y: {y:.2f}, Z: {z:.2f}')
#print(f'knee: {knee_deg:.2f}, hip: {hip_deg:.2f}, abad: {abad_deg:.2f} ')
# print(f'knee: {kinematics.rad_to_deg(kinematics.robot_to_rad_for_knee(robot_knee_rot_org)):.2f} / '
# f'{kinematics.rad_to_deg(kinematics.robot_to_rad_for_knee(robot_knee_rot_proc)):.2f}, '
# f'hip: {kinematics.rad_to_deg(kinematics.robot_to_rad_for_hip(robot_hip_rot_org)):.2f} / '
# f'{kinematics.rad_to_deg(kinematics.robot_to_rad_for_hip(robot_hip_rot_proc)):.2f}, '
# f'abad: {kinematics.rad_to_deg(kinematics.robot_to_rad_for_abad(robot_abad_rot_org)):.2f} / '
# f'{kinematics.rad_to_deg(kinematics.robot_to_rad_for_abad(robot_abad_rot_proc)):.2f}')
# print(kinematics.if_ik_possible(x, z))
# robot_hip_rot_processed, robot_knee_rot_processed = kinematics.ik(x, z)
#
# knee_deg_processed = kinematics.rad_to_deg(kinematics.robot_to_rad_for_knee(robot_knee_rot_processed))
# hip_deg_processed = kinematics.rad_to_deg(kinematics.robot_to_rad_for_hip(robot_hip_rot_processed))
#
# print(f'knee: {knee_deg_org:.2f} / {knee_deg_processed:.2f}, hip: {hip_deg_org:.2f} / {hip_deg_processed:.2f}, X: {x:.2f}, Z: {z:.2f}')
sleep = (1 / freq) - (time.time() - freq_measure_time)
if sleep < 0: sleep = 0
time.sleep(sleep)
def main():
controller_knee = Controller(controller_ID=1)
controller_hip = Controller(controller_ID=2)
controller_abad = Controller(controller_ID=3)
response_data_knee = controller_knee.get_data()
response_data_hip = controller_hip.get_data()
response_data_abad = controller_abad.get_data()
voltage = (response_data_knee[MoteusReg.MOTEUS_REG_V] +
response_data_hip[MoteusReg.MOTEUS_REG_V] +
response_data_abad[MoteusReg.MOTEUS_REG_V]) / 6
percentage = (voltage - 3.2 * 8) / (8 * (4.2 - 3.2))
print(percentage)
def main():
controller_1 = Controller(controller_ID = 2)
freq=300
devider = 80
while True:
freq_measure_time = time.time()
response_data_c1=controller_1.get_data()
pos_deg_c1 = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]*360
pos_set_c1=(pos_deg_c1-((pos_deg_c1)%(360/devider)-(360/devider/2)))
controller_1.set_position(position=pos_set_c1 / 360, max_torque=0.6, kd_scale=0., kp_scale=1)
sleep = (1/(freq))-(time.time()-freq_measure_time)
if(sleep<0): sleep = 0
time.sleep(sleep)
def main():
controller_knee = Controller(controller_ID=1)
controller_hip = Controller(controller_ID=2)
controller_abad = Controller(controller_ID=3)
kinematics = Kinematics()
freq = 200
period = 60 / 173
jump_duration = 0.1
idle_duration = 0.1 # 0.35
jump_torque = 2 #2
land_torque = 1.5 #1.2
jump_stance_low = 220
jump_stance_high = 290
retract_duration = period - jump_duration - idle_duration
begin_time = time.time()
x = 0
y = 0
x_rand_prev = 0
y_rand_prev = 0
i = 0
while i < 100:
i = i + 1
knee, hip, abad = kinematics.ik(0, 58, 200)
controller_knee.set_position(position=knee,
max_torque=jump_torque,
kd_scale=0.8,
kp_scale=0.7)
controller_hip.set_position(position=hip,
max_torque=jump_torque,
kd_scale=0.8,
kp_scale=0.7)
controller_abad.set_position(position=abad,
max_torque=jump_torque,
kd_scale=0.8,
kp_scale=0.7)
time.sleep(0.01)
while True:
while True:
y_rand = 58 #50*math.cos(time.time()/0.8) +48#random.randrange(58-30, 58+30)
x_rand = 0 #60*math.sin(time.time()/0.8) - 30 # random.randrange(-41, -40)
controller_knee.get_data(print_data=True)
controller_hip.get_data(print_data=True)
print(x_rand, y_rand)
print(" ")
break
'''
if ((y_rand-y_rand_prev)**2+(x_rand-x_rand_prev)**2)**(1/2)>60:
print(f't: {time.time():.2f} x: {x_rand:.2f} x_prev {x_rand_prev:.2f} y: {x_rand:.2f} y_prev {x_rand_prev:.2f}')
break'''
phase = 0
#print('NEW')
while phase < (period - 1 / freq):
#print(phase)
freq_measure_time = time.time()
phase = (time.time() - begin_time) % (period)
if phase <= jump_duration:
jump_phase = phase / jump_duration
x = x_rand_prev
y = y_rand_prev
z = (jump_stance_low +
(30 / 150 * (128 - y))) + ((jump_stance_high - 40 / 150 *
((y**2 + x**2)**(1 / 2))) -
(jump_stance_low +
(30 / 150 *
(128 - y)))) * jump_phase
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=jump_torque,
kd_scale=0.01,
kp_scale=1)
controller_hip.set_position(position=hip,
max_torque=jump_torque,
kd_scale=0.01,
kp_scale=1)
controller_abad.set_position(position=abad,
max_torque=jump_torque,
kd_scale=0.01,
kp_scale=1)
if (period - retract_duration) > phase >= jump_duration:
idle_phase = (
phase - jump_duration) / idle_duration # normalize to 0-1
x = x_rand_prev + (x_rand - x_rand_prev) * idle_phase
y = y_rand_prev + (y_rand - y_rand_prev) * idle_phase
z = (jump_stance_high - 40 / 150 *
(((y_rand)**2 + x_rand**2)**(1 / 2)))
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=jump_torque,
kd_scale=0.1,
kp_scale=1)
controller_hip.set_position(position=hip,
max_torque=jump_torque,
kd_scale=0.1,
kp_scale=1)
controller_abad.set_position(position=abad,
max_torque=jump_torque,
kd_scale=0.5,
kp_scale=1)
if phase > (period - retract_duration):
retract_phase = (phase - jump_duration - idle_duration
) / retract_duration # normalize to 0-1
x = x_rand
y = y_rand
z = (jump_stance_high - 40 / 150 *
(((y_rand)**2 + x_rand**2)**(1 / 2))) - (
(jump_stance_high - 40 / 150 *
(((y_rand)**2 + x_rand**2)**(1 / 2))) -
(jump_stance_low + (30 / 150 *
(128 - y)))) * retract_phase
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=land_torque,
kd_scale=0.8,
kp_scale=0.4)
controller_hip.set_position(position=hip,
max_torque=land_torque,
kd_scale=0.8,
kp_scale=0.4)
controller_abad.set_position(position=abad,
max_torque=land_torque,
kd_scale=0.8,
kp_scale=1)
sleep = (1 / freq) - (time.time() - freq_measure_time)
if sleep < 0: sleep = 0
time.sleep(sleep)
#print(f'freq: {1 / (time.time() - freq_measure_time):.1f}')
x_rand_prev = x_rand
y_rand_prev = y_rand
def main():
controller_1 = Controller(controller_ID=1)
controller_2 = Controller(controller_ID=2)
freq=600
response_data_c1 = controller_1.get_data()
pos_deg_c1 = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]
vel_dps_c1 = response_data_c1[MoteusReg.MOTEUS_REG_VELOCITY]
response_data_c2 = controller_2.get_data()
pos_deg_c2 = response_data_c2[MoteusReg.MOTEUS_REG_POSITION]
vel_dps_c2 = response_data_c2[MoteusReg.MOTEUS_REG_VELOCITY]
filter_pos = 0.1
filter_vel = 5# 4
multiplyer = 1
kp_scale = 0.2
kd_scale = 1
max_torque = 1
response_data_c1 = controller_1.get_data()
response_data_c2 = controller_2.get_data()
while True:
# response_data_c1 = controller_1.get_data()
# pos_deg_c1 = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]*360
# vel_dps_c1 = response_data_c1[MoteusReg.MOTEUS_REG_VELOCITY] * 360
#
# response_data_c2 = controller_2.get_data()
# pos_deg_c2 = -response_data_c2[MoteusReg.MOTEUS_REG_POSITION] * 360-110
# vel_dps_c2 = -response_data_c2[MoteusReg.MOTEUS_REG_VELOCITY] * 360
#
#
# controller_1.set_position(position=(pos_deg_c2)/ 360, velocity=vel_dps_c2/360, max_torque=0.5, kd_scale=0.2, kp_scale=1)
# controller_2.set_position(position=(-pos_deg_c1-110) / 360, velocity=-vel_dps_c1/360, max_torque=0.5, kd_scale=0.2, kp_scale=1)
freq_measure_time = time.time()
pos_deg_c1 = (pos_deg_c1*filter_pos + response_data_c1[MoteusReg.MOTEUS_REG_POSITION])/(filter_pos+1)
vel_dps_c1 = (vel_dps_c1*filter_vel + response_data_c1[MoteusReg.MOTEUS_REG_VELOCITY])/(filter_vel+1)
pos_deg_c2 = (pos_deg_c2*filter_pos + response_data_c2[MoteusReg.MOTEUS_REG_POSITION])/(filter_pos+1)
vel_dps_c2 = (vel_dps_c2*filter_vel + response_data_c2[MoteusReg.MOTEUS_REG_VELOCITY])/(filter_vel+1)
response_data_c1 = controller_1.set_position(position=(pos_deg_c2 / multiplyer),
velocity=vel_dps_c2 / multiplyer, max_torque=max_torque,
kd_scale=kd_scale, kp_scale=kp_scale, get_data=True)
response_data_c2 = controller_2.set_position(position=(multiplyer * pos_deg_c1),
velocity=vel_dps_c1 * multiplyer, max_torque=max_torque,
kd_scale=kd_scale, kp_scale=kp_scale, get_data=True)
print(f'pos c1: {pos_deg_c1:.1f} pos c2: {pos_deg_c2:.1f}')
sleep = (1/(freq))-(time.time()-freq_measure_time)
if(sleep<0): sleep = 0
time.sleep(sleep)
def main():
controller_knee = Controller(controller_ID = 1)
controller_hip = Controller(controller_ID = 2)
controller_abad = Controller(controller_ID=3)
response_data_c1 = controller_knee.get_data()
robot_knee_rot = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]
response_data_c2 = controller_hip.get_data()
robot_hip_rot = response_data_c2[MoteusReg.MOTEUS_REG_POSITION]
kinematics = Kinematics(robot_knee_rot, robot_hip_rot)
freq=300
period =1
jump_duration = 0.1
idle_duration = 0.2 # 0.35
jump_torque = 2
land_torque = 1.4
jump_stance_low = 70
jump_stance_high = 280
x_pos = 0
retract_duration = period - jump_duration - idle_duration
begin_time=time.time()
while True:
freq_measure_time = time.time()
phase = (time.time()-begin_time+idle_duration+jump_duration) % (period)
if phase <= jump_duration:
jump_phase = phase/jump_duration
x = -x_pos
z = jump_stance_low + (jump_stance_high-jump_stance_low)*jump_phase
if kinematics.if_ik_possible(x, z):
robot_hip_rot, robot_knee_rot = kinematics.ik(x, z)
controller_hip.set_position(position=robot_hip_rot, max_torque=jump_torque, kd_scale=0.4, kp_scale=1)
controller_knee.set_position(position=robot_knee_rot, max_torque=jump_torque, kd_scale=0.4, kp_scale=1)
if (period - retract_duration) > phase >= jump_duration:
idle_phase = (phase - jump_duration) / idle_duration # normalize to 0-1
x = -x_pos-0*idle_phase
z = jump_stance_high
# z = jump_stance_high - 10 - 40 * idle_phase
# if idle_phase<0.25:
# x=-60*(idle_phase*4) -15
# elif idle_phase<0.5:
# x=60-120*(idle_phase*2-0.5) -15
# else:
# x=-60+60* (idle_phase * 4 - 3) -15
if kinematics.if_ik_possible(x, z):
robot_hip_rot, robot_knee_rot = kinematics.ik(x, z)
controller_hip.set_position(position=robot_hip_rot, max_torque=jump_torque, kd_scale=0.2, kp_scale=1)
controller_knee.set_position(position=robot_knee_rot, max_torque=jump_torque, kd_scale=0.2, kp_scale=1)
else:
print(x,z)
if phase > (period-retract_duration):
retract_phase = (phase - jump_duration-idle_duration) / retract_duration # normalize to 0-1
x = -x_pos + 0*(1-retract_phase)
z = jump_stance_high - (jump_stance_high-jump_stance_low) * retract_phase
if kinematics.if_ik_possible(x, z):
robot_hip_rot, robot_knee_rot = kinematics.ik(x, z)
controller_hip.set_position(position=robot_hip_rot, max_torque=land_torque, kd_scale=1, kp_scale=0.3)
controller_knee.set_position(position=robot_knee_rot, max_torque=land_torque, kd_scale=1, kp_scale=0.3)
# if kinematics.if_ik_possible(x, z):
# robot_hip_rot_calculated, robot_knee_rot_calculated = kinematics.ik(x, z)
#
# response_data_c1 = controller_knee.get_data()
# robot_knee_rot_measured = response_data_c1[MoteusReg.MOTEUS_REG_POSITION]
# response_data_c2 = controller_hip.get_data()
# robot_hip_rot_measured = response_data_c2[MoteusReg.MOTEUS_REG_POSITION]
#
# #print(robot_hip_rot_calculated, robot_hip_rot_measured, robot_knee_rot_calculated, robot_hip_rot_measured)
#
# controller_hip.set_position(position=robot_hip_rot_calculated, max_torque=1.5, kd_scale=0.8, kp_scale=1.2)
# controller_knee.set_position(position=robot_knee_rot_calculated, max_torque=1.5, kd_scale=0.8, kp_scale=1.2)
sleep = (1/freq)-(time.time()-freq_measure_time)
if sleep < 0: sleep = 0
time.sleep(sleep)