def main():
controller_1 = Controller(controller_ID=2)
# VitesseDeRotation = 0.5 #Vitesse de rotation (0.5 lent et 5 rapide)
CoupleMoteur = 0.5
position= 0
while True:
position = position + 0.01
controller_1.set_position(position=position, max_torque=CoupleMoteur)
time.sleep(0.01)
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_1 = Controller(controller_ID=1)
freq = 300
while True:
freq_measure_time = time.time()
phase = (time.time() * 1) % (2. * math.pi)
angle_deg = 100.0 / 360 * math.sin(phase)
velocity_dps = 100.0 / 360 * math.cos(phase)
controller_1.set_position(position=angle_deg,
max_torque=0.2,
kd_scale=0.2,
get_data=True,
print_data=False)
# controller_1.set_velocity(velocity=velocity_dps, max_torque=0.5)
# controller_1.set_torque(torque=torque_Nm)
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():
############################################# SETUP PARAMETERS ################################################
pull_duration = 0.07 # First part of the cycle - over this amount of time the motor pulls the cable with full Troque
release_duration = 0.15 # next part of the cycle - a gradual release starting at max torque ending at standby_torque
duty_cycle = 0.8 #duty cycle of the test
max_torque = 3 #maximum torque exerted by the motor
standbay_torque = 0.05 #between pulls the motor still exerts some torque - to keep the cable tight
###################################################################################################################
c = Controller(controller_ID=1) #create controller object
c.command_stop() #comand_stop cleares anny errors/faults in the controller
max_measured_velocity = 0 #this variable stores the max measured rotational velocity of the motor and is used for safety and can be observed as an erformance indicator
counter = 0 #if you had to interrupt testing and need to start again but not count from 0 - input the number starting number you want here.
time_zero = time.time()
previous_phase = 0 #variable used to detect switching from one cycle to the next
while True:
phase = (time.time() - time_zero) % (
duty_cycle
) #what is happening during the cycle depends on the phase value. Phase is in seconds
#below - the behaviour of the system is dependent on the value of phase - in different value ranges different behaviours are triggered
if phase < pull_duration:
#initiall full torque pull
measurements = c.set_position(
position=0,
max_torque=4,
ff_torque=max_torque,
kp_scale=0,
kd_scale=0,
get_data=True,
print_data=False
) #measurements are collected to extract velocity - which if too high stops the system (safety)
elif phase >= pull_duration and phase <= pull_duration + release_duration:
#gradual release
release_phase = (
(pull_duration + release_duration - phase) / release_duration
) #value normalized from 1 to 0.
torque = standbay_torque + (
max_torque - standbay_torque
) * release_phase #torque ges from max_torque to standby_torque
measurements = c.set_position(
position=0,
max_torque=4,
ff_torque=torque,
kp_scale=0,
kd_scale=0,
get_data=True,
print_data=False
) #measurements are collected to extract velocity - which if too high stops the system (safety)
else:
measurements = c.set_position(
position=0,
max_torque=4,
ff_torque=standbay_torque,
kp_scale=0,
kd_scale=0.1,
get_data=True,
print_data=False
) #measurements are collected to extract velocity - which if too high stops the system (safety)
if max_measured_velocity < abs(
measurements[MoteusReg.MOTEUS_REG_VELOCITY]): #for diagnostics
max_measured_velocity = abs(
measurements[MoteusReg.MOTEUS_REG_VELOCITY])
print("Max velocity that occured so far: " +
str(abs(measurements[MoteusReg.MOTEUS_REG_VELOCITY])))
if max_measured_velocity > 25: break
if (previous_phase > phase): #code for counting phase cycles
counter = counter + 1
print("Temperature = " +
str(measurements[MoteusReg.MOTEUS_REG_TEMP_C]) +
" Counter = " + str(counter))
previous_phase = phase
c.set_position(
position=0,
velocity=0,
max_torque=1,
ff_torque=0,
kp_scale=0,
kd_scale=1,
get_data=True,
print_data=False
) #if there is a break in the loop above - the velocity limit is exceeded - this command stops the rotr
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)
def main():
c = Controller(controller_ID=1)
pos = 0
while True:
pos = pos + 0.0003
c.set_position(position=0, max_torque=3, kp_scale=1, kd_scale=0.8)
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)