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_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=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
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()
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()
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():
############################################### SETUP PARAMETERS ############################################
rod_length = 500 # rod length from center of motor to the center of the bearing in meters
max_torque = 4 # range of commanded torques will be from zero to this value
step_count = 20 # how many measurements you want to do
ramp_up_time = 0.2 # no to create a hard hit torque will ramp up to the max value over this amount of time
hold_time = 1 # if no user input is provided motor will turn off after this time to prevent overheating
safety_max_velocity = 1 #if rotational velocity gets higher than this the machine stops commanding torque
direction_flip = True
###################################################################################################################
# don't know how to do it yet but we need functionality to redo a measurement
print("Welcome to Motor Cracker")
print(
"Please set all the settable parameters in code. You will be asked to perform a series of measurements. \n"
"To start a measurement you will click Enter. Than the motor will start exerting force on the scale, it will \n"
"quickly ramp it up (over ramp_up_time) instead of instantly turning it on to avoid a sudden hit. \n"
"It will hold torque for some time (hold_time) and than release. You need to read out the value shown by the \n"
"scale during the time when the motor holds (in grams) and input it into this program, and click enter to go \n"
"to next measurement. If you already read the value and want the motor to releace before hold_time passes - click Space\n"
)
# create multi-dim array by providing shape
results = numpy.empty(shape=(step_count + 1, 2), dtype='object')
results[0, 0] = 0
results[0, 1] = 0
if direction_flip: direction = -1
else: direction = 1
c = Controller(controller_ID=1)
for i in range(1, step_count + 1):
c.command_stop(
) #comand_stop cleares anny errors/faults in the controller
input("Start looking at the scale and click enter")
#wait for user to start the test
time.sleep(0.5) #to give user time to switch attention
torque_reached_in_this_cycle = (
max_torque / step_count
) * i #torque that will be commanded in this measurement step
#code to ramp up torque from 0 to torque_reached_in_this_cycle over ramp_up_time
time_before_ramp_start = time.time()
while time.time() <= time_before_ramp_start + ramp_up_time:
commanded_torque = ((time.time() - time_before_ramp_start) /
ramp_up_time) * torque_reached_in_this_cycle
measurement = c.set_torque(torque=direction * commanded_torque,
get_data=True,
print_data=False)
if abs(measurement[
MoteusReg.MOTEUS_REG_VELOCITY]) > safety_max_velocity:
c.command_stop(
) #this will disable the motor fi it reaches a velocity over safety_max_velocity
break
#code to hold the torque_reached_in_this_cycle over the hold_time
time_before_measurement = time.time()
while time.time() <= time_before_measurement + hold_time:
commanded_torque = torque_reached_in_this_cycle
measurement = c.set_torque(torque=direction * commanded_torque,
get_data=True,
print_data=False)
if abs(measurement[
MoteusReg.MOTEUS_REG_VELOCITY]) > safety_max_velocity:
c.command_stop(
) # this will disable the motor fi it reaches a velocity over safety_max_velocity
break
#if "user clicks space" : break
scale_redout = input(
"commanded " + str(int(torque_reached_in_this_cycle * 100) / 100) +
" Nm. \nPlease input the redout from the kitchen scale (in grams!) : "
)
real_torque = float(
scale_redout
) * rod_length / 100000 #calculation and unit conversion
# code to put those two values it in a new line in a CSV file
results[i, 0] = torque_reached_in_this_cycle
results[i, 1] = real_torque
time.sleep(
torque_reached_in_this_cycle * 2
) # wait between measurements to let the motor cool down to minimise the importance of thermal effects - this is not what we are measuring in this test
print(results)
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
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)