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)
kinematics = Kinematics()
freq = 200
while True:
freq_measure_time = time.time()
x_in, y_in = pyautogui.position()
MaxX = 80
MaxY = 100 + 58
MinX = -120
MinY = -100 + 58
y = (y_in - 1440) / 8 + 80 + 58
x = -x_in / 8 + 80
z = 250 # -y_in/10+280
# x = lim(x, MinX, MaxX)
# y = lim(y, MinY, MaxY)
print(x, y, z)
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=1,
kd_scale=0.4,
kp_scale=0.8)
controller_hip.set_position(position=hip,
max_torque=1,
kd_scale=0.4,
kp_scale=0.8)
controller_abad.set_position(position=abad,
max_torque=1,
kd_scale=0.4,
kp_scale=0.8)
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
while True:
freq_measure_time = time.time()
phase1 = (time.time() * 10) % (2 * math.pi)
phase2 = (time.time() * 0.5) % (2 * math.pi)
x = radius * 1.5 * math.cos(phase1) * math.cos(phase2) - 40
y = radius * math.cos(phase1) * math.sin(phase2) + 58
z = 220 + radius * math.sin(phase1)
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=torque,
kd_scale=0.5,
kp_scale=1)
controller_hip.set_position(position=hip,
max_torque=torque,
kd_scale=0.5,
kp_scale=1)
controller_abad.set_position(position=abad,
max_torque=torque,
kd_scale=0.5,
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_knee = Controller(controller_ID=1)
controller_hip = Controller(controller_ID=2)
controller_abad = Controller(controller_ID=3)
kinematics = Kinematics()
filter = 20
freq = 200
period = 1.5
robot_knee_torque = 0
robot_hip_torque = 0
robot_abad_torque = 0
homing_speed = 2
while 1:
freq_measure_time = time.time()
if (robot_hip_torque < 0.4):
response_data_hip = controller_hip.set_position(
position=math.nan,
velocity=homing_speed,
max_torque=0.5,
kd_scale=0.2,
get_data=True,
print_data=False)
robot_hip_rot_org = response_data_hip[
MoteusReg.MOTEUS_REG_POSITION]
robot_hip_torque = (response_data_hip[MoteusReg.MOTEUS_REG_TORQUE]
+ filter * robot_hip_torque) / (filter + 1)
else:
hip_home_offset = robot_hip_rot_org
print("hip:", hip_home_offset)
break
sleep = (1 / freq) - (time.time() - freq_measure_time)
if sleep < 0: sleep = 0
time.sleep(sleep)
while 1:
freq_measure_time = time.time()
if (robot_knee_torque > -0.4):
response_data_knee = controller_knee.set_position(
position=math.nan,
velocity=-homing_speed,
max_torque=0.5,
kd_scale=0.2,
get_data=True,
print_data=False)
controller_hip.set_position(position=math.nan,
velocity=0.1,
max_torque=0.8,
kd_scale=0.2)
robot_knee_rot_org = response_data_knee[
MoteusReg.MOTEUS_REG_POSITION]
robot_knee_torque = (
response_data_knee[MoteusReg.MOTEUS_REG_TORQUE] +
filter * robot_knee_torque) / (filter + 1)
else:
knee_home_offset = robot_knee_rot_org
print("knee:", knee_home_offset)
break
while 1:
freq_measure_time = time.time()
if (robot_abad_torque > -0.3):
response_data_abad = controller_abad.set_position(
position=math.nan,
velocity=-homing_speed,
max_torque=0.4,
kd_scale=0.2,
get_data=True,
print_data=False)
controller_hip.set_position(position=hip_home_offset - 1,
max_torque=0.5,
kd_scale=1)
controller_knee.set_position(position=math.nan,
velocity=math.nan,
max_torque=0.8,
kd_scale=0.2)
robot_abad_rot_org = response_data_abad[
MoteusReg.MOTEUS_REG_POSITION]
robot_abad_torque = (
response_data_abad[MoteusReg.MOTEUS_REG_TORQUE] +
filter * robot_abad_torque) / (filter + 1)
else:
abad_home_offset = robot_abad_rot_org
print("abad:", abad_home_offset)
break
while 1:
freq_measure_time = time.time()
controller_hip.set_position(position=hip_home_offset - 1.3,
max_torque=0.3,
kd_scale=1)
controller_knee.set_position(position=knee_home_offset + 0.19,
max_torque=0.3,
kd_scale=0.7)
controller_abad.set_position(position=abad_home_offset + 1.4,
max_torque=0.3,
kd_scale=0.7)
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 = 300
#PID
kd_scale_air = 0.3 #D
kp_scale_air = 1 #P
kd_scale_ground = 0.6 # D
kp_scale_ground = 0.6 # P
torque = 1
period = 0.5
#time proportins
ground = 15
ret = 4
lift = 8
descend = 10
#geometry - [mm]
direction_deg = 30
stride = 0
ground_z = 250
air_z = ground_z - 65
y_zero = 58
begin_time = time.time()
while True:
stride = math.sin(((time.time() * 0.5) % (2 * math.pi))) * 50 + 50
print(stride)
if stride >= 30:
sin_scaler = 0.76
else:
sin_scaler = stride / 45
start_x = 0.5 * stride
end_x = -stride + start_x
sum = ground + lift + ret + descend
ground_end = (ground / sum) * period
lift_end = ((ground + lift) / sum) * period
return_end = ((ground + lift + ret) / sum) * period
descend_end = ((ground + lift + ret + descend) / sum) * period
ground_duration = ground_end
lift_duration = lift_end - ground_end
return_duration = return_end - lift_end
descend_duration = descend_end - return_end
direction = direction_deg / 360 * math.pi * 2
freq_measure_time = time.time()
phase = (time.time() - begin_time + lift_duration +
ground_duration) % period
if phase <= ground_end:
ground_phase = phase / ground_duration
x = math.cos(direction) * (start_x +
(end_x - start_x) * ground_phase)
y = y_zero + math.sin(direction) * (
start_x + (end_x - start_x) * ground_phase)
z = ground_z + (ground_z - ground_z) * ground_phase
if kinematics.if_ik_possible(x, y, z):
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=torque,
kd_scale=kd_scale_ground,
kp_scale=kp_scale_ground)
controller_hip.set_position(position=hip,
max_torque=torque,
kd_scale=kd_scale_ground,
kp_scale=kp_scale_ground)
controller_abad.set_position(position=abad,
max_torque=torque,
kd_scale=kd_scale_ground,
kp_scale=kp_scale_ground)
else:
print(x, z)
if ground_end < phase <= lift_end:
lift_phase = (phase -
ground_end) / lift_duration # normalize to 0-1
x = math.cos(direction) * (end_x + (sin_scaler *
(ground_z - air_z)) *
(-math.sin(lift_phase * math.pi)))
y = y_zero + math.sin(direction) * (
end_x + (sin_scaler * (ground_z - air_z)) *
(-math.sin(lift_phase * math.pi)))
z = ground_z + (air_z - ground_z) * (
math.sin(lift_phase * math.pi - math.pi / 2) / 2 + 0.5)
if kinematics.if_ik_possible(x, y, z):
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(
position=knee,
max_torque=torque,
kd_scale=kd_scale_ground -
(kd_scale_ground - kd_scale_air) * lift_phase,
kp_scale=kp_scale_ground -
(kp_scale_ground - kp_scale_air) * lift_phase)
controller_hip.set_position(
position=hip,
max_torque=torque,
kd_scale=kd_scale_ground -
(kd_scale_ground - kd_scale_air) * lift_phase,
kp_scale=kp_scale_ground -
(kp_scale_ground - kp_scale_air) * lift_phase)
controller_abad.set_position(
position=abad,
max_torque=torque,
kd_scale=kd_scale_ground -
(kd_scale_ground - kd_scale_air) * lift_phase,
kp_scale=kp_scale_ground -
(kp_scale_ground - kp_scale_air) * lift_phase)
else:
print(x, z)
if lift_end < phase <= return_end:
return_phase = (phase -
lift_end) / return_duration # normalize to 0-1
x = math.cos(direction) * (end_x +
(start_x - end_x) * return_phase)
y = y_zero + math.sin(direction) * (
end_x + (start_x - end_x) * return_phase)
z = air_z
if kinematics.if_ik_possible(x, y, z):
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(position=knee,
max_torque=torque,
kd_scale=kd_scale_air,
kp_scale=kp_scale_air)
controller_hip.set_position(position=hip,
max_torque=torque,
kd_scale=kd_scale_air,
kp_scale=kp_scale_air)
controller_abad.set_position(position=abad,
max_torque=torque,
kd_scale=kd_scale_air,
kp_scale=kp_scale_air)
else:
print(x, z)
if return_end < phase <= descend_end:
descend_phase = (phase -
return_end) / descend_duration # normalize to 0-1
x = math.cos(direction) * (start_x + (sin_scaler *
(ground_z - air_z)) *
(math.sin(descend_phase * math.pi)))
y = y_zero + math.sin(direction) * (
start_x + (sin_scaler * (ground_z - air_z)) *
(math.sin(descend_phase * math.pi)))
z = air_z + (ground_z - air_z) * (
math.sin(descend_phase * math.pi - math.pi / 2) / 2 + 0.5)
if kinematics.if_ik_possible(x, y, z):
knee, hip, abad = kinematics.ik(x, y, z)
controller_knee.set_position(
position=knee,
max_torque=torque,
kd_scale=kd_scale_air +
(kd_scale_ground - kd_scale_air) * descend_phase,
kp_scale=kp_scale_air +
(kp_scale_ground - kp_scale_air) * descend_phase)
controller_hip.set_position(
position=hip,
max_torque=torque,
kd_scale=kd_scale_air +
(kd_scale_ground - kd_scale_air) * descend_phase,
kp_scale=kp_scale_air +
(kp_scale_ground - kp_scale_air) * descend_phase)
controller_abad.set_position(
position=abad,
max_torque=torque,
kd_scale=kd_scale_air +
(kd_scale_ground - kd_scale_air) * descend_phase,
kp_scale=kp_scale_air +
(kp_scale_ground - kp_scale_air) * descend_phase)
else:
print(x, z)
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
knee = math.nan
hip = math.nan
abad = math.nan
velocity_knee = 0
velocity_hip = 0
velocity_abad = 0
tq = 0
while True:
freq_measure_time = time.time()
x = 0
y = 0
z = 0
data_knee = controller_knee.set_position(position=knee,
velocity=velocity_knee / 2,
max_torque=1.5 * tq,
kd_scale=0.2 * tq,
kp_scale=1.4 * tq,
get_data=True)
data_hip = controller_hip.set_position(position=hip,
velocity=velocity_hip / 2,
max_torque=0.8 * tq,
kd_scale=0.2 * tq,
kp_scale=1.4 * tq,
get_data=True)
data_abad = controller_abad.set_position(position=abad,
velocity=velocity_abad / 2,
max_torque=0.8 * tq,
kd_scale=0.2 * tq,
kp_scale=1.4 * tq,
get_data=True)
tq = 0
x, y, z = kinematics.fk(data_knee[MoteusReg.MOTEUS_REG_POSITION],
data_hip[MoteusReg.MOTEUS_REG_POSITION],
data_abad[MoteusReg.MOTEUS_REG_POSITION])
if z > 240:
z = -((z - 240)) * 1.5 + z
tq = 1
if z < 160:
z = -((z - 160)) * 1.5 + z
tq = 1
if y > 108:
y = -((y - 108)) * 1.5 + y
tq = 1
if y < -8:
y = -((y + 8)) * 1.5 + y
tq = 1
if x > 80:
x = -((x - 80)) * 1.5 + x
tq = 1
if x < -100:
x = -((x + 100)) * 1.5 + x
tq = 1
knee, hip, abad = kinematics.ik(x, y, z)
velocity_knee = data_knee[MoteusReg.MOTEUS_REG_VELOCITY]
velocity_hip = data_hip[MoteusReg.MOTEUS_REG_VELOCITY]
velocity_abad = data_abad[MoteusReg.MOTEUS_REG_VELOCITY]
sleep = (1 / freq) - (time.time() - freq_measure_time)
if sleep < 0: sleep = 0
time.sleep(sleep)