├── Docs ├── .gitkeep ├── SpotMicro_Power_Supply.pdf └── Spotmicro_Inverse_Kinematics.pdf ├── Files ├── .gitkeep ├── Spot_Micro_Control_v01.py ├── Spotmicro_Animation_Library_v01.py ├── Spotmicro_Gravity_Center_Library_v01.py └── Spotmicro_Inverse_Kinematics_and_Position_Library_v01.py ├── Full Working Version ├── Spot_Micro_animation_029_pygame.py ├── Spotmicro_Animate_lib_009.py ├── Spotmicro_Gravity_Center_lib_007.py └── Spotmicro_lib_020.py ├── LICENSE ├── README.md ├── SpotMicro_Power_Supply.pdf └── Utility ├── .gitkeep └── Essai_Joystick_01.py /Docs/.gitkeep: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Docs/SpotMicro_Power_Supply.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avbotics/Spot-Micro-Control-and-Animation/f0bb0e6d876608a1c6486ae0e4a2deda0d00bfd6/Docs/SpotMicro_Power_Supply.pdf -------------------------------------------------------------------------------- /Docs/Spotmicro_Inverse_Kinematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avbotics/Spot-Micro-Control-and-Animation/f0bb0e6d876608a1c6486ae0e4a2deda0d00bfd6/Docs/Spotmicro_Inverse_Kinematics.pdf -------------------------------------------------------------------------------- /Files/.gitkeep: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Files/Spot_Micro_Control_v01.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | @author: Arnaud Villeneuve 6 | 7 | This file contains the main program to run the Spotmicro Controller 8 | 9 | """ 10 | 11 | 12 | from time import sleep, time 13 | from math import pi, sin, cos, atan, atan2, sqrt 14 | import numpy as np 15 | 16 | import pygame 17 | pygame.init() 18 | screen = pygame.display.set_mode((600, 600)) 19 | pygame.display.set_caption("SPOTMICRO") 20 | 21 | 22 | import Spotmicro_Inverse_Kinematics_and_Position_Library_v01 23 | Spot = Spotmicro_Inverse_Kinematics_and_Position_Library_v01.Spot() 24 | import Spotmicro_Gravity_Center_Library_v01 25 | SpotCG = Spotmicro_Gravity_Center_Library_v01.SpotCG() 26 | import Spotmicro_Animation_Library_v01 27 | SpotAnim = Spotmicro_Animation_Library_v01.SpotAnim() 28 | 29 | 30 | """ Walking parameters """ 31 | b_height = 200 32 | h_amp = 100# horizontal step length 33 | v_amp = 40 #vertical step length 34 | track = 58.09 35 | x_offset = 0 #body offset in x direction 36 | ra_longi = 30# body distance 37 | ra_lat = 30#20 38 | steering =200 #Initial steering radius (arbitrary) 39 | walking_direction = 90/180*pi #Initial steering angle (arbitrary) 40 | stepl = 0.125 #duration of leg lifting typically between 0.1 and 0.2 41 | 42 | Angle = [0, 0] 43 | 44 | center_x = steering*cos(walking_direction) #steering center x relative to body center 45 | center_y = steering*sin(walking_direction) #steering center y relative to body center 46 | cw =1 47 | 48 | """ Joystick Init """ 49 | 50 | pygame.joystick.init() 51 | joystick = pygame.joystick.Joystick(0) 52 | joystick.init() 53 | 54 | """ XBOX One controller settings """ 55 | """ use Essai_Joystick_01.py utility to find out the right parameters """ 56 | 57 | nj = 6 # Number of joysticks 58 | nb = 10 # number of buttons 59 | 60 | but_walk = 7 61 | but_sit = 2 62 | but_lie = 3 63 | but_twist = 1 64 | but_pee = 0 65 | but_move = 4 66 | but_anim = 5 67 | 68 | pos_frontrear = 4 69 | pos_leftright = 3 70 | pos_turn = 0 71 | pos_rightpaw = 5 72 | pos_leftpaw = 2 73 | 74 | 75 | 76 | 77 | joypos =np.zeros(6) #xbox one controller has 6 analog inputs 78 | joybut = np.zeros(10) #xbox one controller has 10 buttons 79 | 80 | continuer = True 81 | clock = pygame.time.Clock() 82 | t = 0 #Initializing timing/clock 83 | tstart = 1 #End of start sequence 84 | tstop = 1000 #Start of stop sequence by default 85 | tstep = 0.01 #Timing/clock step 86 | tstep1 = tstep 87 | 88 | distance =[] #distance to support polygon edge 89 | balance =[] #balance status (True or False) 90 | timing = [] #timing to plot distance 91 | 92 | 93 | Free = True #Spot is ready to receive new command 94 | sitting = False 95 | walking = False #walking sequence activation 96 | lying = False 97 | twisting = False 98 | pawing = False 99 | shifting = False 100 | peeing = False 101 | stop = False # walking stop sequence activation 102 | lock = False # locking key/button stroke as a "rebound filter" 103 | lockmouse = False 104 | mouseclick = False 105 | cw = 1 106 | walking_speed = 0 107 | walking_direction = 0 108 | steeering = 1e6 109 | module = 0 110 | joype = -1 # Initial joysick value for peeing 111 | joypar = -1 # Initial joysick value for pawing right 112 | joypal = -1 # Initial joysick value for pawing left 113 | 114 | Tcomp = 0.02 115 | 116 | x_spot = [0, x_offset, Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr,0,0,0] 117 | y_spot = [0,0,Spot.ylf+track, Spot.yrf-track, Spot.yrr-track, Spot.ylr+track,0,0,0] 118 | z_spot = [0,b_height,0,0,0,0,0,0,0] 119 | theta_spot = [0,0,0,0,0,0] 120 | 121 | """theta_spot = [x angle ground, y angle ground, z angle body in space, x angle body, y angle body, z angle body] """ 122 | 123 | stance = [True, True, True, True] # True = foot on the ground, False = Foot lifted 124 | 125 | #theta xyz of ground then theta xyz of frame/body 126 | pos_init = [-x_offset,track,-b_height,-x_offset,-track,-b_height,-x_offset,-track,-b_height,-x_offset,track,-b_height] 127 | 128 | thetalf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[0], pos_init[1], pos_init[2], 1)[0] 129 | thetarf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[3], pos_init[4], pos_init[5], -1)[0] 130 | thetarr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[6], pos_init[7], pos_init[8], -1)[0] 131 | thetalr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[9], pos_init[10], pos_init[11], 1)[0] 132 | 133 | CG = SpotCG.CG_calculation (thetalf,thetarf,thetarr,thetalr) 134 | #Calculation of CG absolute position 135 | M = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],theta_spot[2],False) 136 | CGabs = Spot.new_coordinates(M,CG[0],CG[1],CG[2],x_spot[1],y_spot[1],z_spot[1]) 137 | dCG = SpotCG.CG_distance(x_spot[2:6],y_spot[2:6],z_spot[2:6],CGabs[0],CGabs[1],stance) 138 | 139 | x_spot = [0, x_offset, Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr,CG[0],CGabs[0],dCG[1]] 140 | y_spot = [0,0,Spot.ylf+track, Spot.yrf-track, Spot.yrr-track, Spot.ylr+track,CG[1],CGabs[1],dCG[2]] 141 | z_spot = [0,b_height,0,0,0,0,CG[2],CGabs[2],dCG[3]] 142 | 143 | pos = [-x_offset,track,-b_height,-x_offset,-track,-b_height,-x_offset,-track,-b_height,-x_offset,track,-b_height,theta_spot,x_spot,y_spot,z_spot] 144 | 145 | 146 | """ 147 | Main Loop 148 | """ 149 | while (continuer): 150 | clock.tick(50) 151 | 152 | for event in pygame.event.get(): # User did something. 153 | if event.type == pygame.QUIT: # If user clicked close window. 154 | continuer = False 155 | if event.type == pygame.MOUSEBUTTONDOWN: 156 | mouseclick = True 157 | else: 158 | mouseclick = False 159 | 160 | for i in range (0,nj): #read analog joystick position 161 | joypos[i] = joystick.get_axis(i) 162 | for i in range (0,nb): #read buttons 163 | joybut[i] = joystick.get_button(i) 164 | joyhat = joystick.get_hat(0) #read hat 165 | 166 | """Animation""" 167 | 168 | if (joybut[but_walk] == 0)&(joybut[but_pee] == 0)&(joybut[but_twist] == 0)&(joybut[but_sit] == 0)&(joybut[but_lie] == 0)&(joybut[but_anim] == 0)&(joybut[but_move] == 0)&(lock == True): 169 | lock = False 170 | 171 | #WALKING 172 | if (joybut[but_walk] == 1)&(walking == True)&(stop == False)&(lock == False): #Quit walk mode 173 | stop = True 174 | lock = True 175 | if (abs(t-int(t))<=tstep): 176 | tstop = int(t) 177 | else: 178 | tstop = int(t)+1 179 | 180 | if (t==0): 181 | tstop = 1 182 | 183 | if (joybut[but_walk] == 1)&(walking == False)&(Free == True): #Enter in walk mode 184 | walking = True 185 | stop = False 186 | Free = False 187 | t=0 188 | tstart = 1 189 | tstop = 1000 190 | lock = True 191 | trec = int(t) 192 | 193 | 194 | #SITTING and GIVING PAW 195 | if (joybut[but_sit] == 1)&(sitting == False)&(Free == True): #Enter in sitting mode 196 | sitting = True 197 | stop = False 198 | Free = False 199 | t=0 200 | lock = True 201 | 202 | 203 | if (joybut[but_sit] == 1)&(sitting == True)&(stop == False)&(lock == False): #Quit sitting mode 204 | stop = True 205 | lock = True 206 | 207 | 208 | #SHIFTING and PEEING 209 | if (joybut[but_pee] == 1)&(shifting == False)&(Free == True): #Enter in sitting mode 210 | shifting = True 211 | stop = False 212 | Free = False 213 | t=0 214 | lock = True 215 | 216 | 217 | if (joybut[but_pee] == 1)&(shifting == True)&(stop == False)&(lock == False): #Quit sitting mode 218 | stop = True 219 | lock = True 220 | 221 | 222 | #LYING 223 | if (joybut[but_lie] == 1)&(lying == False)&(Free == True): #Enter in sitting mode 224 | lying = True 225 | stop = False 226 | Free = False 227 | t=0 228 | lock = True 229 | 230 | 231 | if (joybut[but_lie] == 1)&(lying== True)&(stop == False)&(lock == False): #Quit sitting mode 232 | stop = True 233 | lock = True 234 | 235 | 236 | 237 | #TWISTING 238 | if (joybut[but_twist] == 1)&(twisting == False)&(Free == True): #Enter in sitting mode 239 | twisting = True 240 | Free = False 241 | t=0 242 | lock = True 243 | 244 | 245 | 246 | if (walking == True): 247 | coef = 1.2 248 | #set walking direction and speed 249 | #set steering radius 250 | 251 | if (joybut[but_move] == True)&(tstep > 0)&(lock == False): 252 | tstep = 0 253 | lock = True 254 | 255 | if (joybut[but_move] == True)&(tstep == 0)&(lock == False): 256 | tstep = tstep1 257 | lock = True 258 | 259 | print (tstep) 260 | 261 | if (abs(joypos[pos_leftright])>0.2)|(abs(joypos[pos_frontrear])>0.2)|(stop == True): 262 | t=t+tstep 263 | trec = int(t)+1 264 | 265 | module_old = module 266 | walking_direction_old = walking_direction 267 | steering_old = steering 268 | 269 | x_old = module_old*cos(walking_direction_old) 270 | y_old = module_old*sin(walking_direction_old) 271 | 272 | #update request 273 | module = sqrt(joypos[pos_leftright]**2 + joypos[pos_frontrear]**2) 274 | walking_direction = (atan2(-joypos[pos_leftright],-joypos[pos_frontrear])%(2*pi)+pi/2)%(2*pi) 275 | 276 | x_new = module*cos(walking_direction) 277 | y_new = module*sin(walking_direction) 278 | 279 | #steering update 280 | if (abs(joypos[pos_turn]) < 0.2): 281 | cw = 1 282 | if (steering<2000): 283 | steering = min(1e6,steering_old*coef) 284 | else: 285 | steering = 1e6 286 | else: 287 | steering = 2000-(abs(joypos[0])-0.2)*2000/0.8+0.001 288 | if ((steering/steering_old)>coef): 289 | steering = steering_old*coef 290 | if ((steering_old/steering)>coef): 291 | steering = steering_old/coef 292 | if (steering <0.001): 293 | steering = 0.001 294 | cw = -np.sign(joypos[0]) 295 | 296 | 297 | gap = sqrt((x_new-x_old)**2+(y_new-y_old)**2) 298 | 299 | if (gap>0.01): 300 | x_new = x_old+ (x_new-x_old)/gap*0.01 301 | y_new = y_old+ (y_new-y_old)/gap*0.01 302 | module = sqrt(x_new**2+y_new**2) 303 | walking_direction = atan2(y_new,x_new) 304 | 305 | #reduces speed sideways and backwards 306 | min_h_amp = h_amp*(1/2e6*steering+1/2) 307 | xa = 1+cos(walking_direction-pi/2) 308 | walking_speed = min (1, module) * min(h_amp,min_h_amp) * (1/8*xa**2+1/8*xa+1/4) 309 | 310 | 311 | if ((abs(joypos[pos_leftright])<0.2)&(abs(joypos[pos_frontrear])<0.2))&(stop == False): 312 | t=t+tstep 313 | module = max (0, module-0.01) 314 | walking_speed = module* h_amp * ((1+cos(walking_direction-pi/2))/2*0.75+0.25) 315 | if (steering<2000): 316 | steering = min(1e6,steering*coef) 317 | else: 318 | steering = 1e6 319 | cw=1 320 | if (t>trec): 321 | t=trec 322 | 323 | """ 324 | If you have an IMU that measures Angle[0] and Angle [1] 325 | values can be transferred to theta_spot 326 | """ 327 | theta_spot[3] = Angle [0] # angle around x axis 328 | theta_spot[4] = Angle [1] # angle around y axis 329 | theta_spot[0] = Angle [0] # angle around x axis 330 | theta_spot[1] = Angle [1] # angle around y axis 331 | 332 | if (t< tstart): 333 | pos = Spot.start_walk_stop (track,x_offset,steering,walking_direction,cw,walking_speed,v_amp,b_height,stepl,t,tstep,theta_spot,x_spot,y_spot,z_spot,'start') 334 | else: 335 | if (t(tstop+1-tstep)): 347 | stop = False 348 | walking = False 349 | Free = True 350 | 351 | if (sitting == True): 352 | alpha_sitting = -30/180*pi 353 | alpha_pawing = 0/180*pi 354 | L_paw = 220 355 | 356 | x_end_sitting = Spot.xlr-Spot.L2 + Spot.L1*cos(pi/3) +Spot.Lb/2*cos(-alpha_sitting) - Spot.d*sin (-alpha_sitting) 357 | z_end_sitting = Spot.L1*sin(pi/3)+ Spot.Lb/2*sin(-alpha_sitting) + Spot.d*cos(-alpha_sitting) 358 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 359 | 360 | #end_frame_pos = [0,0,0,x_offset,0,b_height-20] # x,y,z rotations then translations 361 | end_frame_pos = [0,alpha_sitting,0, x_end_sitting,0,z_end_sitting] # x,y,z rotations then translations 362 | pos = Spot.moving (t, start_frame_pos,end_frame_pos, pos) 363 | 364 | if (t==1)&(pawing == False): 365 | pos_sit_init = pos 366 | 367 | if (t == 1): #pawing is possible 368 | if (pawing == True): 369 | #print (pos_sit_init[3],pos_sit_init[5]) 370 | pos[3] = pos_sit_init[3]+ (L_paw*cos(alpha_pawing)-pos_sit_init[3])*(joypar+1)/2 371 | pos[5] = pos_sit_init[5]+ (-Spot.d-L_paw*sin(alpha_pawing)-pos_sit_init[5])*(joypar+1)/2 372 | 373 | pos[0] = pos_sit_init[0]+ (L_paw*cos(alpha_pawing)-pos_sit_init[0])*(joypal+1)/2 374 | pos[2] = pos_sit_init[2]+ (-Spot.d-L_paw*sin(alpha_pawing)-pos_sit_init[2])*(joypal+1)/2 375 | 376 | thetarf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[3], pos[4], pos[5], -1)[0] 377 | thetalf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[0], pos[1], pos[2], -1)[0] 378 | #update of right front leg absolute position 379 | legrf = Spot.FK(thetarf,-1) 380 | leglf = Spot.FK(thetalf,-1) 381 | xlegrf =Spot.xrf+pos[3] 382 | ylegrf =Spot.yrf+pos[4] 383 | zlegrf =pos[5] 384 | xleglf =Spot.xlf+pos[0] 385 | yleglf =Spot.ylf+pos[1] 386 | zleglf =pos[2] 387 | 388 | theta_spot_sit = pos[12] 389 | 390 | x_spot_sit = pos[13] 391 | y_spot_sit = pos[14] 392 | z_spot_sit = pos[15] 393 | 394 | M = Spot.xyz_rotation_matrix(theta_spot_sit[3],theta_spot_sit[4],theta_spot_sit[2]+theta_spot_sit[5],False) 395 | 396 | paw_rf = Spot.new_coordinates(M,xlegrf,ylegrf,zlegrf,x_spot_sit[1],y_spot_sit[1],z_spot_sit[1]) 397 | paw_lf = Spot.new_coordinates(M,xleglf,yleglf,zleglf,x_spot_sit[1],y_spot_sit[1],z_spot_sit[1]) 398 | 399 | x_spot_sit[3] = paw_rf[0] 400 | y_spot_sit[3] = paw_rf[1] 401 | z_spot_sit[3] = paw_rf[2] 402 | x_spot_sit[2] = paw_lf[0] 403 | y_spot_sit[2] = paw_lf[1] 404 | z_spot_sit[2] = paw_lf[2] 405 | 406 | 407 | pos[13] = x_spot_sit 408 | pos[14] = y_spot_sit 409 | pos[15] = z_spot_sit 410 | 411 | joypar_old = joypar 412 | if (joypal == -1): 413 | if (((joypos[pos_rightpaw] != 0)&(joypos[pos_rightpaw] !=-1))|(joypar != -1)): 414 | pawing = True 415 | if (joypos[pos_rightpaw]>= joypar): 416 | joypar = min (joypos[pos_rightpaw],joypar+0.05) 417 | else: 418 | joypar = max (joypos[pos_rightpaw],joypar-0.05) 419 | else: 420 | pawing = False 421 | 422 | if (joypar_old == -1): 423 | if (((joypos[pos_leftpaw] != 0)&(joypos[pos_leftpaw] !=-1))|(joypal != -1)): 424 | pawing = True 425 | if (joypos[pos_leftpaw]>= joypal): 426 | joypal = min (joypos[pos_leftpaw],joypal+0.05) 427 | else: 428 | joypal = max (joypos[pos_leftpaw],joypal-0.05) 429 | else: 430 | pawing = False 431 | 432 | 433 | if (stop == False): 434 | t=t+4*tstep 435 | if (t>=1): 436 | t= 1 437 | elif (pawing == False): 438 | t=t-4*tstep 439 | if (t<= 0): 440 | t= 0 441 | stop = False 442 | sitting = False 443 | Free = True 444 | 445 | if (shifting == True): 446 | x_end_shifting = ra_longi 447 | y_end_shifting = -ra_lat 448 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 449 | end_frame_pos = [0,0,0, x_end_shifting+x_offset,y_end_shifting,b_height] # x,y,z rotations then translations 450 | pos = Spot.moving (t, start_frame_pos,end_frame_pos, pos) 451 | 452 | if (t==1)&(peeing == False): 453 | pos_shift_init = pos 454 | 455 | if (t == 1): #lifting left hid leg is possible 456 | 457 | if (peeing == True): 458 | pos[9] = pos_shift_init[9]+ (0-pos_shift_init[9])*(joype+1)/2 459 | pos[10] = pos_shift_init[10]+ (130-pos_shift_init[10])*(joype+1)/2 460 | pos[11] = pos_shift_init[11]+ (-20-pos_shift_init[11])*(joype+1)/2 461 | 462 | thetalr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[9], pos[10], pos[11], 1)[0] 463 | #update of left hind leg absolute position 464 | leglr = Spot.FK(thetalr,1) 465 | xleglr =Spot.xlr+pos[9] 466 | yleglr =Spot.ylr+pos[10] 467 | zleglr =pos[11] 468 | theta_spot_shift = pos[12] 469 | x_spot_shift = pos[13] 470 | y_spot_shift = pos[14] 471 | z_spot_shift = pos[15] 472 | M = Spot.xyz_rotation_matrix(theta_spot_shift[3],theta_spot_shift[4],theta_spot_shift[2]+theta_spot_shift[5],False) 473 | pee_lr = Spot.new_coordinates(M,xleglr,yleglr,zleglr,x_spot_shift[1],y_spot_shift[1],z_spot_shift[1]) 474 | 475 | x_spot_shift[5] = pee_lr[0] 476 | y_spot_shift[5] = pee_lr[1] 477 | z_spot_shift[5] = pee_lr[2] 478 | pos[13] = x_spot_shift 479 | pos[14] = y_spot_shift 480 | pos[15] = z_spot_shift 481 | 482 | if ((joypos[pos_leftpaw] != 0)&(joypos[pos_leftpaw] !=-1))|(joype != -1): 483 | peeing = True 484 | if (joypos[pos_leftpaw]>= joype): 485 | joype = min (joypos[pos_leftpaw],joype+0.1) 486 | else: 487 | joype = max (joypos[pos_leftpaw],joype-0.1) 488 | else: 489 | peeing = False 490 | 491 | 492 | 493 | 494 | if (stop == False): 495 | t=t+4*tstep 496 | if (t>=1): 497 | t= 1 498 | elif (peeing == False): 499 | t=t-4*tstep 500 | if (t<= 0): 501 | t= 0 502 | stop = False 503 | shifting = False 504 | Free = True 505 | 506 | 507 | if (lying == True): 508 | angle_lying = 40/180*pi 509 | x_end_lying= Spot.xlr-Spot.L2 + Spot.L1*cos(angle_lying)+Spot.Lb/2 510 | z_end_lying = Spot.L1*sin(angle_lying)+Spot.d 511 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 512 | end_frame_pos = [0,0,0, x_end_lying,0,z_end_lying] # x,y,z rotations then translations 513 | pos = Spot.moving (t, start_frame_pos,end_frame_pos, pos) 514 | if (stop == False): 515 | t=t+3*tstep 516 | if (t>=1): 517 | t= 1 518 | else: 519 | t=t-3*tstep 520 | if (t<= 0): 521 | t= 0 522 | stop = False 523 | lying = False 524 | Free = True 525 | 526 | if (twisting == True): 527 | x_angle_twisting = 0/180*pi 528 | y_angle_twisting = 0/180*pi 529 | z_angle_twisting = 30/180*pi 530 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 531 | 532 | t=t+4*tstep 533 | if (t>=1): 534 | t=1 535 | twisting = False 536 | Free = True 537 | 538 | if (t<0.25): 539 | end_frame_pos = [x_angle_twisting,y_angle_twisting,z_angle_twisting, x_offset,0,b_height] # x,y,z rotations then translations 540 | pos = Spot.moving (t*4, start_frame_pos,end_frame_pos, pos) 541 | if (t>=0.25)&(t<0.5): 542 | end_frame_pos = [x_angle_twisting,y_angle_twisting,z_angle_twisting, x_offset,0,b_height] # x,y,z rotations then translations 543 | pos = Spot.moving ((t-0.25)*4, end_frame_pos,start_frame_pos, pos) 544 | if (t>=0.5)&(t<0.75): 545 | end_frame_pos = [-x_angle_twisting,-y_angle_twisting,-z_angle_twisting, x_offset,0,b_height] 546 | pos = Spot.moving ((t-0.5)*4, start_frame_pos,end_frame_pos, pos) 547 | if (t>=0.75)&(t<=1): 548 | end_frame_pos = [-x_angle_twisting,-y_angle_twisting,-z_angle_twisting, x_offset,0,b_height] 549 | pos = Spot.moving ((t-0.75)*4, end_frame_pos,start_frame_pos, pos) 550 | 551 | 552 | xc = steering* cos(walking_direction) 553 | yc = steering* sin(walking_direction) 554 | 555 | center_x = x_spot[0]+(xc*cos(theta_spot[2])-yc*sin(theta_spot[2])) #absolute center x position 556 | center_y = y_spot[0]+(xc*sin(theta_spot[2])+yc*cos(theta_spot[2])) #absolute center y position 557 | 558 | 559 | 560 | thetalf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[0], pos[1], pos[2], 1)[0] 561 | thetarf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[3], pos[4], pos[5], -1)[0] 562 | thetarr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[6], pos[7], pos[8], -1)[0] 563 | thetalr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[9], pos[10], pos[11], 1)[0] 564 | 565 | """ 566 | ************************************************************************************************ 567 | 568 | thetalf, thetarf, thetarr, thetalr are the sets of angles thant can be sent to the servos 569 | to generate the motion of Spotmicro 570 | 571 | This is where you can place the call to the servo moving function 572 | Moving function depends on the type of servos and drivers that are used to control them 573 | -I2c shields, PWM generators 574 | -Servos maximum race 180°, 270°, 360°... 575 | 576 | Servos zero positions and races must be tuned 577 | 578 | ************************************************************************************************ 579 | """ 580 | 581 | 582 | 583 | stance = [False, False, False, False] 584 | if (pos[15][2] < 0.01): 585 | stance[0] = True 586 | if (pos[15][3] < 0.01): 587 | stance[1] = True 588 | if (pos[15][4] < 0.01): 589 | stance[2] = True 590 | if (pos[15][5] < 0.01): 591 | stance[3] = True 592 | 593 | 594 | SpotAnim.animate(pos,t,pi/12,-135/180*pi,Angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance) 595 | #SpotAnim.animate(pos,t,pi/2,-0/180*pi,Angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance) 596 | #SpotAnim.animate(pos,t,0,-0/180*pi,Angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance) 597 | 598 | pygame.display.flip() 599 | if (Free == True): 600 | sleep(0.1) 601 | 602 | """ CG update """ 603 | CG = SpotCG.CG_calculation (thetalf,thetarf,thetarr,thetalr) 604 | #Calculation of CG absolute position 605 | M = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],theta_spot[2],False) 606 | CGabs = Spot.new_coordinates(M,CG[0],CG[1],CG[2],x_spot[1],y_spot[1],z_spot[1]) 607 | dCG = SpotCG.CG_distance(x_spot[2:6],y_spot[2:6],z_spot[2:6],CGabs[0],CGabs[1],stance) 608 | 609 | 610 | pos[13][6] = CG[0] #x 611 | pos[14][6] = CG[1] #y 612 | pos[15][6] = CG[2] #z 613 | 614 | pos[13][7] = CGabs[0] #x 615 | pos[14][7] = CGabs[1] #y 616 | pos[15][7] = CGabs[2] #z 617 | 618 | pos[13][8] = dCG[1] #xint 619 | pos[14][8] = dCG[2] #yint 620 | pos[15][8] = dCG[3] #balance 621 | 622 | distance.append(dCG[0]) 623 | timing.append(t) 624 | 625 | pygame.quit() 626 | 627 | -------------------------------------------------------------------------------- /Files/Spotmicro_Animation_Library_v01.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | @author: Arnaud Villeneuve 5 | 6 | This file contains a class of functions to generate the animation frames 7 | 8 | """ 9 | 10 | from time import sleep, time 11 | from math import pi, sin, cos, atan, atan2, sqrt 12 | import numpy as np 13 | import pygame 14 | import Spotmicro_Inverse_Kinematics_and_Position_Library_v01 15 | Spot = Spotmicro_Inverse_Kinematics_and_Position_Library_v01.Spot() 16 | import Spotmicro_Gravity_Center_Library_v01 17 | SpotCG = Spotmicro_Gravity_Center_Library_v01.SpotCG() 18 | pygame.init() 19 | screen = pygame.display.set_mode((600, 600)) 20 | 21 | class SpotAnim: 22 | 23 | """Display colors """ 24 | BLACK = (0, 0, 0) 25 | WHITE = (255, 255, 255) 26 | BLUE = (24, 84, 231) 27 | GREEN = (51, 204, 51) 28 | RED = (255, 0, 0) 29 | GREY = (225, 225, 225) 30 | DARK_GREY=(100,100,100) 31 | DARK_RED =(176,0,0) 32 | VIOLET =(207,175,255) 33 | CYAN = (0,255,255) 34 | DARK_CYAN =(0,127,127) 35 | MED_GREY = (170,170,170) 36 | 37 | """Axis coordinates for animation""" 38 | xX=[0,100] 39 | yX=[0,0] 40 | zX=[0,0] 41 | 42 | xY=[0,0] 43 | yY=[0,100] 44 | zY=[0,0] 45 | 46 | xZ=[0,0] 47 | yZ=[0,0] 48 | zZ=[0,100] 49 | 50 | """Floor coordinates for animation""" 51 | xFloor = [500,500,-500,-500] 52 | yFloor = [500,-500,-500,500] 53 | zFloor = [0,0,0,0] 54 | 55 | 56 | conv = 0 57 | 58 | def display_rotate(self,x_spot,y_spot,z_spot,theta_spot,thetax,thetaz,xl,yl,zl): 59 | line = [] 60 | Ma = np.zeros(9) 61 | Mb = np.zeros(9) 62 | M1 = np.zeros(9) 63 | Ma = Spot.xyz_rotation_matrix(theta_spot[3],theta_spot[4],theta_spot[2]+theta_spot[5],False) 64 | Mb = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],0,False) 65 | M1 = Spot.xyz_rotation_matrix(thetax,0,thetaz,True) 66 | 67 | for i in range (0,len(xl)): 68 | #absolute coordinates of Spot lines in x,y,z frame 69 | out0 = Spot.new_coordinates(Ma,xl[i],yl[i],zl[i],x_spot,y_spot,z_spot) 70 | out = Spot.new_coordinates(Mb,out0[0],out0[1],out0[2],0,0,0) 71 | 72 | #Coordinaites for dispaly on screen 73 | disp = Spot.new_coordinates(M1,out[0],out[1],out[2],0,0,0) 74 | yd= disp[1] 75 | xd= disp[0]/2**(yd*SpotAnim.conv/2000) 76 | zd= disp[2]/2**(yd*SpotAnim.conv/2000) 77 | 78 | line.append([int(300+xd),int(300-zd)]) 79 | return line 80 | 81 | 82 | def animate(self,pos,t,thetax,thetaz,angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance): 83 | 84 | theta_spot = pos[12] 85 | x_spot = pos[13] 86 | y_spot = pos[14] 87 | z_spot = pos[15] 88 | CGabs = [x_spot[7],y_spot[7],z_spot[7]] 89 | dCG = [x_spot[8],y_spot[8],z_spot[8]] 90 | 91 | 92 | screen.fill(SpotAnim.WHITE) 93 | #pos = Spot.walking (track,x_offset,steering_radius,steering_angle,cw,h_amp,v_amp,b_height,ra_longi,ra_lat,0.2,t) 94 | 95 | "Floor Display""" 96 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[theta_spot[0],theta_spot[1],0,0,0,0],thetax,thetaz,SpotAnim.xFloor,SpotAnim.yFloor,SpotAnim.zFloor) 97 | pygame.draw.polygon(screen,SpotAnim.GREY,line,0) 98 | 99 | """Floor Grid Display """ 100 | for i in range (0,11): 101 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[theta_spot[0],theta_spot[1],0,0,0,0],thetax,thetaz,[-500+i*100,-500+i*100],[-500,500],[0,0]) 102 | pygame.draw.lines(screen,SpotAnim.DARK_GREY,False,line,1) 103 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[theta_spot[0],theta_spot[1],0,0,0,0],thetax,thetaz,[-500,500],[-500+i*100,-500+i*100],[0,0]) 104 | pygame.draw.lines(screen,SpotAnim.DARK_GREY,False,line,1) 105 | 106 | """ X,Y,Z frame display""" 107 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,SpotAnim.xX,SpotAnim.yX,SpotAnim.zX) 108 | pygame.draw.lines(screen,SpotAnim.RED,False,line,2) 109 | 110 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,SpotAnim.xY,SpotAnim.yY,SpotAnim.zY) 111 | pygame.draw.lines(screen,SpotAnim.GREEN,False,line,2) 112 | 113 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,SpotAnim.xZ,SpotAnim.yZ,SpotAnim.zZ) 114 | pygame.draw.lines(screen,SpotAnim.BLUE,False,line,2) 115 | 116 | """ Radius display """ 117 | center_display = True 118 | if (steering<2000): 119 | lineR = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[center_x,x_spot[0]],[center_y,y_spot[0]],[0,0]) 120 | else: 121 | center_x1 = x_spot[0] + (center_x-x_spot[0])/steering*2000 122 | center_y1 = y_spot[0] + (center_y-y_spot[0])/steering*2000 123 | lineR = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[center_x1,x_spot[0]],[center_y1,y_spot[0]],[0,0]) 124 | center_display = False 125 | 126 | """ Direction display """ 127 | xd = x_spot[0] + walking_speed*cos(theta_spot[2]+ walking_direction-pi/2) 128 | yd = y_spot[0] + walking_speed*sin(theta_spot[2]+ walking_direction-pi/2) 129 | lineD = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[xd,x_spot[0]],[yd,y_spot[0]],[0,0]) 130 | 131 | """Legs lines""" 132 | 133 | leglf = Spot.FK(thetalf,1) 134 | legrf = Spot.FK(thetarf,-1) 135 | legrr = Spot.FK(thetarr,-1) 136 | leglr = Spot.FK(thetalr,1) 137 | 138 | """ Center of Gravity """ 139 | #Calculation of CG absolute position 140 | lineCG = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[CGabs[0],CGabs[0]],[CGabs[1],CGabs[1]],[0,CGabs[2]]) 141 | 142 | xleglf =[Spot.xlf,Spot.xlf+leglf[0],Spot.xlf+leglf[1],Spot.xlf+leglf[2],Spot.xlf+pos[0]] 143 | yleglf =[Spot.ylf,Spot.ylf+leglf[3],Spot.ylf+leglf[4],Spot.ylf+leglf[5],Spot.ylf+pos[1]] 144 | zleglf =[0,leglf[6],leglf[7],leglf[8],pos[2]] 145 | linelf = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xleglf,yleglf,zleglf) 146 | 147 | xlegrf =[Spot.xrf,Spot.xrf+legrf[0],Spot.xrf+legrf[1],Spot.xrf+legrf[2],Spot.xrf+pos[3]] 148 | ylegrf =[Spot.yrf,Spot.yrf+legrf[3],Spot.yrf+legrf[4],Spot.yrf+legrf[5],Spot.yrf+pos[4]] 149 | zlegrf =[0,legrf[6],legrf[7],legrf[8],pos[5]] 150 | linerf = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xlegrf,ylegrf,zlegrf) 151 | 152 | xlegrr =[Spot.xrr,Spot.xrr+legrr[0],Spot.xrr+legrr[1],Spot.xrr+legrr[2],Spot.xrr+pos[6]] 153 | ylegrr =[Spot.yrr,Spot.yrr+legrr[3],Spot.yrr+legrr[4],Spot.yrr+legrr[5],Spot.yrr+pos[7]] 154 | zlegrr = [0,legrr[6],legrr[7],legrr[8],pos[8]] 155 | linerr = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xlegrr,ylegrr,zlegrr) 156 | 157 | xleglr =[Spot.xlr,Spot.xlr+leglr[0],Spot.xlr+leglr[1],Spot.xlr+leglr[2],Spot.xlr+pos[9]] 158 | yleglr =[Spot.ylr,Spot.ylr+leglr[3],Spot.ylr+leglr[4],Spot.ylr+leglr[5],Spot.ylr+pos[10]] 159 | zleglr = [0,leglr[6],leglr[7],leglr[8],pos[11]] 160 | linelr = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xleglr,yleglr,zleglr) 161 | """ Body frame lines """ 162 | lineb = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,[Spot.xlf,Spot.xrf,Spot.xrr,Spot.xlr,Spot.xlf],[Spot.ylf,Spot.yrf,Spot.yrr,Spot.ylr,Spot.ylf],[Spot.zlf,Spot.zrf,Spot.zrr,Spot.zlr,Spot.zlf]) 163 | 164 | 165 | """Sustentation area lines""" 166 | #stance = False when leg is lifted from the floor 167 | 168 | linesus =[] 169 | if (stance[0]==True): 170 | linesus.append(linelf[4]) 171 | if (stance[1]==True): 172 | linesus.append(linerf[4]) 173 | if (stance[2]==True): 174 | linesus.append(linerr[4]) 175 | if (stance[3]==True): 176 | linesus.append(linelr[4]) 177 | 178 | """ Center of gravity into sustentation area """ 179 | linedCG = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[CGabs[0],dCG[0]],[CGabs[1],dCG[1]],[0,0]) 180 | 181 | pygame.draw.polygon(screen,SpotAnim.VIOLET,linesus,0) 182 | pygame.draw.lines(screen,SpotAnim.BLACK,True,linesus,1) 183 | pygame.draw.lines(screen,SpotAnim.CYAN,False,lineR,2) 184 | pygame.draw.lines(screen,SpotAnim.GREEN,False,lineD,2) 185 | 186 | if (center_display == True): 187 | pygame.draw.circle(screen,SpotAnim.BLACK,lineR[0],5) 188 | pygame.draw.lines(screen,SpotAnim.BLACK,False, linedCG,1) 189 | pygame.draw.circle(screen,SpotAnim.DARK_CYAN,lineR[1],5) 190 | 191 | pygame.draw.lines(screen,SpotAnim.BLACK,False, lineCG,1) 192 | if (dCG[2] == True): 193 | pygame.draw.circle(screen,SpotAnim.GREEN,lineCG[0],3) 194 | else: 195 | pygame.draw.circle(screen,SpotAnim.RED,lineCG[0],3) 196 | 197 | pygame.draw.circle(screen,SpotAnim.DARK_GREY,lineCG[1],10) 198 | pygame.draw.lines(screen,SpotAnim.RED,False, linelf,4) 199 | pygame.draw.lines(screen,SpotAnim.RED,False, linerf,4) 200 | pygame.draw.lines(screen,SpotAnim.RED,False, linerr,4) 201 | pygame.draw.lines(screen,SpotAnim.RED,False, linelr,4) 202 | pygame.draw.lines(screen,SpotAnim.BLUE,False,lineb,10) 203 | pygame.draw.lines(screen,SpotAnim.BLACK, False,[[angle[0]/pi*180/45*300+300,0],[angle[0]/pi*180/45*300+300,50]],5) 204 | pygame.draw.lines(screen,SpotAnim.BLACK, False,[[0,angle[1]/pi*180/45*300+300],[50,angle[1]/pi*180/45*300+300]],5) 205 | 206 | return 207 | -------------------------------------------------------------------------------- /Files/Spotmicro_Gravity_Center_Library_v01.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | @author: Arnaud Villeneuve 5 | 6 | This file contains a class of functions to calculate the center of gravity position 7 | and distance to the support polygon edge 8 | 9 | """ 10 | 11 | from math import sqrt 12 | import Spotmicro_Inverse_Kinematics_and_Position_Library_v01 13 | Spot = Spotmicro_Inverse_Kinematics_and_Position_Library_v01.Spot() 14 | 15 | 16 | class SpotCG: 17 | def CG_calculation (self,thetalf,thetarf,thetarr,thetalr): 18 | cgposlf=(Spot.FK_Weight(thetalf,1)) 19 | cgposrf=(Spot.FK_Weight(thetarf,-1)) 20 | cgposrr=(Spot.FK_Weight(thetarr,-1)) 21 | cgposlr=(Spot.FK_Weight(thetalr,1)) 22 | 23 | Weightsum = Spot.Weight_Body+4*(Spot.Weight_Shoulder+Spot.Weight_Leg+Spot.Weight_Foreleg) 24 | 25 | xcglf=(cgposlf[0]+Spot.xlf)*Spot.Weight_Shoulder+(cgposlf[1]+Spot.xlf)*Spot.Weight_Leg+(cgposlf[2]+Spot.xlf)*Spot.Weight_Foreleg 26 | xcgrf=(cgposrf[0]+Spot.xrf)*Spot.Weight_Shoulder+(cgposrf[1]+Spot.xrf)*Spot.Weight_Leg+(cgposrf[2]+Spot.xrf)*Spot.Weight_Foreleg 27 | xcgrr=(cgposrr[0]+Spot.xrr)*Spot.Weight_Shoulder+(cgposrr[1]+Spot.xrr)*Spot.Weight_Leg+(cgposrr[2]+Spot.xrr)*Spot.Weight_Foreleg 28 | xcglr=(cgposlr[0]+Spot.xlr)*Spot.Weight_Shoulder+(cgposlr[1]+Spot.xlr)*Spot.Weight_Leg+(cgposlr[2]+Spot.xlr)*Spot.Weight_Foreleg 29 | xcg= (xcglf+xcgrf+xcgrr+xcglr+Spot.xCG_Body*Spot.Weight_Body)/Weightsum 30 | 31 | ycglf=(cgposlf[3]+Spot.ylf)*Spot.Weight_Shoulder+(cgposlf[4]+Spot.ylf)*Spot.Weight_Leg+(cgposlf[5]+Spot.ylf)*Spot.Weight_Foreleg 32 | ycgrf=(cgposrf[3]+Spot.yrf)*Spot.Weight_Shoulder+(cgposrf[4]+Spot.yrf)*Spot.Weight_Leg+(cgposrf[5]+Spot.yrf)*Spot.Weight_Foreleg 33 | ycgrr=(cgposrr[3]+Spot.yrr)*Spot.Weight_Shoulder+(cgposrr[4]+Spot.yrr)*Spot.Weight_Leg+(cgposrr[5]+Spot.yrr)*Spot.Weight_Foreleg 34 | ycglr=(cgposlr[3]+Spot.ylr)*Spot.Weight_Shoulder+(cgposlr[4]+Spot.ylr)*Spot.Weight_Leg+(cgposlr[5]+Spot.ylr)*Spot.Weight_Foreleg 35 | ycg= (ycglf+ycgrf+ycgrr+ycglr+Spot.yCG_Body*Spot.Weight_Body)/Weightsum 36 | 37 | zcglf=(cgposlf[6]+Spot.zlf)*Spot.Weight_Shoulder+(cgposlf[7]+Spot.zlf)*Spot.Weight_Leg+(cgposlf[8]+Spot.zlf)*Spot.Weight_Foreleg 38 | zcgrf=(cgposrf[6]+Spot.zrf)*Spot.Weight_Shoulder+(cgposrf[7]+Spot.zrf)*Spot.Weight_Leg+(cgposrf[8]+Spot.zrf)*Spot.Weight_Foreleg 39 | zcgrr=(cgposrr[6]+Spot.zrr)*Spot.Weight_Shoulder+(cgposrr[7]+Spot.zrr)*Spot.Weight_Leg+(cgposrr[8]+Spot.zrr)*Spot.Weight_Foreleg 40 | zcglr=(cgposlr[6]+Spot.zlr)*Spot.Weight_Shoulder+(cgposlr[7]+Spot.zlr)*Spot.Weight_Leg+(cgposlr[8]+Spot.zlr)*Spot.Weight_Foreleg 41 | zcg= (zcglf+zcgrf+zcgrr+zcglr+Spot.zCG_Body*Spot.Weight_Body)/Weightsum 42 | 43 | return (xcg,ycg,zcg) 44 | 45 | 46 | def CG_distance (self,x_legs,y_legs,z_legs,xcg,ycg,stance): 47 | 48 | #line equation c * x + s * y - p = 0 49 | # with c = a/m et s = b/m 50 | 51 | a1 = (y_legs[0]-y_legs[2]) 52 | b1 = -(x_legs[0]-x_legs[2]) 53 | m1 =sqrt(a1**2 + b1**2) 54 | c1 = a1/m1 55 | s1 = b1/m1 56 | 57 | a2 = (y_legs[1]-y_legs[3]) 58 | b2 = -(x_legs[1]-x_legs[3]) 59 | m2 =sqrt(a2**2 + b2**2) 60 | c2 = a2/m2 61 | s2 = b2/m2 62 | 63 | p1 = c1*x_legs[0] + s1*y_legs[0] 64 | p2 = c2*x_legs[1] + s2*y_legs[1] 65 | 66 | """ Dstance calculation """ 67 | d1 = c1*xcg + s1*ycg - p1 68 | d2 = c2*xcg + s2*ycg - p2 69 | 70 | """ intersection calculation """ 71 | #perpendicalar line equation -s * x + c * y - q = 0 72 | 73 | q1 = -s1*xcg +c1*ycg 74 | q2 = -s2*xcg +c2*ycg 75 | 76 | xint1 = c1*p1 - s1*q1 77 | yint1 = c1*q1 + s1*p1 78 | 79 | xint2 = c2*p2 - s2*q2 80 | yint2 = c2*q2 + s2*p2 81 | 82 | """ Check if inside sustentation triangle """ 83 | d = 0 84 | xint = xcg 85 | yint = ycg 86 | if (stance[0]== False)|(stance[2]== False): 87 | d = d2 88 | xint = xint2 89 | yint = yint2 90 | 91 | 92 | if (stance[1]== False)|(stance[3]== False): 93 | d = d1 94 | xint = xint1 95 | yint = yint1 96 | 97 | balance = True 98 | 99 | if (stance[0] == False)&(d< 0): 100 | balance = False 101 | 102 | if (stance[1] == False)&(d> 0): 103 | balance = False 104 | 105 | if (stance[2] == False)&(d> 0): 106 | balance = False 107 | 108 | if (stance[3] == False)&(d< 0): 109 | balance = False 110 | 111 | if (balance == False): 112 | d=-abs(d) 113 | else: 114 | d=abs(d) 115 | 116 | return (d,xint,yint,balance) 117 | -------------------------------------------------------------------------------- /Files/Spotmicro_Inverse_Kinematics_and_Position_Library_v01.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @author: Arnaud Villeneuve 4 | 5 | This file contains a class of paramaters and functions to calculate Spot micro position 6 | It includes : 7 | -The main dimensions of Spot micro 8 | -The center of gravity position and weight of each spotmicro limbs and body 9 | -The forward kinematics function 10 | -The forward kinematics functions 11 | -The function that generates walking positions 12 | -The function that generates positions from known start and end positions 13 | 14 | 15 | """ 16 | 17 | from math import pi, sin, cos, asin, acos, atan2, sqrt 18 | import numpy as np 19 | 20 | 21 | class Spot: 22 | """" Spotmicro dimensions """ 23 | Wb = 78 #Shoulder/hip width 24 | Lb = 187.1 #Shoulder to hip length 25 | L0 = 58.09 #Shoulder articulation width 26 | d = 10.73 #Shoulder articulation height 27 | L1 = 108.31 #Leg length 28 | L2 = 138 #Foreleg length 29 | 30 | """ Anchor points """ 31 | 32 | """ Front left shoulder""" 33 | xlf = 93.55 34 | ylf = 39 35 | zlf = 0 36 | 37 | """Front right shoulder""" 38 | xrf = 93.55 39 | yrf = -39 40 | zrf = 0 41 | 42 | """Rear left hip """ 43 | xlr = -93.55 44 | ylr = 39 45 | zlr = 0 46 | 47 | """Rear right hip """ 48 | xrr = -93.55 49 | yrr = -39 50 | zrr = 0 51 | 52 | 53 | """Inertia Centers""" 54 | 55 | """ Body""" 56 | xCG_Body = 0 57 | yCG_Body = 0 58 | zCG_Body = 0 59 | Weight_Body = 897 60 | 61 | """ Left Shoulder """ 62 | xCG_Shoulder = 0 63 | yCG_Shoulder = 5 #minus for right leg 64 | zCG_Shoulder = -9 65 | Weight_Shoulder = 99.3 66 | 67 | """ Left Leg """ 68 | xCG_Leg = 0 69 | yCG_Leg = 0 #minus for right leg 70 | zCG_Leg = -31 71 | Weight_Leg = 133.3 72 | 73 | """ Left Leg """ 74 | xCG_Foreleg = 0 75 | yCG_Foreleg = 0 #minus for right leg 76 | zCG_Foreleg = -28 77 | Weight_Foreleg = 107 78 | 79 | 80 | seq= [0, 0.5, 0.25, 0.75] 81 | phase = pi/8 # optimum position when leg is fully lifted 82 | 83 | def interp(x1,x2,steps): 84 | out = np.zeros(steps) 85 | for i in range (steps): 86 | out[i] = x1 +(x2-x1)/(steps-1)*i 87 | return out 88 | 89 | def interp1(x1,x2,i,steps): 90 | out = x1 +(x2-x1)/(steps-1)*i 91 | return out 92 | 93 | 94 | 95 | def xyz_rotation_matrix (self,thetax,thetay,thetaz,inverse): 96 | if (inverse == True): 97 | #Rx*Ry*Rz 98 | t2 = cos(thetay) 99 | t3 = sin(thetaz) 100 | t4 = cos(thetaz) 101 | t5 = sin(thetay) 102 | t6 = cos(thetax) 103 | t7 = sin(thetax) 104 | M = [t2*t4,t3*t6+t4*t5*t7,t3*t7-t4*t5*t6,-t2*t3,t4*t6-t3*t5*t7,t4*t7+t3*t5*t6,t5,-t2*t7,t2*t6] 105 | else: 106 | #Rz*Ry*Rx 107 | t2 = cos(thetaz); 108 | t3 = sin(thetax); 109 | t4 = sin(thetaz); 110 | t5 = cos(thetax); 111 | t6 = sin(thetay); 112 | t7 = cos(thetay); 113 | M = [t2*t7,t4*t7,-t6,-t4*t5+t2*t3*t6,t2*t5+t3*t4*t6,t3*t7,t3*t4+t2*t5*t6,-t2*t3+t4*t5*t6,t5*t7] 114 | return M 115 | 116 | 117 | def new_coordinates (self,M,x,y,z,x0,y0,z0): 118 | xout= x0 + M[0]*x + M[3]*y + M[6]*z 119 | yout= y0 + M[1]*x + M[4]*y + M[7]*z 120 | zout= z0 + M[2]*x + M[5]*y + M[8]*z 121 | return [xout,yout,zout] 122 | 123 | 124 | 125 | def foot_coordinate (self,x,y,z,thetax,thetay): 126 | cx = cos(thetax) 127 | sx = sin(thetax) 128 | cy = cos(thetay) 129 | sy = sin(thetay) 130 | xf = x*cy+ z*sy 131 | yf = y*cx - z*cy*sx + x*sx*sy 132 | zf = y*sx + z*cx*cy - x*cx*sy 133 | return [xf,yf,zf] 134 | 135 | 136 | def IK (self,L0, L1, L2, d, x, y, z, side): #Inverse Kinematics 137 | """ 138 | s = 1 for left leg 139 | s = -1 for right leg 140 | """ 141 | t2 = y**2 142 | t3 = z**2 143 | t4 = t2+t3 144 | t5 = 1/sqrt(t4) 145 | t6 = L0**2 146 | t7 = t2+t3-t6 147 | t8 = sqrt(t7) 148 | t9 = d-t8 149 | t10 = x**2 150 | t11 = t9**2 151 | t15 = L1**2 152 | t16 = L2**2 153 | t12 = t10+t11-t15-t16 154 | t13 = t10+t11 155 | t14 = 1/sqrt(t13) 156 | error = False 157 | try: 158 | theta1 = side*(-pi/2+asin(t5*t8))+asin(t5*y) 159 | theta2= -asin(t14*x)+asin(L2*t14*sqrt(1/t15*1/t16*t12**2*(-1/4)+1)) 160 | theta3 =-pi + acos(-t12/2/(L1*L2)) 161 | 162 | except ValueError: 163 | print ('ValueError IK') 164 | error = True 165 | theta1=90 166 | theta2=90 167 | theta3=90 168 | 169 | theta = [theta1, theta2, theta3] 170 | return (theta,error) 171 | 172 | def FK (self, theta, side): #Forward Kinematics 173 | """ Calculation of articulation points """ 174 | """ 175 | s = 1 for left leg 176 | s = -1 for right leg 177 | """ 178 | x_shoulder1 = 0 179 | y_shoulder1 = Spot.d*sin(theta[0]) 180 | z_shoulder1 = -Spot.d*cos(theta[0]) 181 | 182 | x_shoulder2 = 0 183 | y_shoulder2 =side*Spot.L0*cos(theta[0])+Spot.d*sin(theta[0]) 184 | z_shoulder2 =side*Spot.L0*sin(theta[0])-Spot.d*cos(theta[0]) 185 | 186 | x_elbow = -Spot.L1*sin(theta[1]) 187 | y_elbow = side*Spot.L0*cos(theta[0])-(-Spot.d-Spot.L1*cos(theta[1]))*sin(theta[0]) 188 | z_elbow = side*Spot.L0*sin(theta[0]) +(-Spot.d-Spot.L1*cos(theta[1]))*cos(theta[0]) 189 | 190 | return [x_shoulder1,x_shoulder2,x_elbow,y_shoulder1,y_shoulder2,y_elbow,z_shoulder1,z_shoulder2,z_elbow] 191 | 192 | 193 | def FK_Weight (self, theta, side): #Forward Kinematics for calculation of Center of Gravity 194 | """ Calculation of articulation points """ 195 | """ 196 | side = 1 for left leg 197 | side = -1 for right leg 198 | """ 199 | 200 | xCG_Shoulder1 = Spot.xCG_Shoulder 201 | yCG_Shoulder1 =side*Spot.yCG_Shoulder*cos(theta[0])-Spot.zCG_Shoulder*sin(theta[0]) 202 | zCG_Shoulder1 =side*Spot.yCG_Shoulder*sin(theta[0])+Spot.zCG_Shoulder*cos(theta[0]) 203 | 204 | xCG_Leg1 = Spot.xCG_Leg*cos(theta[1]) + Spot.zCG_Leg*sin(theta[1]) 205 | yCG_Leg1 = cos(theta[0])*(Spot.L0*side + side*Spot.yCG_Leg) + sin(theta[0])*(Spot.d - Spot.zCG_Leg*cos(theta[1]) + Spot.xCG_Leg*sin(theta[1])) 206 | zCG_Leg1 = sin(theta[0])*(Spot.L0*side + side*Spot.yCG_Leg) - cos(theta[0])*(Spot.d - Spot.zCG_Leg*cos(theta[1]) + Spot.xCG_Leg*sin(theta[1])) 207 | 208 | xCG_Foreleg1 = cos(theta[1])*(Spot.xCG_Foreleg*cos(theta[2]) + Spot.zCG_Foreleg*sin(theta[2])) - sin(theta[1])*(Spot.L1 - Spot.zCG_Foreleg*cos(theta[2]) + Spot.xCG_Foreleg*sin(theta[2])) 209 | yCG_Foreleg1 = cos(theta[0])*(Spot.L0*side + side*Spot.yCG_Foreleg) + sin(theta[0])*(Spot.d + sin(theta[1])*(Spot.xCG_Foreleg*cos(theta[2]) + Spot.zCG_Foreleg*sin(theta[2])) + cos(theta[1])*(Spot.L1 - Spot.zCG_Foreleg*cos(theta[2]) + Spot.xCG_Foreleg*sin(theta[2]))) 210 | zCG_Foreleg1 = sin(theta[0])*(Spot.L0*side + side*Spot.yCG_Foreleg) - cos(theta[0])*(Spot.d + sin(theta[1])*(Spot.xCG_Foreleg*cos(theta[2]) + Spot.zCG_Foreleg*sin(theta[2])) + cos(theta[1])*(Spot.L1 - Spot.zCG_Foreleg*cos(theta[2]) + Spot.xCG_Foreleg*sin(theta[2]))) 211 | 212 | return [xCG_Shoulder1,xCG_Leg1,xCG_Foreleg1,yCG_Shoulder1,yCG_Leg1,yCG_Foreleg1,zCG_Shoulder1,zCG_Leg1,zCG_Foreleg1] 213 | 214 | """ 215 | Walking Function that generates the walking positions 216 | """ 217 | 218 | def start_walk_stop (self,track,x_offset,steering_radius, steering_angle,cw,h_amp,v_amp,height,stepl,t,tstep,theta_spot,x_spot,y_spot,z_spot,step_phase): 219 | 220 | alpha = np.zeros(4) 221 | alphav =np.zeros(4) 222 | 223 | theta_spot_updated = theta_spot 224 | 225 | CG = [x_spot[6],y_spot[6],z_spot[6]] 226 | 227 | """ Steering center coordinates in spot frame """ 228 | xc = steering_radius* cos(steering_angle) 229 | yc = steering_radius* sin(steering_angle) 230 | 231 | #rotation matrix for frame position 232 | #Mf = Spot.xyz_rotation_matrix (self,frame_pos[0],frame_pos[1],frame_pos[2],False) 233 | 234 | Ms = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2],False) 235 | 236 | s = Spot.new_coordinates(self,Ms,xc,yc,0,x_spot[0],y_spot[0],z_spot[0]) 237 | xs = s[0] 238 | ys = s[1] 239 | 240 | """ Nominal Foot Position """ 241 | xn = [Spot.xlf, Spot.xrf,Spot.xrr, Spot.xlr] 242 | yn = [Spot.ylf+track,Spot.yrf-track,Spot.yrr-track,Spot.ylr+track] 243 | 244 | radii = np.zeros(4) 245 | an = np.zeros(4) 246 | for i in range (0,4): 247 | """ Steering radius """ 248 | radii[i] = sqrt((xc-xn[i])**2+(yc-yn[i])**2) 249 | """ Foot nominal angle""" 250 | an[i] = atan2(yn[i]-yc,xn[i]-xc) 251 | 252 | """ Motion angle """ 253 | maxr = max(radii) 254 | mangle = h_amp/maxr 255 | 256 | """ Rotation angle and translation calculation""" 257 | if (step_phase =='start')|(step_phase == 'stop'): 258 | dtheta = mangle/(1-stepl)*tstep/2*cw 259 | else: 260 | dtheta = mangle/(1-stepl)*tstep*cw 261 | theta_spot_updated[2] = dtheta + theta_spot[2] 262 | 263 | #Matrix from local body frame to absolute space frame 264 | Ms_updated = Spot.xyz_rotation_matrix (self,theta_spot_updated[3],theta_spot_updated[4],theta_spot_updated[2]+theta_spot_updated[5],False) 265 | #Matrix from absolute space frame to local body frame 266 | Msi_updated = Spot.xyz_rotation_matrix (self,-theta_spot_updated[3],-theta_spot_updated[4],-(theta_spot_updated[2]+theta_spot_updated[5]),True) 267 | #Delta rotation matric from from body center to absolute space frame 268 | dMs = Spot.xyz_rotation_matrix (self,0,0,dtheta,False) 269 | 270 | """ Foot nominal center absolute position""" 271 | foot_center = Spot.new_coordinates(self,dMs,x_spot[0]-xs, y_spot[0]-ys,0,xs,ys,0) 272 | 273 | t1 = t%1 274 | kcomp = 1 275 | stance = [True,True,True,True] 276 | 277 | for i in range (0,4): 278 | alphav[i] =0 279 | if (t1<=Spot.seq[i]): 280 | stance[i] = True #Leg is on the ground (absolute position value unchanged) 281 | else: 282 | if (t1<(Spot.seq[i]+stepl)): 283 | 284 | stance[i] = False #leg is lifted (absolute position value changes) 285 | alphav[i] = -pi/2+2*pi/stepl*(t1-Spot.seq[i]) 286 | t2 = Spot.seq[i]+stepl 287 | if (step_phase == 'start'): 288 | #End position alpha 289 | alpha[i] = -Spot.seq[i]/(1-stepl)/2 + (t2-Spot.seq[i])/stepl/(1-stepl)*Spot.seq[i] 290 | if (step_phase == 'stop'): 291 | alpha[i] = -1/2 + Spot.seq[i]/(1-stepl)/2 + (t2-Spot.seq[i])/stepl*(1-Spot.seq[i]/(1-stepl)) 292 | if (step_phase == 'walk'): 293 | alpha[i] = -1/2 + ((t2-Spot.seq[i])/stepl) 294 | else: 295 | stance[i] = True #Leg is on the ground (absolute position value unchanged) 296 | 297 | """ Compensation Calculation """ 298 | stance_test = np.sum(stance) #if sum = 4 all feet are on the floor --> body balance 299 | 300 | #absolute stance area target point 301 | #Barycenter of sustentation area with higher weight of diagonal points 302 | weight = 1.2 303 | x_abs_area = np.zeros(4) 304 | y_abs_area = np.zeros(4) 305 | 306 | 307 | x_abs_area[0] = ((x_spot[3]+x_spot[5])*weight+x_spot[4])/(2*weight+1) 308 | y_abs_area[0] = ((y_spot[3]+y_spot[5])*weight+y_spot[4])/(2*weight+1) 309 | x_abs_area[1] = ((x_spot[2]+x_spot[4])*weight+x_spot[5])/(2*weight+1) 310 | y_abs_area[1] = ((y_spot[2]+y_spot[4])*weight+y_spot[5])/(2*weight+1) 311 | x_abs_area[2] = ((x_spot[3]+x_spot[5])*weight+x_spot[2])/(2*weight+1) 312 | y_abs_area[2] = ((y_spot[3]+y_spot[5])*weight+y_spot[2])/(2*weight+1) 313 | x_abs_area[3] = ((x_spot[2]+x_spot[4])*weight+x_spot[3])/(2*weight+1) 314 | y_abs_area[3] = ((y_spot[2]+y_spot[4])*weight+y_spot[3])/(2*weight+1) 315 | 316 | if (stance_test == 4): 317 | istart = 0 318 | iend = 0 319 | #identify transition start and target 320 | tstart = (int(t1/0.25)*0.25) 321 | tend = tstart+0.25 322 | if (tend==1): 323 | tend = 0 324 | 325 | for i in range (0,4): 326 | if (tstart == Spot.seq[i]): 327 | istart = i 328 | if (tend == Spot.seq[i]): 329 | iend = i 330 | 331 | if (t1>(Spot.seq[istart]+stepl)): 332 | x_abs_comp= x_abs_area[istart]+(x_abs_area[iend]-x_abs_area[istart])*(t1-tstart-stepl)/(0.25-stepl) 333 | y_abs_comp= y_abs_area[istart]+(y_abs_area[iend]-y_abs_area[istart])*(t1-tstart-stepl)/(0.25-stepl) 334 | else: 335 | x_abs_comp = x_abs_area[istart] 336 | y_abs_comp = y_abs_area[istart] 337 | else: 338 | for i in range (0,4): 339 | if (stance[i]==0): 340 | x_abs_comp = x_abs_area[i] 341 | y_abs_comp = y_abs_area[i] 342 | 343 | Msi_comp = Spot.xyz_rotation_matrix (self,0,0,-theta_spot_updated[2],True) 344 | #compensation calculation in body center frame 345 | comp= Spot.new_coordinates(self,Msi_comp,x_abs_comp-x_spot[0],y_abs_comp-y_spot[0],0,0,0,0) 346 | """ Compensation calculation with theta """ 347 | v_amp_t = v_amp 348 | ts = 0.25 349 | if (step_phase == 'start'): 350 | if (t1< ts): 351 | kcomp = t1/ts 352 | v_amp_t = 0 353 | elif (step_phase == 'stop'): 354 | if (t1 > (1-ts)): 355 | kcomp = (1-t1)/ts 356 | v_amp_t = 0 357 | Ms_comp = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2],False) 358 | #Compensation calculation absoltute space frame 359 | compt = Spot.new_coordinates(self,Ms_comp,(comp[0]-CG[0])*kcomp+x_offset,(comp[1]-CG[1])*kcomp,0,0,0,0) 360 | """ Frame center new position with gravity center compensation, offset and height """ 361 | x_framecenter_comp = foot_center[0] + compt[0] 362 | y_framecenter_comp = foot_center[1] + compt[1] 363 | z_framecenter_comp = height 364 | 365 | """ New Frame corners position absolute including compensation """ 366 | x_frame = [Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr] 367 | y_frame = [Spot.ylf, Spot.yrf, Spot.yrr, Spot.ylr] 368 | z_frame = [0,0,0,0] 369 | 370 | x_framecorner = np.zeros(4) 371 | y_framecorner = np.zeros(4) 372 | z_framecorner = np.zeros(4) 373 | 374 | for i in range (0,4): 375 | #Body corners calculation in absolute space frame 376 | frame_corner = Spot.new_coordinates(self,Ms_updated,x_frame[i],y_frame[i],z_frame[i],x_framecenter_comp,y_framecenter_comp,z_framecenter_comp) 377 | x_framecorner[i] = frame_corner[0] 378 | y_framecorner[i] = frame_corner[1] 379 | z_framecorner[i] = frame_corner[2] 380 | 381 | 382 | xleg = np.zeros(4) 383 | yleg = np.zeros(4) 384 | zleg = np.zeros(4) 385 | xabs = np.zeros(4) 386 | yabs = np.zeros(4) 387 | zabs = np.zeros(4) 388 | xint = np.zeros(4) 389 | yint = np.zeros(4) 390 | zint = np.zeros(4) 391 | 392 | for i in range (0,4): 393 | if stance[i] == False: 394 | #relative position calculation (used for inverse kinematics) 395 | alphah = an[i]+mangle*alpha[i]*cw 396 | xleg_target = xc + radii[i]*cos(alphah) -(comp[0]-CG[0])*kcomp -x_offset -x_frame[i] 397 | yleg_target = yc + radii[i]*sin(alphah) -(comp[1]-CG[1])*kcomp -y_frame[i] 398 | 399 | leg_current = Spot.new_coordinates(self,Msi_comp,x_spot[i+2]-x_framecorner[i],y_spot[i+2]-y_framecorner[i],-z_framecorner[i],0,0,0) 400 | #interpolate between current position and targe 401 | if ((Spot.seq[i]+stepl-t1)>tstep): 402 | xint[i] = leg_current[0]+(xleg_target - leg_current[0])*(tstep)/(Spot.seq[i]+stepl-t1) 403 | yint[i] = leg_current[1]+(yleg_target - leg_current[1])*(tstep)/(Spot.seq[i]+stepl-t1) 404 | else: 405 | xint[i] = xleg_target 406 | yint[i] = yleg_target 407 | zint[i] = leg_current[2] + v_amp_t*(1+sin(alphav[i]))/2 408 | #print (leg_current[2],zint[i],leg_current[2]-zint[i]) 409 | Msi_body = Spot.xyz_rotation_matrix (self,-theta_spot_updated[3],-theta_spot_updated[4],-theta_spot_updated[5],True) 410 | legs = Spot.new_coordinates(self,Msi_body,xint[i],yint[i],zint[i],0,0,0) 411 | xleg[i]= legs[0] 412 | yleg[i]= legs[1] 413 | zleg[i]= legs[2] 414 | 415 | #absolute foot position 416 | #Msb_updated = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2]+theta_spot_updated[5],False) 417 | foot_abs = Spot.new_coordinates(self,Ms_updated,xleg[i],yleg[i],zleg[i],x_framecorner[i],y_framecorner[i],z_framecorner[i]) 418 | 419 | 420 | xabs[i] = foot_abs[0] 421 | yabs[i] = foot_abs[1] 422 | zabs[i] = foot_abs[2] 423 | 424 | else: 425 | xabs[i] = x_spot[i+2] 426 | yabs[i] = y_spot[i+2] 427 | zabs[i] = 0 428 | 429 | #relative foot position of foot on the ground/floor for inverse kinematics 430 | leg = Spot.new_coordinates(self,Msi_updated,xabs[i]-x_framecorner[i],yabs[i]-y_framecorner[i],zabs[i]-z_framecorner[i],0,0,0) 431 | xleg[i] = leg[0] 432 | yleg[i] = leg[1] 433 | zleg[i] = leg[2] 434 | 435 | x_spot_updated = [foot_center[0],x_framecenter_comp, xabs[0], xabs[1], xabs[2], xabs[3],x_spot[6],x_spot[7],x_spot[8]] 436 | y_spot_updated = [foot_center[1],y_framecenter_comp, yabs[0], yabs[1], yabs[2], yabs[3],y_spot[6],y_spot[7],y_spot[8]] 437 | z_spot_updated = [foot_center[2],z_framecenter_comp, zabs[0], zabs[1], zabs[2], zabs[3],z_spot[6],z_spot[7],z_spot[8]] 438 | 439 | 440 | pos = [xleg[0],yleg[0],zleg[0],xleg[1],yleg[1],zleg[1],xleg[2],yleg[2],zleg[2],xleg[3],yleg[3],zleg[3],theta_spot_updated,x_spot_updated,y_spot_updated,z_spot_updated] 441 | return pos 442 | 443 | 444 | """" 445 | Moving Function from known start and end positions (used for sitting, lying, etc...) 446 | """ 447 | 448 | def moving (self,t, start_frame_pos,end_frame_pos, pos): 449 | 450 | theta_spot_updated = pos[12] 451 | x_spot_updated = pos[13] 452 | y_spot_updated = pos[14] 453 | z_spot_updated = pos[15] 454 | 455 | 456 | #interpolate new frame position 457 | frame_pos = np.zeros(6) 458 | 459 | for i in range (0,6): 460 | frame_pos[i] = start_frame_pos[i] + (end_frame_pos[i]- start_frame_pos[i])*t 461 | 462 | theta_spot_updated [3] = frame_pos[0] 463 | theta_spot_updated [4] = frame_pos[1] 464 | theta_spot_updated [5] = frame_pos[2] 465 | #rotation matrix for frame position 466 | Mf = Spot.xyz_rotation_matrix (self,frame_pos[0],frame_pos[1],frame_pos[2],False) 467 | 468 | #rotation matrix for spot position (only around z axis) 469 | Ms = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2],False) 470 | 471 | # frame corners position coordinaterelative to frame center 472 | x_frame = [Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr] 473 | y_frame = [Spot.ylf, Spot.yrf, Spot.yrr, Spot.ylr] 474 | z_frame = [0,0,0,0] 475 | 476 | #New absolute frame center position 477 | frame_center_abs = Spot.new_coordinates(self,Ms,frame_pos[3],frame_pos[4],frame_pos[5],x_spot_updated[0],y_spot_updated[0],z_spot_updated[0]) 478 | 479 | #absolute frame corners position coordinates 480 | x_frame_corner_abs = np.zeros(4) 481 | y_frame_corner_abs = np.zeros(4) 482 | z_frame_corner_abs = np.zeros(4) 483 | 484 | for i in range (0,4): 485 | frame_corner = Spot.new_coordinates(self,Mf,x_frame[i],y_frame[i],z_frame[i],0,0,0) 486 | frame_corner_abs = Spot.new_coordinates(self,Ms,frame_corner[0],frame_corner[1],frame_corner[2],frame_center_abs[0],frame_center_abs[1],frame_center_abs[2]) 487 | x_frame_corner_abs[i] = frame_corner_abs[0] 488 | y_frame_corner_abs[i] = frame_corner_abs[1] 489 | z_frame_corner_abs[i] = frame_corner_abs[2] 490 | 491 | #calculate current relative position 492 | xleg = np.zeros(4) 493 | yleg = np.zeros(4) 494 | zleg = np.zeros(4) 495 | 496 | 497 | #Leg relative position to front corners 498 | Mi = Spot.xyz_rotation_matrix(self,-theta_spot_updated[3],-theta_spot_updated[4],-(theta_spot_updated[2]+theta_spot_updated[5]),True) 499 | 500 | for i in range (0,4): 501 | leg = Spot.new_coordinates(self,Mi,x_spot_updated[i+2]-x_frame_corner_abs[i],y_spot_updated[i+2]-y_frame_corner_abs[i],z_spot_updated[i+2]-z_frame_corner_abs[i],0,0,0) 502 | xleg[i] = leg[0] 503 | yleg[i] = leg[1] 504 | zleg[i] = leg[2] 505 | 506 | 507 | x_spot_updated[1] = frame_center_abs [0] 508 | y_spot_updated[1] = frame_center_abs [1] 509 | z_spot_updated[1] = frame_center_abs [2] 510 | 511 | 512 | pos = [xleg[0],yleg[0],zleg[0],xleg[1],yleg[1],zleg[1],xleg[2],yleg[2],zleg[2],xleg[3],yleg[3],zleg[3],theta_spot_updated,x_spot_updated,y_spot_updated,z_spot_updated] 513 | return pos 514 | 515 | 516 | 517 | -------------------------------------------------------------------------------- /Full Working Version/Spot_Micro_animation_029_pygame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | @author: Arnaud Villeneuve 5 | """ 6 | 7 | """ 8 | type %matplotlib qt on console to enable animation 9 | """ 10 | import board 11 | import busio 12 | import adafruit_pca9685 13 | import adafruit_mpu6050 14 | from adafruit_servokit import ServoKit 15 | import adafruit_ads1x15.ads1015 as ADS 16 | from adafruit_ads1x15.analog_in import AnalogIn 17 | from time import sleep, time 18 | from math import pi, sin, cos, atan, atan2, sqrt 19 | import numpy as np 20 | 21 | import pygame 22 | pygame.init() 23 | screen = pygame.display.set_mode((600, 600)) #seems necessary to have access to keyboard 24 | pygame.display.set_caption("SPOTMICRO") 25 | 26 | 27 | import Spotmicro_lib_020 28 | Spot = Spotmicro_lib_020.Spot() 29 | import Spotmicro_Gravity_Center_lib_007 30 | SpotCG = Spotmicro_Gravity_Center_lib_007.SpotCG() 31 | import Spotmicro_Animate_lib_009 32 | SpotAnim = Spotmicro_Animate_lib_009.SpotAnim() 33 | import matplotlib.pyplot as plt 34 | 35 | 36 | smallfont = pygame.font.SysFont('Corbel',20) 37 | text_animon = smallfont.render('Anim On' , True , SpotAnim.BLACK) 38 | text_animoff = smallfont.render('Anim Off' , True , SpotAnim.WHITE) 39 | text_moveon = smallfont.render('Move On' , True , SpotAnim.BLACK) 40 | text_moveoff = smallfont.render('Move Off' , True , SpotAnim.WHITE) 41 | 42 | """ Walking parameters """ 43 | b_height = 200 44 | h_amp2 = 100#was 100 45 | v_amp2 = 20 #was 50 46 | track2 = 58 47 | h_amp4 = 100#was 100 48 | v_amp4 = 30 #was 50 49 | track4 = 58 50 | x_offset = 0#40 51 | 52 | ra_longi = 30#30 53 | ra_lat = 30#20 54 | 55 | bend_angle = 0 56 | 57 | steering =200 #Initial steering radius (arbitrary) 58 | walking_direction = 90/180*pi #Initial steering angle (arbitrary) 59 | center_x = steering*cos(walking_direction) 60 | center_y = steering*sin(walking_direction) 61 | 62 | stepl2 = 0.16 63 | stepl4= 0.125 64 | 65 | tstep2 = stepl2/8 #Timing/clock step 0.00666666666666 66 | tstep4 = 0.006666666666 67 | 68 | height = b_height 69 | 70 | """Initialization to 4 steps walk """ 71 | stepl = stepl4 72 | h_amp = h_amp4 73 | v_amp = v_amp4 74 | track = track4 75 | tstep = tstep4 76 | 77 | """ Joystick settings """ 78 | but_walk = 7 79 | but_sit = 2 80 | but_lie = 3 81 | but_twist = 1 82 | but_pee = 0 83 | but_move =5 84 | but_anim = 4 85 | 86 | pos_frontrear = 3 87 | pos_leftright = 2 88 | pos_turn = 0 89 | pos_rightpaw = 4 90 | pos_leftpaw = 5 #also used for hind leg lifting when peeing 91 | 92 | joypos =np.zeros(6) 93 | joybut = np.zeros(15) 94 | 95 | joype = -1 # Initial joysick value for peeing 96 | joypar = -1 # Initial joysick value for pawing right 97 | joypal = -1 # Initial joysick value for pawing left 98 | joybendover = -1 # Initial joystick value for bending over 99 | joyleanback = -1 # Initial joystick value for pawing left 100 | 101 | 102 | """ Data logging intialization""" 103 | distance =[] #distance to sustentation limit record 104 | balance =[] #balance status (True or False) 105 | timing = [] #timing to plot distance 106 | xCG = [] 107 | yCG = [] 108 | xCenter = [] 109 | yCenter = [] 110 | xZMP = [] 111 | yZMP = [] 112 | Integral_Angle = [0,0] 113 | sCG = [0,0] #Center of Gravity speed 114 | aCG = [0,0] #Center of Gravity acceleration 115 | tt = 0 #time to calculate speed and acceleration 116 | dtt = 0 #real time step 117 | ZMP = [0,0] 118 | 119 | 120 | """ States initialization""" 121 | anim = True#animation display 122 | Free = True #Spot is ready to receive new command 123 | sitting = False 124 | walking = False #walking sequence activation 125 | lying = False 126 | twisting = False 127 | pawing = False 128 | shifting = False 129 | peeing = False 130 | stop = False # walking stop sequence activation 131 | lock = False # locking start key temporarily in order to avoid start and stop if start key is pressed too long 132 | move =False# servo move activation 133 | lockmouse = False 134 | mouseclick = False 135 | IMU_Comp = False 136 | trot = False 137 | 138 | stance = [True, True, True, True] 139 | cw = 1 140 | walking_speed = 0 141 | walking_direction = 0 142 | steeering = 1e6 143 | 144 | """ Complementary filter (IMU) initialization """ 145 | module = 0 146 | iangle=0 147 | anglex_buff = np.zeros(10) 148 | angley_buff = np.zeros(10) 149 | zeroangle_x = 0.0338 150 | zeroangle_y = -0.0594 151 | angle_count = 1 # number of samples to calculate average angles 152 | Tcomp = 0.02 153 | angle = np.zeros(2) 154 | Angle =np.zeros(2) 155 | Angle_old = np.zeros(2) 156 | 157 | 158 | """ Counter for Battery check initialization """ 159 | Bat = 0 #counter for battery check 160 | 161 | """Horizontal Compensation PID Gains """ 162 | Kp = 4 163 | Ki = 22 #was up to 22 164 | Kd = 0 # was up to 0.08 165 | 166 | """Servo trimming """ 167 | xtlf = 14 168 | ytlf = 0 169 | ztlf = 0 170 | 171 | xtrf = 14 172 | ytrf = 0 173 | ztrf = 3 174 | 175 | xtrr = 14 176 | ytrr = 0 177 | ztrr = 0 178 | 179 | xtlr = 14 180 | ytlr = 0 181 | ztlr = 0 182 | 183 | 184 | """ Main loop intialization """ 185 | continuer = True 186 | clock = pygame.time.Clock() 187 | t = 0 #Initializing timing/clock 188 | tstart = 1 #End of start sequence 189 | tstop = 1000 #Start of stop sequence by default 190 | trans = 0 191 | transtep = 0.025 192 | 193 | """ Display Management """ 194 | DISPLAY_TEXT_ADDR = 0x3e 195 | DISPLAY_RGB_ADDR = 0x60 196 | 197 | 198 | def setRGB(r,g,b): 199 | i2c.writeto (DISPLAY_RGB_ADDR,bytes([0x00,0x00]),stop=False) 200 | i2c.writeto (DISPLAY_RGB_ADDR,bytes([0x01,0x00]),stop=False) 201 | i2c.writeto (DISPLAY_RGB_ADDR,bytes([0x08,0xaa]),stop=False) 202 | i2c.writeto (DISPLAY_RGB_ADDR,bytes([4,r]),stop=False) 203 | i2c.writeto (DISPLAY_RGB_ADDR,bytes([3,g]),stop=False) 204 | i2c.writeto (DISPLAY_RGB_ADDR,bytes([2,b]),stop=False) 205 | 206 | 207 | def textCommand(cmd): 208 | msg = [0x80]+cmd 209 | i2c.writeto (DISPLAY_TEXT_ADDR,bytes(msg), stop = False) 210 | 211 | def setText(text): 212 | textCommand([0x01]) # clear display 213 | sleep(.05) 214 | textCommand([0x08 | 0x04 | 0x00 | 0x00]) # display on, no cursor 215 | #0x02 cursor 216 | #0x01 Cursor Blinking 217 | textCommand([0x28]) # 2 lines 218 | sleep(.05) 219 | count = 0 220 | row = 0 221 | for c in text: 222 | if c == '\n' or count == 16: 223 | count = 0 224 | row += 1 225 | if row == 2: 226 | break 227 | textCommand([0xc0])#switch to second line (address 0x40 to 0x67) 228 | #01xxxxxxx 229 | #First line addresses are 0x00 to 0x27) 230 | if c == '\n': 231 | continue 232 | count += 1 233 | msg = [0x40]+[ord(c)] 234 | i2c.writeto (DISPLAY_TEXT_ADDR,bytes(msg), stop = False) 235 | 236 | 237 | """ Angle measurement Complementary filter """ 238 | 239 | def comp_filter (angle,t,T): 240 | #Complementary filter calculates body angles around xt and y axis from IMU data 241 | acc = mpu.acceleration 242 | gyr = mpu.gyro 243 | denb = sqrt(acc[1]**2+acc[2]**2) 244 | dena = sqrt(acc[2]**2+acc[0]**2) 245 | 246 | if (dena == 0): 247 | alpha = 0 248 | else: 249 | alpha = atan (acc[1]/dena) 250 | 251 | if (denb == 0): 252 | beta = 0 253 | else: 254 | beta = atan (acc[0]/denb) 255 | 256 | A = T/(T+t) 257 | 258 | anglex = A*(angle[0]+t*gyr[0]/180*pi)+(1-A)*alpha 259 | angley = A*(angle[1]+t*gyr[1]/180*pi)+(1-A)*beta 260 | 261 | return [anglex, angley] 262 | 263 | 264 | 265 | def moving (pos,move): 266 | thetalf_reply = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[0]+xtlf, pos[1]+ytlf, pos[2]+ztlf, 1) 267 | thetarf_reply = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[3]+xtrf, pos[4]+ytrf, pos[5]+ztrf, -1) 268 | thetarr_reply = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[6]+xtrr, pos[7]+ytrr, pos[8]+ztrr, -1) 269 | thetalr_reply = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[9]+xtlr, pos[10]+ytlr, pos[11]+ztlr, 1) 270 | 271 | thetalf = thetalf_reply[0] 272 | thetarf = thetarf_reply[0] 273 | thetarr = thetarr_reply[0] 274 | thetalr = thetalr_reply[0] 275 | 276 | 277 | if move == True: 278 | if (thetalf_reply[1]==False): 279 | try: 280 | kit.servo[Spot.servo_table[0]].angle = thetalf[0]/pi*180*Spot.angle_scale_factor_lf1*Spot.dir01+Spot.zero01 281 | kit.servo[Spot.servo_table[1]].angle = thetalf[1]/pi*180*Spot.angle_scale_factor_lf2*Spot.dir02+Spot.zero02 282 | kit.servo[Spot.servo_table[2]].angle = thetalf[2]/pi*180*Spot.angle_scale_factor_lf3*Spot.dir03+Spot.zero03 283 | except ValueError: 284 | print ('Angle out of Range') 285 | 286 | 287 | if (thetarf_reply[1]==False): 288 | try: 289 | kit.servo[Spot.servo_table[3]].angle = thetarf[0]/pi*180*Spot.angle_scale_factor_rf1*Spot.dir04+Spot.zero04 290 | kit.servo[Spot.servo_table[4]].angle = thetarf[1]/pi*180*Spot.angle_scale_factor_rf2*Spot.dir05+Spot.zero05 291 | kit.servo[Spot.servo_table[5]].angle = thetarf[2]/pi*180*Spot.angle_scale_factor_rf3*Spot.dir06+Spot.zero06 292 | except ValueError: 293 | print ('Angle out of Range') 294 | 295 | if (thetarr_reply[1]==False): 296 | try: 297 | kit.servo[Spot.servo_table[6]].angle = thetarr[0]/pi*180*Spot.angle_scale_factor_rr1*Spot.dir07+Spot.zero07 298 | kit.servo[Spot.servo_table[7]].angle = thetarr[1]/pi*180*Spot.angle_scale_factor_rr2*Spot.dir08+Spot.zero08 299 | kit.servo[Spot.servo_table[8]].angle = thetarr[2]/pi*180*Spot.angle_scale_factor_rr3*Spot.dir09+Spot.zero09 300 | except ValueError: 301 | print ('Angle out of Range') 302 | 303 | if (thetalr_reply[1]==False): 304 | try: 305 | kit.servo[Spot.servo_table[9]].angle = thetalr[0]/pi*180*Spot.angle_scale_factor_lr1*Spot.dir10+Spot.zero10 306 | kit.servo[Spot.servo_table[10]].angle = thetalr[1]/pi*180*Spot.angle_scale_factor_lr2*Spot.dir11+Spot.zero11 307 | kit.servo[Spot.servo_table[11]].angle = thetalr[2]/pi*180*Spot.angle_scale_factor_lr3*Spot.dir12+Spot.zero12 308 | except ValueError: 309 | print ('Angle out of Range') 310 | 311 | """ i2C Initialization""" 312 | i2c= busio.I2C(board.SCL, board.SDA) 313 | pca = adafruit_pca9685.PCA9685(i2c) 314 | mpu = adafruit_mpu6050.MPU6050(i2c) 315 | pca.frequency = 50 316 | kit = ServoKit(channels=16) 317 | ads = ADS.ADS1015(i2c) 318 | ads.gain = 2/3 319 | 320 | """PWM range setting """ 321 | for i in range(0,12): 322 | kit.servo[Spot.servo_table[i]].set_pulse_width_range(500, 2500) 323 | 324 | 325 | """ """ 326 | 327 | 328 | """ Main Program """ 329 | 330 | 331 | """ """ 332 | 333 | 334 | """ Joystick Init """ 335 | pygame.joystick.init() 336 | joystick = pygame.joystick.Joystick(0) 337 | joystick.init() 338 | 339 | """set screen background color""" 340 | setRGB (127,127,255) 341 | 342 | 343 | """ Initialize legs and body positions """ 344 | x_spot = [0, x_offset, Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr,0,0,0,0] 345 | y_spot = [0,0,Spot.ylf+track, Spot.yrf-track, Spot.yrr-track, Spot.ylr+track,0,0,0,0] 346 | z_spot = [0,b_height,0,0,0,0,0,0,0,0] 347 | theta_spot = [0,0,0,0,0,0] 348 | 349 | 350 | """theta_spot = [x angle ground, y angle ground, z angle body in space, x angle body, y angle body, z angle body] """ 351 | #theta xyz of ground then theta xyz of frame/body 352 | pos_init = [-x_offset,track,-b_height,-x_offset,-track,-b_height,-x_offset,-track,-b_height,-x_offset,track,-b_height] 353 | 354 | thetalf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[0], pos_init[1], pos_init[2], 1)[0] 355 | thetarf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[3], pos_init[4], pos_init[5], -1)[0] 356 | thetarr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[6], pos_init[7], pos_init[8], -1)[0] 357 | thetalr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos_init[9], pos_init[10], pos_init[11], 1)[0] 358 | 359 | CG = SpotCG.CG_calculation (thetalf,thetarf,thetarr,thetalr) 360 | ZMP = [CG[0],CG[1]] # ZMP is initialized to CG position 361 | #Calculation of CG absolute position 362 | M = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],theta_spot[2],False) 363 | CGabs = Spot.new_coordinates(M,CG[0],CG[1],CG[2],x_spot[1],y_spot[1],z_spot[1]) 364 | dCG = SpotCG.CG_distance(x_spot[2:6],y_spot[2:6],z_spot[2:6],CGabs[0],CGabs[1],stance) 365 | 366 | x_spot = [0, x_offset, Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr,CG[0],CGabs[0],dCG[1],ZMP[0]] 367 | y_spot = [0,0,Spot.ylf+track, Spot.yrf-track, Spot.yrr-track, Spot.ylr+track,CG[1],CGabs[1],dCG[2],ZMP[1]] 368 | z_spot = [0,b_height,0,0,0,0,CG[2],CGabs[2],dCG[3],CGabs[2]] 369 | 370 | pos = [-x_offset,track,-b_height,-x_offset,-track,-b_height,-x_offset,-track,-b_height,-x_offset,track,-b_height,theta_spot,x_spot,y_spot,z_spot] 371 | 372 | #Read Battery Voltage 373 | 374 | chan = AnalogIn(ads, ADS.P0) 375 | 376 | chans='Bat: '+('%.2f' % (chan.voltage*2.0035))+' V' 377 | 378 | 379 | 380 | """ 381 | Main Loop 382 | 383 | """ 384 | 385 | 386 | disptext = 'Ready ! ' 387 | setText(disptext+chans) 388 | 389 | tt = time() #initialize time for speed and acceleration calculation 390 | joystart_rightpaw = True 391 | joystart_leftpaw = True 392 | 393 | while (continuer): 394 | clock.tick(30) 395 | angle = comp_filter (angle,tstep,Tcomp) 396 | anglex_buff[iangle] = angle[0]+zeroangle_x 397 | angley_buff[iangle] = angle[1]+zeroangle_y 398 | Angle_old = Angle 399 | Angle =[np.mean(anglex_buff),np.mean(angley_buff)] 400 | iangle = iangle+1 401 | if (iangle == angle_count): 402 | iangle =0 403 | 404 | 405 | for event in pygame.event.get(): # User did something. 406 | if event.type == pygame.QUIT: # If user clicked close. 407 | continuer = False 408 | if event.type == pygame.MOUSEBUTTONDOWN: 409 | mouseclick = True 410 | else: 411 | mouseclick = False 412 | 413 | for i in range (0,6): #read analog joystick position 414 | joypos[i] = joystick.get_axis(i) 415 | 416 | if (joystart_leftpaw==True)&(joypos[pos_leftpaw]==0): 417 | joypos[pos_leftpaw]= -1 418 | else: 419 | joystart_leftpaw = False 420 | 421 | if (joystart_rightpaw==True)&(joypos[pos_rightpaw]==0): 422 | joypos[pos_rightpaw]= -1 423 | else: 424 | joystart_rightpaw = False 425 | 426 | 427 | for i in range (0,15): #read buttons 428 | joybut[i] = joystick.get_button(i) 429 | joyhat = joystick.get_hat(0) #read hat 430 | 431 | 432 | 433 | """Animation""" 434 | 435 | if (joybut[but_walk] == 0)&(joybut[but_sit] == 0)&(joybut[but_pee] == 0)&(joybut[but_lie] == 0)&(joybut[but_twist] == 0)&(joybut[but_move] == 0)&(joybut[but_anim] == 0)&(lock == True): 436 | lock = False 437 | 438 | #WALKING 439 | if (joybut[but_walk] == 1)&(walking == True)&(stop == False)&(lock == False): #Quit walk mode 440 | stop = True 441 | lock = True 442 | if (abs(t-int(t))<=tstep): 443 | tstop = int(t) 444 | else: 445 | tstop = int(t)+1 446 | 447 | if (t==0): 448 | tstop = 1 449 | 450 | if (joybut[but_walk] == 1)&(walking == False)&(Free == True): #Enter in walk mode 451 | walking = True 452 | stop = False 453 | Free = False 454 | t=0 455 | tstart = 1 456 | tstop = 1000 457 | lock = True 458 | trec = int(t) 459 | setRGB (146,208,80) 460 | disptext ='Walking... ' 461 | setText(disptext+chans) 462 | 463 | #SITTING and GIVING PAW 464 | if (joybut[but_sit] == 1)&(sitting == False)&(Free == True): #Enter in sitting mode 465 | sitting = True 466 | stop = False 467 | Free = False 468 | t=0 469 | lock = True 470 | setRGB (255,153,51) 471 | disptext ='Sitting... ' 472 | setText(disptext+chans) 473 | 474 | if (joybut[but_sit] == 1)&(sitting == True)&(stop == False)&(lock == False): #Quit sitting mode 475 | stop = True 476 | lock = True 477 | 478 | 479 | #SHIFTING and PEEING 480 | if (joybut[but_pee] == 1)&(shifting == False)&(Free == True): #Enter in sitting mode 481 | shifting = True 482 | stop = False 483 | Free = False 484 | t=0 485 | lock = True 486 | setRGB (255,255,255) 487 | disptext ='Peeing... ' 488 | setText(disptext+chans) 489 | 490 | if (joybut[but_pee] == 1)&(shifting == True)&(stop == False)&(lock == False): #Quit sitting mode 491 | stop = True 492 | lock = True 493 | 494 | 495 | #LYING 496 | if (joybut[but_lie] == 1)&(lying == False)&(Free == True): #Enter in sitting mode 497 | lying = True 498 | stop = False 499 | Free = False 500 | t=0 501 | lock = True 502 | setRGB (204,102,255) 503 | disptext ='Lying... ' 504 | setText(disptext+chans) 505 | 506 | if (joybut[but_lie] == 1)&(lying== True)&(stop == False)&(lock == False): #Quit sitting mode 507 | stop = True 508 | lock = True 509 | 510 | 511 | 512 | #TWISTING 513 | if (joybut[but_twist] == 1)&(twisting == False)&(Free == True): #Enter in sitting mode 514 | twisting = True 515 | Free = False 516 | t=0 517 | lock = True 518 | setRGB (255,0,0) 519 | disptext ='Twisting... ' 520 | setText(disptext+chans) 521 | 522 | 523 | 524 | if (walking == True): 525 | coef = 1.2 526 | #set walking direction and speed 527 | #set steering radius 528 | if (abs(joypos[pos_leftright])>0.2)|(abs(joypos[pos_frontrear])>0.2)|(stop == True): 529 | t=t+tstep 530 | trec = int(t)+1 531 | 532 | module_old = module 533 | walking_direction_old = walking_direction 534 | steering_old = steering 535 | 536 | x_old = module_old*cos(walking_direction_old) 537 | y_old = module_old*sin(walking_direction_old) 538 | 539 | #update request 540 | module = sqrt(joypos[pos_leftright]**2 + joypos[pos_frontrear]**2) 541 | walking_direction = (atan2(-joypos[pos_leftright],-joypos[pos_frontrear])%(2*pi)+pi/2)%(2*pi) 542 | 543 | x_new = module*cos(walking_direction) 544 | y_new = module*sin(walking_direction) 545 | 546 | #steering update 547 | if (abs(joypos[pos_turn]) < 0.2): 548 | cw = 1 549 | if (steering<2000): 550 | steering = min(1e6,steering_old*coef) 551 | else: 552 | steering = 1e6 553 | else: 554 | steering = 2000-(abs(joypos[pos_turn])-0.2)*2000/0.8+0.001 555 | if ((steering/steering_old)>coef): 556 | steering = steering_old*coef 557 | if ((steering_old/steering)>coef): 558 | steering = steering_old/coef 559 | if (steering <0.001): 560 | steering = 0.001 561 | cw = -np.sign(joypos[pos_turn]) 562 | 563 | 564 | gap = sqrt((x_new-x_old)**2+(y_new-y_old)**2) 565 | 566 | if (gap>0.01): 567 | x_new = x_old+ (x_new-x_old)/gap*0.01 568 | y_new = y_old+ (y_new-y_old)/gap*0.01 569 | module = sqrt(x_new**2+y_new**2) 570 | walking_direction = atan2(y_new,x_new) 571 | 572 | #reduces speed sideways and backwards 573 | min_h_amp = h_amp*(1/2e6*steering+1/2) 574 | xa = 1+cos(walking_direction-pi/2) 575 | walking_speed = min (1, module) * min(h_amp,min_h_amp) * (1/8*xa**2+1/8*xa+1/4) 576 | 577 | 578 | if ((abs(joypos[pos_leftright])<0.2)&(abs(joypos[pos_frontrear])<0.2))&(stop == False): 579 | t=t+tstep 580 | module = max (0, module-0.01) 581 | walking_speed = module* h_amp * ((1+cos(walking_direction-pi/2))/2*0.75+0.25) 582 | if (steering<2000): 583 | steering = min(1e6,steering*coef) 584 | else: 585 | steering = 1e6 586 | cw=1 587 | if (t>trec): 588 | t=trec 589 | 590 | 591 | if (joypos[pos_rightpaw]>= joybendover): 592 | joybendover = min (joypos[pos_rightpaw],joybendover+0.1) 593 | else: 594 | joybendover = max (joypos[pos_rightpaw],joybendover-0.1) 595 | 596 | 597 | 598 | if (joypos[pos_leftpaw]>= joyleanback): 599 | joyleanback = min (joypos[pos_leftpaw],joyleanback+0.1) 600 | else: 601 | joyleanback = max (joypos[pos_leftpaw],joyleanback-0.1) 602 | 603 | 604 | front_height = (1-joybendover)*25+b_height-50 605 | rear_height = (1-joyleanback)*25+b_height-50 606 | 607 | height = (front_height+rear_height)/2 608 | bend_angle = atan ((front_height-rear_height)/Spot.Lb) 609 | 610 | if (joybut[but_sit] == 1)&(lock == False): #toggle angle compensation with IMU 611 | IMU_Comp = not (IMU_Comp) 612 | Integral_Angle = [0,0] 613 | lock = True 614 | 615 | if (joybut[but_lie] == 1)&(lock == False)&(t%1==0): 616 | if (trot == False): 617 | trot = True 618 | stepl = stepl2 619 | h_amp = h_amp2 620 | v_amp = v_amp2 621 | track = track2 622 | tstep = tstep2 623 | else: 624 | trot = False 625 | stepl = stepl4 626 | h_amp = h_amp4 627 | v_amp = v_amp4 628 | track = track4 629 | tstep = tstep4 630 | lock = True 631 | 632 | if (trot == True): 633 | trans = min(1,trans+transtep); 634 | 635 | if (trot == False): 636 | trans = max(0,trans-transtep); 637 | 638 | 639 | if (IMU_Comp == True): 640 | Integral_Angle[0] = Integral_Angle[0]+Angle[0]*dtt 641 | Integral_Angle[1] = Integral_Angle[1]+Angle[1]*dtt 642 | if (dtt != 0): 643 | Derivative_AngleX = (Angle[0]-Angle_old[0])/dtt 644 | Derivative_AngleY = (Angle[1]-Angle_old[1])/dtt 645 | else: 646 | Derivative_AngleX = 0 647 | Derivative_AngleY = 0 648 | 649 | theta_spot[3] = -(Kp*Angle[0]+Ki*Integral_Angle[0]+Kd*Derivative_AngleX) 650 | theta_spot[4] = (Kp*Angle[1]+Ki*Integral_Angle[1]+Kd*Derivative_AngleY) + bend_angle 651 | theta_spot[0] = Angle[0] 652 | theta_spot[1] = -Angle[1] 653 | x_offset = (height+CG[2])*sin(Kp*Angle[1]+Ki*Integral_Angle[1]+Kd*Derivative_AngleY) 654 | else: 655 | theta_spot[3] = 0 656 | theta_spot[4] = bend_angle 657 | theta_spot[0] = 0 658 | theta_spot[1] = 0 659 | x_offset = 0 660 | 661 | if (t< tstart): 662 | """ start """ 663 | phase = 1+trans 664 | else: 665 | if (t(tstop+1-tstep)): 679 | stop = False 680 | walking = False 681 | Free = True 682 | setRGB (127,127,255) 683 | disptext ='Waiting... ' 684 | setText(disptext+chans) 685 | stepl = stepl4 686 | h_amp = h_amp4 687 | v_amp = v_amp4 688 | track = track4 689 | tstep = tstep4 690 | trans = 0 691 | trot = False 692 | print ('waiting') 693 | 694 | if (sitting == True): 695 | alpha_sitting = -30/180*pi 696 | alpha_pawing = 0/180*pi 697 | L_paw = 220 698 | 699 | x_end_sitting = Spot.xlr-Spot.L2 + Spot.L1*cos(pi/3) +Spot.Lb/2*cos(-alpha_sitting) - Spot.d*sin (-alpha_sitting) 700 | z_end_sitting = Spot.L1*sin(pi/3)+ Spot.Lb/2*sin(-alpha_sitting) + Spot.d*cos(-alpha_sitting) 701 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 702 | 703 | #end_frame_pos = [0,0,0,x_offset,0,b_height-20] # x,y,z rotations then translations 704 | end_frame_pos = [0,alpha_sitting,0, x_end_sitting,0,z_end_sitting] # x,y,z rotations then translations 705 | pos = Spot.moving (t, start_frame_pos,end_frame_pos, pos) 706 | 707 | if (t==1)&(pawing == False): 708 | pos_sit_init = pos 709 | 710 | if (t == 1): #pawing is possible 711 | #paw position is define by joystick 2 712 | if (pawing == True): 713 | #print (pos_sit_init[3],pos_sit_init[5]) 714 | pos[3] = pos_sit_init[3]+ (L_paw*cos(alpha_pawing)-pos_sit_init[3])*(joypar+1)/2 715 | pos[5] = pos_sit_init[5]+ (-Spot.d-L_paw*sin(alpha_pawing)-pos_sit_init[5])*(joypar+1)/2 716 | 717 | pos[0] = pos_sit_init[0]+ (L_paw*cos(alpha_pawing)-pos_sit_init[0])*(joypal+1)/2 718 | pos[2] = pos_sit_init[2]+ (-Spot.d-L_paw*sin(alpha_pawing)-pos_sit_init[2])*(joypal+1)/2 719 | 720 | thetarf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[3], pos[4], pos[5], -1)[0] 721 | thetalf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[0], pos[1], pos[2], -1)[0] 722 | #update of right front leg absolute position 723 | legrf = Spot.FK(thetarf,-1) 724 | leglf = Spot.FK(thetalf,-1) 725 | xlegrf =Spot.xrf+pos[3] 726 | ylegrf =Spot.yrf+pos[4] 727 | zlegrf =pos[5] 728 | xleglf =Spot.xlf+pos[0] 729 | yleglf =Spot.ylf+pos[1] 730 | zleglf =pos[2] 731 | 732 | theta_spot_sit = pos[12] 733 | 734 | x_spot_sit = pos[13] 735 | y_spot_sit = pos[14] 736 | z_spot_sit = pos[15] 737 | 738 | M = Spot.xyz_rotation_matrix(theta_spot_sit[3],theta_spot_sit[4],theta_spot_sit[2]+theta_spot_sit[5],False) 739 | 740 | paw_rf = Spot.new_coordinates(M,xlegrf,ylegrf,zlegrf,x_spot_sit[1],y_spot_sit[1],z_spot_sit[1]) 741 | paw_lf = Spot.new_coordinates(M,xleglf,yleglf,zleglf,x_spot_sit[1],y_spot_sit[1],z_spot_sit[1]) 742 | 743 | x_spot_sit[3] = paw_rf[0] 744 | y_spot_sit[3] = paw_rf[1] 745 | z_spot_sit[3] = paw_rf[2] 746 | x_spot_sit[2] = paw_lf[0] 747 | y_spot_sit[2] = paw_lf[1] 748 | z_spot_sit[2] = paw_lf[2] 749 | 750 | 751 | pos[13] = x_spot_sit 752 | pos[14] = y_spot_sit 753 | pos[15] = z_spot_sit 754 | 755 | joypar_old = joypar 756 | if (joypal == -1): 757 | if (((joypos[pos_rightpaw] != 0)&(joypos[pos_rightpaw] !=-1))|(joypar != -1)): 758 | pawing = True 759 | if (joypos[pos_rightpaw]>= joypar): 760 | joypar = min (joypos[pos_rightpaw],joypar+0.05) 761 | else: 762 | joypar = max (joypos[pos_rightpaw],joypar-0.05) 763 | else: 764 | pawing = False 765 | 766 | if (joypar_old == -1): 767 | if (((joypos[pos_leftpaw] != 0)&(joypos[pos_leftpaw] !=-1))|(joypal != -1)): 768 | pawing = True 769 | if (joypos[pos_leftpaw]>= joypal): 770 | joypal = min (joypos[pos_leftpaw],joypal+0.05) 771 | else: 772 | joypal = max (joypos[pos_leftpaw],joypal-0.05) 773 | else: 774 | pawing = False 775 | 776 | 777 | if (stop == False): 778 | t=t+4*tstep 779 | if (t>=1): 780 | t= 1 781 | elif (pawing == False): 782 | t=t-4*tstep 783 | if (t<= 0): 784 | t= 0 785 | stop = False 786 | sitting = False 787 | Free = True 788 | setRGB (127,127,255) 789 | disptext ='Waiting... ' 790 | setText(disptext+chans) 791 | 792 | if (shifting == True): 793 | x_end_shifting = ra_longi 794 | y_end_shifting = -ra_lat 795 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 796 | end_frame_pos = [0,0,0, x_end_shifting+x_offset,y_end_shifting,b_height] # x,y,z rotations then translations 797 | pos = Spot.moving (t, start_frame_pos,end_frame_pos, pos) 798 | 799 | if (t==1)&(peeing == False): 800 | pos_shift_init = pos 801 | 802 | if (t == 1): #pawing is possible 803 | #paw position is define by joystick 2 804 | 805 | if (peeing == True): 806 | print (joype) 807 | pos[9] = pos_shift_init[9]+ (0-pos_shift_init[9])*(joype+1)/2 808 | pos[10] = pos_shift_init[10]+ (130-pos_shift_init[10])*(joype+1)/2 809 | pos[11] = pos_shift_init[11]+ (-20-pos_shift_init[11])*(joype+1)/2 810 | 811 | thetalr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[9], pos[10], pos[11], 1)[0] 812 | #update of right front leg absolute position 813 | leglr = Spot.FK(thetalr,1) 814 | xleglr =Spot.xlr+pos[9] 815 | yleglr =Spot.ylr+pos[10] 816 | zleglr =pos[11] 817 | theta_spot_shift = pos[12] 818 | x_spot_shift = pos[13] 819 | y_spot_shift = pos[14] 820 | z_spot_shift = pos[15] 821 | M = Spot.xyz_rotation_matrix(theta_spot_shift[3],theta_spot_shift[4],theta_spot_shift[2]+theta_spot_shift[5],False) 822 | pee_lr = Spot.new_coordinates(M,xleglr,yleglr,zleglr,x_spot_shift[1],y_spot_shift[1],z_spot_shift[1]) 823 | 824 | x_spot_shift[5] = pee_lr[0] 825 | y_spot_shift[5] = pee_lr[1] 826 | z_spot_shift[5] = pee_lr[2] 827 | pos[13] = x_spot_shift 828 | pos[14] = y_spot_shift 829 | pos[15] = z_spot_shift 830 | setRGB (255,255,int(255*(1-joype)/2)) 831 | 832 | #if (joypos[pos_leftpaw] == -1)&(joype == -1): 833 | #peeing = False 834 | 835 | if ((joypos[pos_leftpaw] != 0)&(joypos[pos_leftpaw] !=-1))|(joype != -1): 836 | peeing = True 837 | if (joypos[pos_leftpaw]>= joype): 838 | joype = min (joypos[pos_leftpaw],joype+0.1) 839 | else: 840 | joype = max (joypos[pos_leftpaw],joype-0.1) 841 | else: 842 | peeing = False 843 | 844 | 845 | 846 | 847 | if (stop == False): 848 | t=t+4*tstep 849 | if (t>=1): 850 | t= 1 851 | elif (peeing == False): 852 | t=t-4*tstep 853 | if (t<= 0): 854 | t= 0 855 | stop = False 856 | shifting = False 857 | Free = True 858 | setRGB (127,127,255) 859 | disptext ='Waiting... ' 860 | setText(disptext+chans) 861 | 862 | 863 | if (lying == True): 864 | angle_lying = 40/180*pi 865 | x_end_lying= Spot.xlr-Spot.L2 + Spot.L1*cos(angle_lying)+Spot.Lb/2 866 | z_end_lying = Spot.L1*sin(angle_lying)+Spot.d 867 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 868 | end_frame_pos = [0,0,0, x_end_lying,0,z_end_lying] # x,y,z rotations then translations 869 | pos = Spot.moving (t, start_frame_pos,end_frame_pos, pos) 870 | if (stop == False): 871 | t=t+3*tstep 872 | if (t>=1): 873 | t= 1 874 | else: 875 | t=t-3*tstep 876 | if (t<= 0): 877 | t= 0 878 | stop = False 879 | lying = False 880 | Free = True 881 | setRGB (127,127,255) 882 | disptext ='Waiting... ' 883 | setText(disptext+chans) 884 | 885 | if (twisting == True): 886 | x_angle_twisting = 0/180*pi 887 | y_angle_twisting = 0/180*pi 888 | z_angle_twisting = 30/180*pi 889 | start_frame_pos = [0,0,0,x_offset,0,b_height] # x,y,z rotations then translations 890 | 891 | t=t+4*tstep 892 | if (t>=1): 893 | t=1 894 | twisting = False 895 | Free = True 896 | setRGB (127,127,255) 897 | disptext ='Waiting... ' 898 | setText(disptext+chans) 899 | 900 | if (t<0.25): 901 | end_frame_pos = [x_angle_twisting,y_angle_twisting,z_angle_twisting, x_offset,0,b_height] # x,y,z rotations then translations 902 | pos = Spot.moving (t*4, start_frame_pos,end_frame_pos, pos) 903 | if (t>=0.25)&(t<0.5): 904 | end_frame_pos = [x_angle_twisting,y_angle_twisting,z_angle_twisting, x_offset,0,b_height] # x,y,z rotations then translations 905 | pos = Spot.moving ((t-0.25)*4, end_frame_pos,start_frame_pos, pos) 906 | if (t>=0.5)&(t<0.75): 907 | end_frame_pos = [-x_angle_twisting,-y_angle_twisting,-z_angle_twisting, x_offset,0,b_height] 908 | pos = Spot.moving ((t-0.5)*4, start_frame_pos,end_frame_pos, pos) 909 | if (t>=0.75)&(t<=1): 910 | end_frame_pos = [-x_angle_twisting,-y_angle_twisting,-z_angle_twisting, x_offset,0,b_height] 911 | pos = Spot.moving ((t-0.75)*4, end_frame_pos,start_frame_pos, pos) 912 | 913 | 914 | xc = steering* cos(walking_direction) 915 | yc = steering* sin(walking_direction) 916 | 917 | center_x = x_spot[0]+(xc*cos(theta_spot[2])-yc*sin(theta_spot[2])) #absolute center x position 918 | center_y = y_spot[0]+(xc*sin(theta_spot[2])+yc*cos(theta_spot[2])) #absolute center y position 919 | 920 | thetalf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[0], pos[1], pos[2], 1)[0] 921 | thetarf = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[3], pos[4], pos[5], -1)[0] 922 | thetarr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[6], pos[7], pos[8], -1)[0] 923 | thetalr = Spot.IK(Spot.L0, Spot.L1, Spot.L2, Spot.d, pos[9], pos[10], pos[11], 1)[0] 924 | 925 | stance = [False, False, False, False] 926 | if (pos[15][2] < 10.01): 927 | stance[0] = True 928 | if (pos[15][3] < 10.01): 929 | stance[1] = True 930 | if (pos[15][4] < 10.01): 931 | stance[2] = True 932 | if (pos[15][5] < 10.01): 933 | stance[3] = True 934 | 935 | if (anim == True): 936 | SpotAnim.animate(pos,t,pi/12,-135/180*pi,Angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance) 937 | #SpotAnim.animate(pos,t,pi/2,-0/180*pi,Angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance) 938 | #SpotAnim.animate(pos,t,0,-0/180*pi,Angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance) 939 | 940 | mouse = pygame.mouse.get_pos() 941 | 942 | if (joybut[but_anim] == 1)&(lock == False): 943 | anim =not(anim) 944 | lock = True 945 | 946 | if (joybut[but_move] == 1)&(Free == True)&(lock == False): 947 | move = not(move) 948 | lock = True 949 | 950 | if ((mouse[0]>=510)&(mouse[0]<=590)&(mouse[1]>=500)&(mouse[1]<=540)): 951 | pygame.draw.rect(screen,SpotAnim.BLACK,[510,500,80,40], 5) 952 | if (mouseclick == True)&(lockmouse == False): 953 | lockmouse = True 954 | anim = not(anim) 955 | else: 956 | pygame.draw.rect(screen,SpotAnim.WHITE,[510,500,80,40], 5) 957 | 958 | if ((mouse[0]>=510)&(mouse[0]<=590)&(mouse[1]>=550)&(mouse[1]<=590)): 959 | pygame.draw.rect(screen,SpotAnim.BLACK,[510,550,80,40], 5) 960 | if (mouseclick == True)&(lockmouse == False)&(Free == True): 961 | lockmouse = True 962 | move = not(move) 963 | else: 964 | pygame.draw.rect(screen,SpotAnim.WHITE,[510,550,80,40], 5) 965 | 966 | if (mouseclick == False)&(lockmouse == True): 967 | lockmouse = False 968 | 969 | if (anim == True): 970 | pygame.draw.rect(screen,SpotAnim.GREEN,[510,500,80,40]) 971 | screen.blit (text_animon, (520,510)) 972 | else: 973 | pygame.draw.rect(screen,SpotAnim.RED,[510,500,80,40]) 974 | screen.blit (text_animoff, (520,510)) 975 | 976 | if (move == True): 977 | pygame.draw.rect(screen,SpotAnim.GREEN,[510,550,80,40]) 978 | screen.blit (text_moveon, (520,560)) 979 | else: 980 | pygame.draw.rect(screen,SpotAnim.RED,[510,550,80,40]) 981 | screen.blit (text_moveoff, (520,560)) 982 | 983 | 984 | pygame.display.flip() 985 | if (Free == True): 986 | sleep(0.1) 987 | moving (pos, move) 988 | 989 | """ CG update """ 990 | CG = SpotCG.CG_calculation (thetalf,thetarf,thetarr,thetalr) 991 | #Calculation of CG absolute position 992 | M = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],theta_spot[2],False) 993 | CGabs = Spot.new_coordinates(M,CG[0],CG[1],CG[2],x_spot[1],y_spot[1],z_spot[1]) 994 | 995 | 996 | """ Data Logging """ 997 | ta = time() 998 | dtt = ta-tt 999 | tt = ta 1000 | sCGo = sCG 1001 | sCG = [(CG[0]-pos[13][6])/dtt,(CG[1]-pos[14][6])/dtt] # in mm/s 1002 | aCG = [(sCG[0]-sCGo[0])/dtt,(sCG[1]-sCGo[1])/dtt] # in mm/s² 1003 | ZMP = [CG[0]-CGabs[2]/9810*aCG[0],CG[1]-CGabs[2]/9810*aCG[1]] # in mm 1004 | dCG = SpotCG.CG_distance(x_spot[2:6],y_spot[2:6],z_spot[2:6],CGabs[0],CGabs[1],stance) 1005 | 1006 | if (sum(stance)>2): 1007 | xZMP.append(0) 1008 | yZMP.append(0) 1009 | xCG.append(0) 1010 | yCG.append(0) 1011 | 1012 | else: 1013 | xZMP.append(ZMP[0]) 1014 | yZMP.append(ZMP[1]) 1015 | xCG.append(CG[0]) 1016 | yCG.append(CG[1]) 1017 | 1018 | 1019 | #Support line center calculation 1020 | xce=0 1021 | yce=0 1022 | if (stance[0] == False)|(stance[2] == False): 1023 | xce= (Spot.xrf+pos[3]+Spot.xlr+pos[9])/2 1024 | yce= (Spot.yrf+pos[4]+Spot.ylr+pos[10])/2 1025 | 1026 | if (stance[1] == False)|(stance[3] == False): 1027 | xce= (Spot.xlf+pos[0]+Spot.xrr+pos[6])/2 1028 | yce= (Spot.ylf+pos[1]+Spot.yrr+pos[7])/2 1029 | 1030 | xCenter.append(xce) 1031 | yCenter.append(yce) 1032 | 1033 | 1034 | """Center of Gravity Data update """ 1035 | pos[13][6] = CG[0] #x 1036 | pos[14][6] = CG[1] #y 1037 | pos[15][6] = CG[2] #z 1038 | 1039 | pos[13][7] = CGabs[0] #x 1040 | pos[14][7] = CGabs[1] #y 1041 | pos[15][7] = CGabs[2] #z 1042 | 1043 | pos[13][8] = dCG[1] #xint 1044 | pos[14][8] = dCG[2] #yint 1045 | pos[15][8] = dCG[3] #balance 1046 | 1047 | pos[13][9] = ZMP[0] #xint 1048 | pos[14][9] = ZMP[1] #yint 1049 | pos[15][9] = CGabs[2] #balance 1050 | 1051 | distance.append(dCG[0]) 1052 | timing.append(t) 1053 | Bat = Bat+1 1054 | if (Bat == 30): #update Battery voltage 1055 | chan = AnalogIn(ads, ADS.P0) 1056 | chans='Bat: '+('%.2f' % (chan.voltage*2.0035))+' V' 1057 | Bat =0 1058 | 1059 | setText('') 1060 | setRGB (0,0,0) 1061 | pygame.quit() 1062 | plt.plot(timing,distance) 1063 | 1064 | 1065 | 1066 | -------------------------------------------------------------------------------- /Full Working Version/Spotmicro_Animate_lib_009.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | @author: Arnaud Villeneuve 5 | """ 6 | 7 | from time import sleep, time 8 | from math import pi, sin, cos, atan, atan2, sqrt 9 | import numpy as np 10 | import pygame 11 | import Spotmicro_lib_020 12 | Spot = Spotmicro_lib_020.Spot() 13 | import Spotmicro_Gravity_Center_lib_007 14 | SpotCG = Spotmicro_Gravity_Center_lib_007.SpotCG() 15 | pygame.init() 16 | screen = pygame.display.set_mode((600, 600)) #seems necessary to have access to keyboard 17 | 18 | class SpotAnim: 19 | 20 | """Display colors """ 21 | BLACK = (0, 0, 0) 22 | WHITE = (255, 255, 255) 23 | BLUE = (24, 84, 231) 24 | GREEN = (51, 204, 51) 25 | RED = (255, 0, 0) 26 | GREY = (225, 225, 225) 27 | DARK_GREY=(100,100,100) 28 | DARK_RED =(176,0,0) 29 | VIOLET =(207,175,255) 30 | DARKVIOLET =(103,87,127) 31 | CYAN = (0,255,255) 32 | DARK_CYAN =(0,127,127) 33 | MED_GREY = (170,170,170) 34 | 35 | """Axis coordinates for animation""" 36 | xX=[0,100] 37 | yX=[0,0] 38 | zX=[0,0] 39 | 40 | xY=[0,0] 41 | yY=[0,100] 42 | zY=[0,0] 43 | 44 | xZ=[0,0] 45 | yZ=[0,0] 46 | zZ=[0,100] 47 | 48 | """Floor coordinates for animation""" 49 | xFloor = [500,500,-500,-500] 50 | yFloor = [500,-500,-500,500] 51 | zFloor = [0,0,0,0] 52 | 53 | 54 | conv = 0 55 | 56 | def display_rotate(self,x_spot,y_spot,z_spot,theta_spot,thetax,thetaz,xl,yl,zl): 57 | """ rotation related to movement""" 58 | line = [] 59 | Ma = np.zeros(9) 60 | Mb = np.zeros(9) 61 | M1 = np.zeros(9) 62 | Ma = Spot.xyz_rotation_matrix(theta_spot[3],theta_spot[4],theta_spot[2]+theta_spot[5],False) 63 | Mb = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],0,False) 64 | M1 = Spot.xyz_rotation_matrix(thetax,0,thetaz,True) 65 | 66 | for i in range (0,len(xl)): 67 | #absolute coordinates of Spot lines in x,y,z frame 68 | out0 = Spot.new_coordinates(Ma,xl[i],yl[i],zl[i],x_spot,y_spot,z_spot) 69 | out = Spot.new_coordinates(Mb,out0[0],out0[1],out0[2],0,0,0) 70 | 71 | #Coordinaites for dispaly on screen 72 | disp = Spot.new_coordinates(M1,out[0],out[1],out[2],0,0,0) 73 | yd= disp[1] 74 | xd= disp[0]/2**(yd*SpotAnim.conv/2000) 75 | zd= disp[2]/2**(yd*SpotAnim.conv/2000) 76 | 77 | line.append([int(300+xd),int(300-zd)]) 78 | return line 79 | 80 | 81 | def animate(self,pos,t,thetax,thetaz,angle,center_x,center_y,thetalf,thetarf,thetarr,thetalr,walking_speed,walking_direction,steering,stance): 82 | 83 | theta_spot = pos[12] 84 | x_spot = pos[13] 85 | y_spot = pos[14] 86 | z_spot = pos[15] 87 | CGabs = [x_spot[7],y_spot[7],z_spot[7]] 88 | dCG = [x_spot[8],y_spot[8],z_spot[8]] 89 | ZMP = [x_spot[9],y_spot[9],z_spot[9]] 90 | M = Spot.xyz_rotation_matrix(theta_spot[0],theta_spot[1],theta_spot[2],False) 91 | ZMPabs = Spot.new_coordinates(M,ZMP[0],ZMP[1],0,x_spot[1],y_spot[1],z_spot[1]) 92 | 93 | screen.fill(SpotAnim.WHITE) 94 | #pos = Spot.walking (track,x_offset,steering_radius,steering_angle,cw,h_amp,v_amp,b_height,ra_longi,ra_lat,0.2,t) 95 | 96 | "Floor Display""" 97 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[theta_spot[0],theta_spot[1],0,0,0,0],thetax,thetaz,SpotAnim.xFloor,SpotAnim.yFloor,SpotAnim.zFloor) 98 | pygame.draw.polygon(screen,SpotAnim.GREY,line,0) 99 | 100 | """Floor Grid Display """ 101 | for i in range (0,11): 102 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[theta_spot[0],theta_spot[1],0,0,0,0],thetax,thetaz,[-500+i*100,-500+i*100],[-500,500],[0,0]) 103 | pygame.draw.lines(screen,SpotAnim.DARK_GREY,False,line,1) 104 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[theta_spot[0],theta_spot[1],0,0,0,0],thetax,thetaz,[-500,500],[-500+i*100,-500+i*100],[0,0]) 105 | pygame.draw.lines(screen,SpotAnim.DARK_GREY,False,line,1) 106 | 107 | """ X,Y,Z frame display""" 108 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,SpotAnim.xX,SpotAnim.yX,SpotAnim.zX) 109 | pygame.draw.lines(screen,SpotAnim.RED,False,line,2) 110 | 111 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,SpotAnim.xY,SpotAnim.yY,SpotAnim.zY) 112 | pygame.draw.lines(screen,SpotAnim.GREEN,False,line,2) 113 | 114 | line = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,SpotAnim.xZ,SpotAnim.yZ,SpotAnim.zZ) 115 | pygame.draw.lines(screen,SpotAnim.BLUE,False,line,2) 116 | 117 | """ Radius display """ 118 | center_display = True 119 | if (steering<2000): 120 | lineR = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[center_x,x_spot[0]],[center_y,y_spot[0]],[0,0]) 121 | else: 122 | center_x1 = x_spot[0] + (center_x-x_spot[0])/steering*2000 123 | center_y1 = y_spot[0] + (center_y-y_spot[0])/steering*2000 124 | lineR = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[center_x1,x_spot[0]],[center_y1,y_spot[0]],[0,0]) 125 | center_display = False 126 | 127 | """ Direction display """ 128 | xd = x_spot[0] + walking_speed*cos(theta_spot[2]+ walking_direction-pi/2) 129 | yd = y_spot[0] + walking_speed*sin(theta_spot[2]+ walking_direction-pi/2) 130 | lineD = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[xd,x_spot[0]],[yd,y_spot[0]],[0,0]) 131 | 132 | """Legs lines""" 133 | 134 | leglf = Spot.FK(thetalf,1) 135 | legrf = Spot.FK(thetarf,-1) 136 | legrr = Spot.FK(thetarr,-1) 137 | leglr = Spot.FK(thetalr,1) 138 | 139 | """ Center of Gravity """ 140 | #Calculation of CG absolute position 141 | lineCG = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[CGabs[0],CGabs[0]],[CGabs[1],CGabs[1]],[0,CGabs[2]]) 142 | 143 | xleglf =[Spot.xlf,Spot.xlf+leglf[0],Spot.xlf+leglf[1],Spot.xlf+leglf[2],Spot.xlf+pos[0]] 144 | yleglf =[Spot.ylf,Spot.ylf+leglf[3],Spot.ylf+leglf[4],Spot.ylf+leglf[5],Spot.ylf+pos[1]] 145 | #zleglf =[b_height,b_height+leglf[6],b_height+leglf[7],b_height+leglf[8],b_height+pos[2]] 146 | zleglf =[0,leglf[6],leglf[7],leglf[8],pos[2]] 147 | linelf = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xleglf,yleglf,zleglf) 148 | 149 | xlegrf =[Spot.xrf,Spot.xrf+legrf[0],Spot.xrf+legrf[1],Spot.xrf+legrf[2],Spot.xrf+pos[3]] 150 | ylegrf =[Spot.yrf,Spot.yrf+legrf[3],Spot.yrf+legrf[4],Spot.yrf+legrf[5],Spot.yrf+pos[4]] 151 | #zlegrf =[b_height,b_height+legrf[6],b_height+legrf[7],b_height+legrf[8],b_height+pos[5]] 152 | zlegrf =[0,legrf[6],legrf[7],legrf[8],pos[5]] 153 | linerf = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xlegrf,ylegrf,zlegrf) 154 | 155 | xlegrr =[Spot.xrr,Spot.xrr+legrr[0],Spot.xrr+legrr[1],Spot.xrr+legrr[2],Spot.xrr+pos[6]] 156 | ylegrr =[Spot.yrr,Spot.yrr+legrr[3],Spot.yrr+legrr[4],Spot.yrr+legrr[5],Spot.yrr+pos[7]] 157 | #zlegrr =[b_height,b_height+legrr[6],b_height+legrr[7],b_height+legrr[8],b_height+pos[8]] 158 | zlegrr = [0,legrr[6],legrr[7],legrr[8],pos[8]] 159 | linerr = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xlegrr,ylegrr,zlegrr) 160 | 161 | xleglr =[Spot.xlr,Spot.xlr+leglr[0],Spot.xlr+leglr[1],Spot.xlr+leglr[2],Spot.xlr+pos[9]] 162 | yleglr =[Spot.ylr,Spot.ylr+leglr[3],Spot.ylr+leglr[4],Spot.ylr+leglr[5],Spot.ylr+pos[10]] 163 | #zleglr =[b_height,b_height+leglr[6],b_height+leglr[7],b_height+leglr[8],b_height+pos[11]] 164 | zleglr = [0,leglr[6],leglr[7],leglr[8],pos[11]] 165 | linelr = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,xleglr,yleglr,zleglr) 166 | """ Body frame lines """ 167 | lineb = SpotAnim.display_rotate (self,x_spot[1]-x_spot[0],y_spot[1]-y_spot[0],z_spot[1]-z_spot[0],theta_spot,thetax,thetaz,[Spot.xlf,Spot.xrf,Spot.xrr,Spot.xlr,Spot.xlf],[Spot.ylf,Spot.yrf,Spot.yrr,Spot.ylr,Spot.ylf],[Spot.zlf,Spot.zrf,Spot.zrr,Spot.zlr,Spot.zlf]) 168 | 169 | 170 | """Sustentation area lines""" 171 | #stance = False when leg is lifted from the floor 172 | 173 | linesus =[] 174 | if (stance[0]==True): 175 | linesus.append(linelf[4]) 176 | if (stance[1]==True): 177 | linesus.append(linerf[4]) 178 | if (stance[2]==True): 179 | linesus.append(linerr[4]) 180 | if (stance[3]==True): 181 | linesus.append(linelr[4]) 182 | 183 | """ Center of gravity into sustentation area """ 184 | linedCG = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[CGabs[0],dCG[0]],[CGabs[1],dCG[1]],[0,0]) 185 | lineZMP = SpotAnim.display_rotate (self,-x_spot[0],-y_spot[0],-z_spot[0],[0,0,0,0,0,0],thetax,thetaz,[ZMPabs[0],CGabs[0]],[ZMPabs[1],CGabs[1]],[0,0]) 186 | if sum(stance)>2: 187 | pygame.draw.polygon(screen,SpotAnim.VIOLET,linesus,0) 188 | pygame.draw.lines(screen,SpotAnim.BLACK,True,linesus,1) 189 | 190 | pygame.draw.lines(screen,SpotAnim.DARKVIOLET,False,linesus,4) 191 | 192 | pygame.draw.lines(screen,SpotAnim.CYAN,False,lineR,2) 193 | pygame.draw.lines(screen,SpotAnim.GREEN,False,lineD,2) 194 | 195 | if (center_display == True): 196 | pygame.draw.circle(screen,SpotAnim.BLACK,lineR[0],5) 197 | pygame.draw.lines(screen,SpotAnim.BLACK,False, linedCG,1) 198 | pygame.draw.circle(screen,SpotAnim.DARK_CYAN,lineR[1],5) 199 | 200 | pygame.draw.lines(screen,SpotAnim.BLACK,False, lineCG,1) 201 | if (dCG[2] == True): 202 | pygame.draw.circle(screen,SpotAnim.GREEN,lineCG[0],3) 203 | else: 204 | pygame.draw.circle(screen,SpotAnim.RED,lineCG[0],3) 205 | 206 | pygame.draw.circle(screen,SpotAnim.DARK_GREY,lineCG[1],10) 207 | pygame.draw.circle(screen,SpotAnim.BLACK,lineZMP[0],4) 208 | pygame.draw.lines(screen,SpotAnim.RED,False, linelf,4) 209 | pygame.draw.lines(screen,SpotAnim.RED,False, linerf,4) 210 | pygame.draw.lines(screen,SpotAnim.RED,False, linerr,4) 211 | pygame.draw.lines(screen,SpotAnim.RED,False, linelr,4) 212 | pygame.draw.lines(screen,SpotAnim.BLUE,False,lineb,10) 213 | pygame.draw.lines(screen,SpotAnim.BLACK, False,[[angle[0]/pi*180/45*300+300,0],[angle[0]/pi*180/45*300+300,50]],5) 214 | pygame.draw.lines(screen,SpotAnim.BLACK, False,[[0,angle[1]/pi*180/45*300+300],[50,angle[1]/pi*180/45*300+300]],5) 215 | 216 | return 217 | -------------------------------------------------------------------------------- /Full Working Version/Spotmicro_Gravity_Center_lib_007.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | @author: Arnaud Villeneuve 5 | """ 6 | 7 | from math import sqrt 8 | import Spotmicro_lib_020 9 | Spot = Spotmicro_lib_020.Spot() 10 | 11 | 12 | class SpotCG: 13 | def CG_calculation (self,thetalf,thetarf,thetarr,thetalr): 14 | cgposlf=(Spot.FK_Weight(thetalf,1)) 15 | cgposrf=(Spot.FK_Weight(thetarf,-1)) 16 | cgposrr=(Spot.FK_Weight(thetarr,-1)) 17 | cgposlr=(Spot.FK_Weight(thetalr,1)) 18 | 19 | Weightsum = Spot.Weight_Body+4*(Spot.Weight_Shoulder+Spot.Weight_Leg+Spot.Weight_Foreleg) 20 | 21 | xcglf=(cgposlf[0]+Spot.xlf)*Spot.Weight_Shoulder+(cgposlf[1]+Spot.xlf)*Spot.Weight_Leg+(cgposlf[2]+Spot.xlf)*Spot.Weight_Foreleg 22 | xcgrf=(cgposrf[0]+Spot.xrf)*Spot.Weight_Shoulder+(cgposrf[1]+Spot.xrf)*Spot.Weight_Leg+(cgposrf[2]+Spot.xrf)*Spot.Weight_Foreleg 23 | xcgrr=(cgposrr[0]+Spot.xrr)*Spot.Weight_Shoulder+(cgposrr[1]+Spot.xrr)*Spot.Weight_Leg+(cgposrr[2]+Spot.xrr)*Spot.Weight_Foreleg 24 | xcglr=(cgposlr[0]+Spot.xlr)*Spot.Weight_Shoulder+(cgposlr[1]+Spot.xlr)*Spot.Weight_Leg+(cgposlr[2]+Spot.xlr)*Spot.Weight_Foreleg 25 | xcg= (xcglf+xcgrf+xcgrr+xcglr+Spot.xCG_Body*Spot.Weight_Body)/Weightsum 26 | 27 | ycglf=(cgposlf[3]+Spot.ylf)*Spot.Weight_Shoulder+(cgposlf[4]+Spot.ylf)*Spot.Weight_Leg+(cgposlf[5]+Spot.ylf)*Spot.Weight_Foreleg 28 | ycgrf=(cgposrf[3]+Spot.yrf)*Spot.Weight_Shoulder+(cgposrf[4]+Spot.yrf)*Spot.Weight_Leg+(cgposrf[5]+Spot.yrf)*Spot.Weight_Foreleg 29 | ycgrr=(cgposrr[3]+Spot.yrr)*Spot.Weight_Shoulder+(cgposrr[4]+Spot.yrr)*Spot.Weight_Leg+(cgposrr[5]+Spot.yrr)*Spot.Weight_Foreleg 30 | ycglr=(cgposlr[3]+Spot.ylr)*Spot.Weight_Shoulder+(cgposlr[4]+Spot.ylr)*Spot.Weight_Leg+(cgposlr[5]+Spot.ylr)*Spot.Weight_Foreleg 31 | ycg= (ycglf+ycgrf+ycgrr+ycglr+Spot.yCG_Body*Spot.Weight_Body)/Weightsum 32 | 33 | zcglf=(cgposlf[6]+Spot.zlf)*Spot.Weight_Shoulder+(cgposlf[7]+Spot.zlf)*Spot.Weight_Leg+(cgposlf[8]+Spot.zlf)*Spot.Weight_Foreleg 34 | zcgrf=(cgposrf[6]+Spot.zrf)*Spot.Weight_Shoulder+(cgposrf[7]+Spot.zrf)*Spot.Weight_Leg+(cgposrf[8]+Spot.zrf)*Spot.Weight_Foreleg 35 | zcgrr=(cgposrr[6]+Spot.zrr)*Spot.Weight_Shoulder+(cgposrr[7]+Spot.zrr)*Spot.Weight_Leg+(cgposrr[8]+Spot.zrr)*Spot.Weight_Foreleg 36 | zcglr=(cgposlr[6]+Spot.zlr)*Spot.Weight_Shoulder+(cgposlr[7]+Spot.zlr)*Spot.Weight_Leg+(cgposlr[8]+Spot.zlr)*Spot.Weight_Foreleg 37 | zcg= (zcglf+zcgrf+zcgrr+zcglr+Spot.zCG_Body*Spot.Weight_Body)/Weightsum 38 | 39 | return (xcg,ycg,zcg) 40 | 41 | 42 | def CG_distance (self,x_legs,y_legs,z_legs,xcg,ycg,stance): 43 | 44 | #line equation c * x + s * y - p = 0 45 | # with c = a/m et s = b/m 46 | 47 | a1 = (y_legs[0]-y_legs[2]) 48 | b1 = -(x_legs[0]-x_legs[2]) 49 | m1 =sqrt(a1**2 + b1**2) 50 | c1 = a1/m1 51 | s1 = b1/m1 52 | 53 | a2 = (y_legs[1]-y_legs[3]) 54 | b2 = -(x_legs[1]-x_legs[3]) 55 | m2 =sqrt(a2**2 + b2**2) 56 | c2 = a2/m2 57 | s2 = b2/m2 58 | 59 | p1 = c1*x_legs[0] + s1*y_legs[0] 60 | p2 = c2*x_legs[1] + s2*y_legs[1] 61 | 62 | """ Dstance calculation """ 63 | d1 = c1*xcg + s1*ycg - p1 64 | d2 = c2*xcg + s2*ycg - p2 65 | 66 | """ intersection calculation """ 67 | #perpendicalar line equation -s * x + c * y - q = 0 68 | 69 | q1 = -s1*xcg +c1*ycg 70 | q2 = -s2*xcg +c2*ycg 71 | 72 | xint1 = c1*p1 - s1*q1 73 | yint1 = c1*q1 + s1*p1 74 | 75 | xint2 = c2*p2 - s2*q2 76 | yint2 = c2*q2 + s2*p2 77 | 78 | """ Check if inside sustentation triangle """ 79 | d = 0 80 | xint = xcg 81 | yint = ycg 82 | if (stance[0]== False)|(stance[2]== False): 83 | d = d2 84 | xint = xint2 85 | yint = yint2 86 | 87 | 88 | if (stance[1]== False)|(stance[3]== False): 89 | d = d1 90 | xint = xint1 91 | yint = yint1 92 | 93 | balance = True 94 | 95 | if (stance[0] == False)&(d< 0): 96 | balance = False 97 | 98 | if (stance[1] == False)&(d> 0): 99 | balance = False 100 | 101 | if (stance[2] == False)&(d> 0): 102 | balance = False 103 | 104 | if (stance[3] == False)&(d< 0): 105 | balance = False 106 | 107 | if (balance == False): 108 | d=-abs(d) 109 | else: 110 | d=abs(d) 111 | 112 | return (d,xint,yint,balance) 113 | -------------------------------------------------------------------------------- /Full Working Version/Spotmicro_lib_020.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @author: Arnaud Villeneuve 4 | """ 5 | 6 | from math import pi, sin, cos, asin, acos, atan2, sqrt 7 | import numpy as np 8 | 9 | 10 | class Spot: 11 | """ Servos Settings """ 12 | """ rotation direction """ 13 | dir01 = -1 14 | dir02 = 1 15 | dir03 = 1 16 | dir04 = -1 17 | dir05 = -1 18 | dir06 = -1 19 | dir07 = 1 20 | dir08 = -1 21 | dir09 = -1 22 | dir10 = 1 23 | dir11 = 1 24 | dir12 = 1 25 | 26 | """ zero position in µs """ 27 | 28 | angle_scale_factor_lf1 = 1.12 29 | angle_scale_factor_lf2 = 1.12 30 | angle_scale_factor_lf3 = 1.12 #1.12 ( zero = 170) 31 | 32 | angle_scale_factor_rf1 = 1.12 33 | angle_scale_factor_rf2 = 1.12 34 | angle_scale_factor_rf3 = 1.12 #1.06 (zero = 0) 35 | 36 | angle_scale_factor_rr1 = 1.12 37 | angle_scale_factor_rr2 = 1.12 38 | angle_scale_factor_rr3 = 1.12 #1.12 (zero = 7) 39 | 40 | angle_scale_factor_lr1 = 1.12 41 | angle_scale_factor_lr2 = 1.12 42 | angle_scale_factor_lr3 = 1.12 #1.11 (zero = 167) 43 | 44 | zero01 = 95 45 | zero02 = 97 46 | zero03 = 75 + 90* angle_scale_factor_lf3 #was 69 47 | zero04 = 91 48 | zero05 = 90 49 | zero06 = 89 - 90* angle_scale_factor_rf3 #was 95 50 | zero07 = 92 51 | zero08 = 81 52 | zero09 = 102- 90* angle_scale_factor_rr3 # was 108 53 | zero10 = 96 54 | zero11 = 94 55 | zero12 = 73 + 90* angle_scale_factor_lr3 #was 67 56 | 57 | 58 | """" Spotmicro dimensions """ 59 | Wb = 78 #Shoulder/hip width 60 | Lb = 187.1 #Shoulder to hip length 61 | L0 = 58.09 #Shoulder articulation width 62 | d = 10.73 #Shoulder articulation height 63 | L1 = 108.31 #Leg length 64 | L2 = 138 #Foreleg length 65 | 66 | """ Anchor points """ 67 | 68 | """ Front left shoulder""" 69 | xlf = 93.55 70 | ylf = 39 71 | zlf = 0 72 | 73 | """Front right shoulder""" 74 | xrf = 93.55 75 | yrf = -39 76 | zrf = 0 77 | 78 | """Rear left hip """ 79 | xlr = -93.55 80 | ylr = 39 81 | zlr = 0 82 | 83 | """Rear right hip """ 84 | xrr = -93.55 85 | yrr = -39 86 | zrr = 0 87 | 88 | 89 | """Inertia Centers""" 90 | 91 | """ Body""" 92 | xCG_Body = 11.32 # taking into account 82g at + 140 mm + 65 g (at 0 mm) 93 | yCG_Body = 0 94 | zCG_Body = 0 95 | Weight_Body = 1044 #including 65g back + 82g front (at + 140 mm) 96 | 97 | """ Left Shoulder """ 98 | xCG_Shoulder = 0 99 | yCG_Shoulder = 5 #sign to be changed for right leg 100 | zCG_Shoulder = -9 101 | Weight_Shoulder = 99.3 102 | 103 | """ Left Leg """ 104 | xCG_Leg = 0 105 | yCG_Leg = 0 #sign to be changed for right leg 106 | zCG_Leg = -31 107 | Weight_Leg = 133.3 108 | 109 | """ Left Leg """ 110 | xCG_Foreleg = 0 111 | yCG_Foreleg = 0 #sign to be changed for right leg 112 | zCG_Foreleg = -28 113 | Weight_Foreleg = 107 114 | 115 | 116 | phase = pi/8 # optimum position when leg is fully lifted 117 | 118 | servo_table = [0,1,2,3,4,5,7,8,9,10,11,12] 119 | 120 | def interp(x1,x2,steps): 121 | out = np.zeros(steps) 122 | for i in range (steps): 123 | out[i] = x1 +(x2-x1)/(steps-1)*i 124 | return out 125 | 126 | def interp1(x1,x2,i,steps): 127 | out = x1 +(x2-x1)/(steps-1)*i 128 | return out 129 | 130 | 131 | 132 | def xyz_rotation_matrix (self,thetax,thetay,thetaz,inverse): 133 | if (inverse == True): 134 | #Rx*Ry*Rz 135 | t2 = cos(thetay) 136 | t3 = sin(thetaz) 137 | t4 = cos(thetaz) 138 | t5 = sin(thetay) 139 | t6 = cos(thetax) 140 | t7 = sin(thetax) 141 | M = [t2*t4,t3*t6+t4*t5*t7,t3*t7-t4*t5*t6,-t2*t3,t4*t6-t3*t5*t7,t4*t7+t3*t5*t6,t5,-t2*t7,t2*t6] 142 | else: 143 | #Rz*Ry*Rx 144 | t2 = cos(thetaz); 145 | t3 = sin(thetax); 146 | t4 = sin(thetaz); 147 | t5 = cos(thetax); 148 | t6 = sin(thetay); 149 | t7 = cos(thetay); 150 | M = [t2*t7,t4*t7,-t6,-t4*t5+t2*t3*t6,t2*t5+t3*t4*t6,t3*t7,t3*t4+t2*t5*t6,-t2*t3+t4*t5*t6,t5*t7] 151 | return M 152 | 153 | 154 | def new_coordinates (self,M,x,y,z,x0,y0,z0): 155 | xout= x0 + M[0]*x + M[3]*y + M[6]*z 156 | yout= y0 + M[1]*x + M[4]*y + M[7]*z 157 | zout= z0 + M[2]*x + M[5]*y + M[8]*z 158 | return [xout,yout,zout] 159 | 160 | 161 | 162 | def foot_coordinate (self,x,y,z,thetax,thetay): 163 | cx = cos(thetax) 164 | sx = sin(thetax) 165 | cy = cos(thetay) 166 | sy = sin(thetay) 167 | xf = x*cy+ z*sy 168 | yf = y*cx - z*cy*sx + x*sx*sy 169 | zf = y*sx + z*cx*cy - x*cx*sy 170 | return [xf,yf,zf] 171 | 172 | 173 | def IK (self,L0, L1, L2, d, x, y, z, side): #Leg inverse Kinematics 174 | """ 175 | s = 1 for left leg 176 | s = -1 for right leg 177 | """ 178 | t2 = y**2 179 | t3 = z**2 180 | t4 = t2+t3 181 | t5 = 1/sqrt(t4) 182 | t6 = L0**2 183 | t7 = t2+t3-t6 184 | t8 = sqrt(t7) 185 | t9 = d-t8 186 | t10 = x**2 187 | t11 = t9**2 188 | t15 = L1**2 189 | t16 = L2**2 190 | t12 = t10+t11-t15-t16 191 | t13 = t10+t11 192 | t14 = 1/sqrt(t13) 193 | error = False 194 | try: 195 | theta1 = side*(-pi/2+asin(t5*t8))+asin(t5*y) 196 | theta2= -asin(t14*x)+asin(L2*t14*sqrt(1/t15*1/t16*t12**2*(-1/4)+1)) 197 | theta3 =-pi + acos(-t12/2/(L1*L2)) 198 | 199 | except ValueError: 200 | print ('ValueError IK') 201 | error = True 202 | theta1=90 203 | theta2=90 204 | theta3=90 205 | 206 | theta = [theta1, theta2, theta3] 207 | return (theta,error) 208 | 209 | def FK (self, theta, side): 210 | """ Calculation of articulation points """ 211 | """ 212 | s = 1 for left leg 213 | s = -1 for right leg 214 | """ 215 | x_shoulder1 = 0 216 | y_shoulder1 = Spot.d*sin(theta[0]) 217 | z_shoulder1 = -Spot.d*cos(theta[0]) 218 | 219 | x_shoulder2 = 0 220 | y_shoulder2 =side*Spot.L0*cos(theta[0])+Spot.d*sin(theta[0]) 221 | z_shoulder2 =side*Spot.L0*sin(theta[0])-Spot.d*cos(theta[0]) 222 | 223 | x_elbow = -Spot.L1*sin(theta[1]) 224 | y_elbow = side*Spot.L0*cos(theta[0])-(-Spot.d-Spot.L1*cos(theta[1]))*sin(theta[0]) 225 | z_elbow = side*Spot.L0*sin(theta[0]) +(-Spot.d-Spot.L1*cos(theta[1]))*cos(theta[0]) 226 | 227 | return [x_shoulder1,x_shoulder2,x_elbow,y_shoulder1,y_shoulder2,y_elbow,z_shoulder1,z_shoulder2,z_elbow] 228 | 229 | 230 | def FK_Weight (self, theta, side): 231 | """ Calculation of articulation points """ 232 | """ 233 | side = 1 for left leg 234 | side = -1 for right leg 235 | """ 236 | 237 | xCG_Shoulder1 = Spot.xCG_Shoulder 238 | yCG_Shoulder1 =side*Spot.yCG_Shoulder*cos(theta[0])-Spot.zCG_Shoulder*sin(theta[0]) 239 | zCG_Shoulder1 =side*Spot.yCG_Shoulder*sin(theta[0])+Spot.zCG_Shoulder*cos(theta[0]) 240 | 241 | xCG_Leg1 = Spot.xCG_Leg*cos(theta[1]) + Spot.zCG_Leg*sin(theta[1]) 242 | yCG_Leg1 = cos(theta[0])*(Spot.L0*side + side*Spot.yCG_Leg) + sin(theta[0])*(Spot.d - Spot.zCG_Leg*cos(theta[1]) + Spot.xCG_Leg*sin(theta[1])) 243 | zCG_Leg1 = sin(theta[0])*(Spot.L0*side + side*Spot.yCG_Leg) - cos(theta[0])*(Spot.d - Spot.zCG_Leg*cos(theta[1]) + Spot.xCG_Leg*sin(theta[1])) 244 | 245 | xCG_Foreleg1 = cos(theta[1])*(Spot.xCG_Foreleg*cos(theta[2]) + Spot.zCG_Foreleg*sin(theta[2])) - sin(theta[1])*(Spot.L1 - Spot.zCG_Foreleg*cos(theta[2]) + Spot.xCG_Foreleg*sin(theta[2])) 246 | yCG_Foreleg1 = cos(theta[0])*(Spot.L0*side + side*Spot.yCG_Foreleg) + sin(theta[0])*(Spot.d + sin(theta[1])*(Spot.xCG_Foreleg*cos(theta[2]) + Spot.zCG_Foreleg*sin(theta[2])) + cos(theta[1])*(Spot.L1 - Spot.zCG_Foreleg*cos(theta[2]) + Spot.xCG_Foreleg*sin(theta[2]))) 247 | zCG_Foreleg1 = sin(theta[0])*(Spot.L0*side + side*Spot.yCG_Foreleg) - cos(theta[0])*(Spot.d + sin(theta[1])*(Spot.xCG_Foreleg*cos(theta[2]) + Spot.zCG_Foreleg*sin(theta[2])) + cos(theta[1])*(Spot.L1 - Spot.zCG_Foreleg*cos(theta[2]) + Spot.xCG_Foreleg*sin(theta[2]))) 248 | 249 | return [xCG_Shoulder1,xCG_Leg1,xCG_Foreleg1,yCG_Shoulder1,yCG_Leg1,yCG_Foreleg1,zCG_Shoulder1,zCG_Leg1,zCG_Foreleg1] 250 | 251 | 252 | def start_walk_stop (self,track,x_offset,steering_radius, steering_angle,cw,h_amp,v_amp,height,stepl,t,tstep,theta_spot,x_spot,y_spot,z_spot,phase): 253 | 254 | 255 | trans = (phase-1)%2 256 | if (trans==1): #2steps walk 257 | tr =0.5 #2 steps walk 258 | seq = [0,0.5,0,0.5]; 259 | else: 260 | tr =0.25 #4 steps walk 261 | seq = [0,0.5,0.25,0.75] 262 | 263 | alpha = np.zeros(4) 264 | alphav =np.zeros(4) 265 | 266 | theta_spot_updated = theta_spot 267 | 268 | CG = [x_spot[6],y_spot[6],z_spot[6]] 269 | 270 | """ Steering center coordinates in spot frame """ 271 | xc = steering_radius* cos(steering_angle) 272 | yc = steering_radius* sin(steering_angle) 273 | 274 | #rotation matrix for frame position 275 | #Mf = Spot.xyz_rotation_matrix (self,frame_pos[0],frame_pos[1],frame_pos[2],False) 276 | 277 | Ms = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2],False) 278 | 279 | s = Spot.new_coordinates(self,Ms,xc,yc,0,x_spot[0],y_spot[0],z_spot[0]) 280 | xs = s[0] 281 | ys = s[1] 282 | 283 | """ Nominal Foot Position """ 284 | xn = [Spot.xlf, Spot.xrf,Spot.xrr, Spot.xlr] 285 | yn = [Spot.ylf+track,Spot.yrf-track,Spot.yrr-track,Spot.ylr+track] 286 | 287 | radii = np.zeros(4) 288 | an = np.zeros(4) 289 | for i in range (0,4): 290 | """ Steering radius """ 291 | radii[i] = sqrt((xc-xn[i])**2+(yc-yn[i])**2) 292 | """ Foot nominal angle""" 293 | an[i] = atan2(yn[i]-yc,xn[i]-xc) 294 | 295 | """ Motion angle """ 296 | maxr = max(radii) 297 | mangle = h_amp/maxr 298 | 299 | """ Rotation angle and translation calculation""" 300 | if (phase <=2)|(phase >= 5): #stop or start 301 | dtheta = mangle/(1-stepl)*tstep/2*cw 302 | else: 303 | dtheta = mangle/(1-stepl)*tstep*cw 304 | theta_spot_updated[2] = dtheta + theta_spot[2] 305 | 306 | #Matrix from local body frame to absolute space rrame 307 | Ms_updated = Spot.xyz_rotation_matrix (self,theta_spot_updated[3],theta_spot_updated[4],theta_spot_updated[2]+theta_spot_updated[5],False) 308 | #Matrix from absolute space rrame to loacal body frame 309 | Msi_updated = Spot.xyz_rotation_matrix (self,-theta_spot_updated[3],-theta_spot_updated[4],-(theta_spot_updated[2]+theta_spot_updated[5]),True) 310 | #Delta rotation matric from from body center to absolute space frame 311 | dMs = Spot.xyz_rotation_matrix (self,0,0,dtheta,False) 312 | 313 | """ Foot nominal center absolute position""" 314 | foot_center = Spot.new_coordinates(self,dMs,x_spot[0]-xs, y_spot[0]-ys,0,xs,ys,0) 315 | 316 | t1 = t%1 317 | kcomp = 1 318 | anb = pi- asin(10/v_amp) 319 | 320 | stance = [True,True,True,True] 321 | 322 | for i in range (0,4): 323 | 324 | if (t1<(seq[i]+tr))&(t1>(seq[i]+stepl)): 325 | alphav[i] =anb + (pi-anb)/(tr-stepl)*(t1-seq[i]-stepl) 326 | #print (t1,alphav[i]) 327 | if (t1<=seq[i]): 328 | stance[i] = True #Leg is on the ground (absolute position value unchanged) 329 | else: 330 | if (t1<(seq[i]+tr)): 331 | 332 | 333 | if (t1<(seq[i]+stepl)): 334 | stance[i] = False #leg is lifted (absolute position value changes) 335 | alphav[i] = anb*(t1-seq[i])/stepl 336 | #print (t1,alphav[i]) 337 | t2 = seq[i]+stepl 338 | if (phase <= 2): # start 339 | #End position alpha 340 | alpha[i] = -seq[i]/(1-stepl)/2 + (t2-seq[i])/stepl/(1-stepl)*seq[i] 341 | if (phase >= 5): #stop 342 | alpha[i] = -1/2 + seq[i]/(1-stepl)/2 + (t2-seq[i])/stepl*(1-seq[i]/(1-stepl)) 343 | if (phase <=4)&(phase>=3): #walk 344 | alpha[i] = -1/2 + ((t2-seq[i])/stepl) 345 | else: 346 | stance[i] = True #Leg is on the ground (absolute position value unchanged) 347 | 348 | """ Compensation Calculation """ 349 | stance_test = np.sum(stance) #if sum = 4 all feet are on the floor --> body balance 350 | 351 | #absolute stance area target point 352 | #Barycenter of sustentation area with higher weight of diagonal points 353 | weight = 1.4 354 | x_abs_area = np.zeros(4) 355 | y_abs_area = np.zeros(4) 356 | 357 | x_abs_area[0] = ((x_spot[3]+x_spot[5])*weight+x_spot[4])/(2*weight+1)*(1-trans) + (x_spot[3]+x_spot[5])/2 *trans 358 | y_abs_area[0] = ((y_spot[3]+y_spot[5])*weight+y_spot[4])/(2*weight+1)*(1-trans) + (y_spot[3]+y_spot[5])/2 *trans 359 | x_abs_area[1] = ((x_spot[2]+x_spot[4])*weight+x_spot[5])/(2*weight+1)*(1-trans) + (x_spot[2]+x_spot[4])/2 *trans 360 | y_abs_area[1] = ((y_spot[2]+y_spot[4])*weight+y_spot[5])/(2*weight+1)*(1-trans) + (y_spot[2]+y_spot[4])/2 *trans 361 | x_abs_area[2] = ((x_spot[3]+x_spot[5])*weight+x_spot[2])/(2*weight+1)*(1-trans) + (x_spot[3]+x_spot[5])/2 *trans 362 | y_abs_area[2] = ((y_spot[3]+y_spot[5])*weight+y_spot[2])/(2*weight+1)*(1-trans) + (y_spot[3]+y_spot[5])/2 *trans 363 | x_abs_area[3] = ((x_spot[2]+x_spot[4])*weight+x_spot[3])/(2*weight+1)*(1-trans) + (x_spot[2]+x_spot[4])/2 *trans 364 | y_abs_area[3] = ((y_spot[2]+y_spot[4])*weight+y_spot[3])/(2*weight+1)*(1-trans) + (y_spot[2]+y_spot[4])/2 *trans 365 | 366 | if (stance_test == 4): 367 | istart = 0 368 | iend = 0 369 | #identify transition start and target 370 | tstart = (int(t1/tr)*tr) 371 | tend = tstart+tr 372 | if (tend==1): 373 | tend = 0 374 | 375 | for i in range (0,4): 376 | if (tstart == seq[i]): 377 | istart = i 378 | if (tend == seq[i]): 379 | iend = i 380 | 381 | if (t1>(seq[istart]+stepl)): 382 | x_abs_comp= x_abs_area[istart]+(x_abs_area[iend]-x_abs_area[istart])*(t1-tstart-stepl)/(tr-stepl) 383 | y_abs_comp= y_abs_area[istart]+(y_abs_area[iend]-y_abs_area[istart])*(t1-tstart-stepl)/(tr-stepl) 384 | else: 385 | x_abs_comp = x_abs_area[istart] 386 | y_abs_comp = y_abs_area[istart] 387 | #print (t1,x_abs_comp,istart,iend,tstart,tend) 388 | else: 389 | for i in range (0,4): 390 | if (stance[i]==0): 391 | x_abs_comp = x_abs_area[i] 392 | y_abs_comp = y_abs_area[i] 393 | 394 | Msi_comp = Spot.xyz_rotation_matrix (self,0,0,-theta_spot_updated[2],True) 395 | #compensation calculation in body center frame 396 | comp= Spot.new_coordinates(self,Msi_comp,x_abs_comp-x_spot[0],y_abs_comp-y_spot[0],0,0,0,0) 397 | """ Compensation calculation with theta """ 398 | v_amp_t = v_amp 399 | 400 | ts = 0.25 401 | if (phase <= 2): #start 402 | if (t1< ts): 403 | kcomp = t1/ts 404 | v_amp_t = 0 405 | elif (phase >= 5): #stop 406 | if (t1 > (1-ts)): 407 | kcomp = (1-t1)/ts 408 | v_amp_t = 0 409 | 410 | Ms_comp = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2],False) 411 | #Compensation calculation absoltute space frame 412 | compt = Spot.new_coordinates(self,Ms_comp,(comp[0]-CG[0])*kcomp+x_offset,(comp[1]-CG[1])*kcomp,0,0,0,0) 413 | """ Frame center new position with gravity center compensation, offset and height """ 414 | x_framecenter_comp = foot_center[0] + compt[0] 415 | y_framecenter_comp = foot_center[1] + compt[1] 416 | z_framecenter_comp = height 417 | 418 | """ New Frame corners position absolute including compensation """ 419 | x_frame = [Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr] 420 | y_frame = [Spot.ylf, Spot.yrf, Spot.yrr, Spot.ylr] 421 | z_frame = [0,0,0,0] 422 | 423 | x_framecorner = np.zeros(4) 424 | y_framecorner = np.zeros(4) 425 | z_framecorner = np.zeros(4) 426 | 427 | for i in range (0,4): 428 | #Body corners calculation in absolute space frame 429 | frame_corner = Spot.new_coordinates(self,Ms_updated,x_frame[i],y_frame[i],z_frame[i],x_framecenter_comp,y_framecenter_comp,z_framecenter_comp) 430 | x_framecorner[i] = frame_corner[0] 431 | y_framecorner[i] = frame_corner[1] 432 | z_framecorner[i] = frame_corner[2] 433 | 434 | 435 | xleg = np.zeros(4) 436 | yleg = np.zeros(4) 437 | zleg = np.zeros(4) 438 | xabs = np.zeros(4) 439 | yabs = np.zeros(4) 440 | zabs = np.zeros(4) 441 | xint = np.zeros(4) 442 | yint = np.zeros(4) 443 | zint = np.zeros(4) 444 | 445 | for i in range (0,4): 446 | if (t1>seq[i])&(t1<(seq[i]+tr)): 447 | #relative position calculation (used for inverse kinematics) 448 | alphah = an[i]+mangle*alpha[i]*cw 449 | xleg_target = xc + radii[i]*cos(alphah) -(comp[0]-CG[0])*kcomp -x_offset -x_frame[i] 450 | yleg_target = yc + radii[i]*sin(alphah) -(comp[1]-CG[1])*kcomp -y_frame[i] 451 | 452 | leg_current = Spot.new_coordinates(self,Msi_comp,x_spot[i+2]-x_framecorner[i],y_spot[i+2]-y_framecorner[i],-z_framecorner[i],0,0,0) 453 | #interpolate between current position and targe 454 | if ((seq[i]+stepl-t1)>tstep): 455 | xint[i] = leg_current[0]+(xleg_target - leg_current[0])*(tstep)/(seq[i]+stepl-t1) 456 | yint[i] = leg_current[1]+(yleg_target - leg_current[1])*(tstep)/(seq[i]+stepl-t1) 457 | else: 458 | xint[i] = xleg_target 459 | yint[i] = yleg_target 460 | zint[i] = leg_current[2] + v_amp_t*sin(alphav[i]) 461 | #print (leg_current[2],zint[i],leg_current[2]-zint[i]) 462 | Msi_body = Spot.xyz_rotation_matrix (self,-theta_spot_updated[3],-theta_spot_updated[4],-theta_spot_updated[5],True) 463 | legs = Spot.new_coordinates(self,Msi_body,xint[i],yint[i],zint[i],0,0,0) 464 | xleg[i]= legs[0] 465 | yleg[i]= legs[1] 466 | zleg[i]= legs[2] 467 | 468 | #absolute foot position 469 | #Msb_updated = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2]+theta_spot_updated[5],False) 470 | foot_abs = Spot.new_coordinates(self,Ms_updated,xleg[i],yleg[i],zleg[i],x_framecorner[i],y_framecorner[i],z_framecorner[i]) 471 | 472 | 473 | xabs[i] = foot_abs[0] 474 | yabs[i] = foot_abs[1] 475 | zabs[i] = foot_abs[2] 476 | 477 | else: 478 | xabs[i] = x_spot[i+2] 479 | yabs[i] = y_spot[i+2] 480 | zabs[i] = 0 481 | 482 | #relative foot position of foot on the ground/floor for inverse kinematics 483 | leg = Spot.new_coordinates(self,Msi_updated,xabs[i]-x_framecorner[i],yabs[i]-y_framecorner[i],zabs[i]-z_framecorner[i],0,0,0) 484 | xleg[i] = leg[0] 485 | yleg[i] = leg[1] 486 | zleg[i] = leg[2] 487 | 488 | x_spot_updated = [foot_center[0],x_framecenter_comp, xabs[0], xabs[1], xabs[2], xabs[3],x_spot[6],x_spot[7],x_spot[8],x_spot[9]] 489 | y_spot_updated = [foot_center[1],y_framecenter_comp, yabs[0], yabs[1], yabs[2], yabs[3],y_spot[6],y_spot[7],y_spot[8],y_spot[9]] 490 | z_spot_updated = [foot_center[2],z_framecenter_comp, zabs[0], zabs[1], zabs[2], zabs[3],z_spot[6],z_spot[7],z_spot[8],z_spot[9]] 491 | 492 | 493 | pos = [xleg[0],yleg[0],zleg[0],xleg[1],yleg[1],zleg[1],xleg[2],yleg[2],zleg[2],xleg[3],yleg[3],zleg[3],theta_spot_updated,x_spot_updated,y_spot_updated,z_spot_updated] 494 | return pos 495 | 496 | def moving (self,t, start_frame_pos,end_frame_pos, pos): 497 | 498 | theta_spot_updated = pos[12] 499 | x_spot_updated = pos[13] 500 | y_spot_updated = pos[14] 501 | z_spot_updated = pos[15] 502 | 503 | 504 | #interpolate new frame position 505 | frame_pos = np.zeros(6) 506 | 507 | for i in range (0,6): 508 | frame_pos[i] = start_frame_pos[i] + (end_frame_pos[i]- start_frame_pos[i])*t 509 | 510 | theta_spot_updated [3] = frame_pos[0] 511 | theta_spot_updated [4] = frame_pos[1] 512 | theta_spot_updated [5] = frame_pos[2] 513 | #rotation matrix for frame position 514 | Mf = Spot.xyz_rotation_matrix (self,frame_pos[0],frame_pos[1],frame_pos[2],False) 515 | 516 | #rotation matrix for spot position (only around z axis) 517 | Ms = Spot.xyz_rotation_matrix (self,0,0,theta_spot_updated[2],False) 518 | 519 | # frame corners position coordinaterelative to frame center 520 | x_frame = [Spot.xlf, Spot.xrf, Spot.xrr, Spot.xlr] 521 | y_frame = [Spot.ylf, Spot.yrf, Spot.yrr, Spot.ylr] 522 | z_frame = [0,0,0,0] 523 | 524 | #New absolute frame center position 525 | frame_center_abs = Spot.new_coordinates(self,Ms,frame_pos[3],frame_pos[4],frame_pos[5],x_spot_updated[0],y_spot_updated[0],z_spot_updated[0]) 526 | 527 | #absolute frame corners position coordinates 528 | x_frame_corner_abs = np.zeros(4) 529 | y_frame_corner_abs = np.zeros(4) 530 | z_frame_corner_abs = np.zeros(4) 531 | 532 | for i in range (0,4): 533 | frame_corner = Spot.new_coordinates(self,Mf,x_frame[i],y_frame[i],z_frame[i],0,0,0) 534 | frame_corner_abs = Spot.new_coordinates(self,Ms,frame_corner[0],frame_corner[1],frame_corner[2],frame_center_abs[0],frame_center_abs[1],frame_center_abs[2]) 535 | x_frame_corner_abs[i] = frame_corner_abs[0] 536 | y_frame_corner_abs[i] = frame_corner_abs[1] 537 | z_frame_corner_abs[i] = frame_corner_abs[2] 538 | 539 | #calculate current relative position 540 | xleg = np.zeros(4) 541 | yleg = np.zeros(4) 542 | zleg = np.zeros(4) 543 | 544 | 545 | #Leg relative position to front corners 546 | Mi = Spot.xyz_rotation_matrix(self,-theta_spot_updated[3],-theta_spot_updated[4],-(theta_spot_updated[2]+theta_spot_updated[5]),True) 547 | 548 | for i in range (0,4): 549 | leg = Spot.new_coordinates(self,Mi,x_spot_updated[i+2]-x_frame_corner_abs[i],y_spot_updated[i+2]-y_frame_corner_abs[i],z_spot_updated[i+2]-z_frame_corner_abs[i],0,0,0) 550 | xleg[i] = leg[0] 551 | yleg[i] = leg[1] 552 | zleg[i] = leg[2] 553 | 554 | 555 | x_spot_updated[1] = frame_center_abs [0] 556 | y_spot_updated[1] = frame_center_abs [1] 557 | z_spot_updated[1] = frame_center_abs [2] 558 | 559 | 560 | pos = [xleg[0],yleg[0],zleg[0],xleg[1],yleg[1],zleg[1],xleg[2],yleg[2],zleg[2],xleg[3],yleg[3],zleg[3],theta_spot_updated,x_spot_updated,y_spot_updated,z_spot_updated] 561 | return pos 562 | 563 | 564 | 565 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Spot-Micro-Control-and-Animation 2 | Inverse Kinematics based control and animation of Spotmicro developped in Python for Raspeberry Pi 3 | 4 | This first version is a "simulator" of Spot Micro that runs under Python 3 and requires the Pygame Library in order to display animation and use the xbox one (or any similar) controller. I developped it with on a Raspberry Pi 4 but it runs successfully also on a Windows 10 PC. 5 | 6 | The SW does not include any function to control the servos PWM. I chose to do so as this part is very dependent on the HW set-up (type of µC board, servos type, ...) 7 | However, the SW includes all the functions that enable Spot Micro to walk, sit, give the paws, lie, twist and even lift the hind leg... 8 | All these features are based upon Inverse Kinematics. 9 | 10 | * In the main program Spot_Micro_Control_v01.py: 11 | 12 | -Line 57 to 72: set-up of the XBOX One (or similar) controller. You can use the attached utility Essai_Joystick_01.py to identify the parameters. 13 | 14 | -Line 565: This is the best place where you could place the call to your own servo control function. 15 | 16 | Based upon the paws to body position, the Inverse Kinematics functions calculates 4 sets of 3 angles (x shoulder, y shoulder, y elbow/knee): thetalf, thetarf, thetarr, thetalr: 17 | 18 | -lf: left front leg 19 | 20 | -rf : right front leg 21 | 22 | -rr : right rear leg 23 | 24 | -lr : left rear leg 25 | 26 | I expect that it is not necessary to understand exactly how the SW works providing that finally only these generated angles are necessary to control Spot Micro. 27 | When adapting to your own servos, please pay attention to zero setting of the servos and the tuning of the angles range so that actual angles correspond to the calculated ones. 28 | 29 | I attached a pdf file that gives some details about how inverse kinematics is built-up and correspondance between calculated angles and servos angles. 30 | 31 | Attached Libraries: 32 | 33 | * Spotmicro_Inverse_Kinematics_and_Position_Library_v01.py: This file contains a class of paramaters and functions to calculate Spot micro position. It includes: 34 | 35 | -The main dimensions of Spot micro 36 | 37 | -The center of gravity position and weight of each spotmicro limbs and body 38 | 39 | -The forward kinematics function 40 | 41 | -The forward kinematics functions 42 | 43 | -The function that generates walking positions 44 | 45 | -The function that generates positions from known start and end positionsThis library contains a class of functions to calculate the center of gravity position 46 | and distance to the support polygon edge 47 | 48 | 49 | * Spotmicro_Gravity_Center_Library_v01.py: This file contains a class of functions to calculate the center of gravity position and distance to the support polygon edge 50 | 51 | 52 | * Spotmicro_Animation_Library_v01.py: 53 | This file contains a class of functions to generate the animation frames 54 | 55 | Please make sure that all these files are located in the same folder. 56 | 57 | * Essai_Joystick_01.py: This utility was copied from https://www.pygame.org/docs/ref/joystick.html. It is helpfull to identify the controller / joystick parameters 58 | 59 | 60 | 61 | 62 | I will release soon a full version corresponding to my HW for information or for persons willing to apply the same of very close set-up. 63 | Actual S/W includes the control of IMU, voltage sensor, LCD display and ultrasonic range sensors 64 | It also includes features in order to toggle animation display and servos motion. This is very usefull to test movements prior to apply them in real life. 65 | 66 | I connect to the Raspberry Pi with VNC viewer through Wifi (and sometimes LAN Cable). 67 | 68 | My own Spot Micro version uses: 69 | 70 | -Raspberry Pi 4 71 | 72 | -PCA9685 shield (I2c) modified in order to separate servos power supply (6V) and 5V logic supply 73 | 74 | -12 x 30 kg.cm servos 75 | 76 | -MPU 6050 (I2c) IMU 77 | 78 | -ADS 1015 (I2c) for voltage measurement (with a simpe resistors voltage divider) 79 | 80 | -2x16 characters LCD display (I2c) 81 | 82 | -2 HC-SR04 untrasonic range sensors 83 | 84 | -20 A SBEC supply (servos) set at 6V 85 | 86 | -5 A SBEC supply for the RPi and shields logic circuits 87 | 88 | -2S 4000 mAh LiPo Battery 89 | 90 | I hope you will enjoy playing with this software and build-up your own walking Spot Micro ! 91 | 92 | 93 | 94 | Arnaud Villeneuve 95 | 96 | December 2020 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /SpotMicro_Power_Supply.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avbotics/Spot-Micro-Control-and-Animation/f0bb0e6d876608a1c6486ae0e4a2deda0d00bfd6/SpotMicro_Power_Supply.pdf -------------------------------------------------------------------------------- /Utility/.gitkeep: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Utility/Essai_Joystick_01.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | @author: https://www.pygame.org/docs/ref/joystick.htm 5 | """ 6 | 7 | import pygame 8 | 9 | 10 | # Define some colors. 11 | BLACK = pygame.Color('black') 12 | WHITE = pygame.Color('white') 13 | 14 | 15 | # This is a simple class that will help us print to the screen. 16 | # It has nothing to do with the joysticks, just outputting the 17 | # information. 18 | class TextPrint(object): 19 | def __init__(self): 20 | self.reset() 21 | self.font = pygame.font.Font(None, 20) 22 | 23 | def tprint(self, screen, textString): 24 | textBitmap = self.font.render(textString, True, BLACK) 25 | screen.blit(textBitmap, (self.x, self.y)) 26 | self.y += self.line_height 27 | 28 | def reset(self): 29 | self.x = 10 30 | self.y = 10 31 | self.line_height = 15 32 | 33 | def indent(self): 34 | self.x += 10 35 | 36 | def unindent(self): 37 | self.x -= 10 38 | 39 | 40 | pygame.init() 41 | 42 | # Set the width and height of the screen (width, height). 43 | screen = pygame.display.set_mode((500, 700)) 44 | 45 | pygame.display.set_caption("My Game") 46 | 47 | # Loop until the user clicks the close button. 48 | done = False 49 | 50 | # Used to manage how fast the screen updates. 51 | clock = pygame.time.Clock() 52 | 53 | # Initialize the joysticks. 54 | pygame.joystick.init() 55 | 56 | # Get ready to print. 57 | textPrint = TextPrint() 58 | 59 | # -------- Main Program Loop ----------- 60 | while not done: 61 | # 62 | # EVENT PROCESSING STEP 63 | # 64 | # Possible joystick actions: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN, 65 | # JOYBUTTONUP, JOYHATMOTION 66 | for event in pygame.event.get(): # User did something. 67 | if event.type == pygame.QUIT: # If user clicked close. 68 | done = True # Flag that we are done so we exit this loop. 69 | elif event.type == pygame.JOYBUTTONDOWN: 70 | print("Joystick button pressed.") 71 | elif event.type == pygame.JOYBUTTONUP: 72 | print("Joystick button released.") 73 | 74 | # 75 | # DRAWING STEP 76 | # 77 | # First, clear the screen to white. Don't put other drawing commands 78 | # above this, or they will be erased with this command. 79 | screen.fill(WHITE) 80 | textPrint.reset() 81 | 82 | # Get count of joysticks. 83 | joystick_count = pygame.joystick.get_count() 84 | 85 | textPrint.tprint(screen, "Number of joysticks: {}".format(joystick_count)) 86 | textPrint.indent() 87 | 88 | # For each joystick: 89 | for i in range(joystick_count): 90 | joystick = pygame.joystick.Joystick(i) 91 | joystick.init() 92 | 93 | textPrint.tprint(screen, "Joystick {}".format(i)) 94 | textPrint.indent() 95 | 96 | # Get the name from the OS for the controller/joystick. 97 | name = joystick.get_name() 98 | textPrint.tprint(screen, "Joystick name: {}".format(name)) 99 | 100 | # Usually axis run in pairs, up/down for one, and left/right for 101 | # the other. 102 | axes = joystick.get_numaxes() 103 | textPrint.tprint(screen, "Number of axes: {}".format(axes)) 104 | textPrint.indent() 105 | 106 | for i in range(axes): 107 | axis = joystick.get_axis(i) 108 | textPrint.tprint(screen, "Axis {} value: {:>6.3f}".format(i, axis)) 109 | textPrint.unindent() 110 | 111 | buttons = joystick.get_numbuttons() 112 | textPrint.tprint(screen, "Number of buttons: {}".format(buttons)) 113 | textPrint.indent() 114 | 115 | for i in range(buttons): 116 | button = joystick.get_button(i) 117 | textPrint.tprint(screen, 118 | "Button {:>2} value: {}".format(i, button)) 119 | print(button) 120 | textPrint.unindent() 121 | 122 | hats = joystick.get_numhats() 123 | textPrint.tprint(screen, "Number of hats: {}".format(hats)) 124 | textPrint.indent() 125 | 126 | # Hat position. All or nothing for direction, not a float like 127 | # get_axis(). Position is a tuple of int values (x, y). 128 | for i in range(hats): 129 | hat = joystick.get_hat(i) 130 | textPrint.tprint(screen, "Hat {} value: {}".format(i, str(hat))) 131 | textPrint.unindent() 132 | 133 | textPrint.unindent() 134 | 135 | # 136 | # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT 137 | # 138 | 139 | # Go ahead and update the screen with what we've drawn. 140 | pygame.display.flip() 141 | 142 | # Limit to 20 frames per second. 143 | clock.tick(20) 144 | 145 | # Close the window and quit. 146 | # If you forget this line, the program will 'hang' 147 | # on exit if running from IDLE. 148 | pygame.quit() 149 | --------------------------------------------------------------------------------