Upd2 Pentapod Pata2

=~=~=~=~=~=~=~=~=~=~=~= PuTTY log 2020.04.
12 01:15:22 =~=~=~=~=~=~=~=~=~=~=~=
root@hexapod_zynq:~/hexapod_project/Modeling/Python# py
test_locomotion_gait_translation.py vim
includes/hexapod_class.py
"includes/hexapod_class.py" 387L, 15697C326 self.axi_write(6+leg*3, off_q2)
327 self.axi_write(7+leg*3, off_q3)
328 return True
329
330 # Set read offsets
331 def set_default_offsets(self, print_out=False):
332 for i in range(6):
333 offset_q1 = self.dfloat2hfloat(self.sec2rad(self.j_offs[i][0])).lstrip('x')
336
337 if ( print_out ):
338 print('Setting leg '+str(i)+' offset')
339 print('\tQ1 offset = 0x'+offset_q1+' | '+str(self.sec2rad(self.j_offs[i][0])))
342
343 self.axi_set_offset(i, int(offset_q1,16), int(offset_q2,16), int(offset_q3,16))
344 return True
345
347 def set_init_position(self, print_out=False):
349 q1 = self.dfloat2hfloat(self.sec2rad(self.i_pos[i][0]+self.j_offs[i]
[0])).lstrip('x')
[1])).lstrip('x')
[2])).lstrip('x')
352
353 self.config_leg_ctr("one_leg", i)
354 self.axi_set_out_mux(i+1)
355 self.axi_write_params_in(q1, q2, q3)
356 self.axi_write_out_direct()
357
359 [pwm1, pwm2, pwm3] = self.axi_get_pwm(int(i))
360
361 print('Setting leg '+str(i)+' direct output')
362 print('\tQ1 = 0x'+q1+' | '+str(self.hfloat2dfloat(q1))+' |
'+str(self.rad2sec(self.hfloat2dfloat(q1)))+' | '+str(pwm1))
365
366 return True
367
368 # Read offsets
369 def axi_read_offset(self, leg):
370 off1 = self.axi_read(5+leg*3)
373 return [off1, off2, off3]
374
375 # Read offsets
376 def axi_hread_offset(self, leg):
377 off1 = self.axi_hread(5+leg*3)
381
382 # Get PWM values
383 def axi_get_pwm(self, leg):
384 pwm1 = (int(self.axi_hread(23+leg*3),16) & 0x3FC00) >> 10
387 return [pwm1, pwm2, pwm3]g gg 1 # -*- coding: utf-8 -*-
2 import numpy as np
3 import mmap
4 from time import sleep as sleep
5 from cordic_model import CORDIC as CORDIC
6 from numeric_conversions import numeric_conversions as numeric_conversions
7 from devmem_map import axi_ip_mmap as axi_ip_mmap
8 import math as mt
9 import os
10
11 ###############################################################################
12 #### Hexapod Class : Base AXI IP
13 ###############################################################################
14 class hexapod_kinematics(axi_ip_mmap, numeric_conversions):
15 ###########################################################################
16 #### Properties
17 ###########################################################################
18 ## Kinematics Parameters
19 i_pos = np.zeros(shape=(6,3))# Initial Joints Positions
20 j_offs = np.zeros(shape=(6,3))# Joints Offsets
21 i_inv_s = np.zeros(18).astype(int).astype(str)
22 joints = np.zeros(shape=(6,3)).astype(np.float32)
23 gait = np.array([]).astype(bytes)# Gaits
24 bgaits = np.zeros(shape=(30,3))
25 delay = 0.008
26
27 #### Properties
28 @property
29 def step_size(self):
30 return int(self.gait.size/6/3)
31
32 #### Parameters
33 init_position_file_path = ""
34 offsets_file_path= ""
35 init_servo_inv_file_path = ""
36 gait_steps_file_path= ""
37 axi_ip_log_file_path= ""
38
39 ###########################################################################
40 #### Memory Map Parameter
41 ###########################################################################
42 R1_LEG_IN_SELCT= 0x7
43 R1_SET_LEG_IN= 0x8
44 R1_COUNTER_MODE= 0x30
45 R1_LEG_OUTPUT= 0xC0
46 R1_F2F_READ_MUX= 0xE00
47 R1_PWM_INVERT= 0xFFFFC0000
48
49 ###########################################################################
50 #### Methods
51 ###########################################################################
52 #### Constructor
53 def __init__(self, invoke_axi_ip=True, enable_ip_logs=False):
54 if ( invoke_axi_ip ):
55 super(hexapod_kinematics, self).__init__(gen_log_enable=enable_ip_logs)
56 return None
57
58 #### Import & Export IP Params ############################################
59 ## Import parameters
60 def import_offsets(self):
61 if ( self.offsets_file_path == '' or not os.path.exists(self.offsets_file_path)
):
62 print('HEXAPOD CLASS > No joint offsets file
detected.\n>'+self.offsets_file_path)V1 -*- coding: utf-8 -*- -- VISUAL LINE
--1G387326 self.axi_write(6+leg*3, off_q2)
328 return True
329
331 def set_default_offsets(self, print_out=False):
333 offset_q1 = self.dfloat2hfloat(self.sec2rad(self.j_offs[i]
[0])).lstrip('x')
[1])).lstrip('x')
[2])).lstrip('x')
336
338 print('Setting leg '+str(i)+' offset')
339 print('\tQ1 offset = 0x'+offset_q1+' |
'+str(self.sec2rad(self.j_offs[i][0])))
342
343 self.axi_set_offset(i, int(offset_q1,16), int(offset_q2,16),
int(offset_q3,16))
344 return True
345
347 def set_init_position(self, print_out=False):
349 q1 = self.dfloat2hfloat(self.sec2rad(self.i_pos[i]
[0]+self.j_offs[i][0])).lstrip('x')
352
353 self.config_leg_ctr("one_leg", i)
354 self.axi_set_out_mux(i+1)
355 self.axi_write_params_in(q1, q2, q3)
356 self.axi_write_out_direct()
357
359 [pwm1, pwm2, pwm3] = self.axi_get_pwm(int(i))
360
361 print('Setting leg '+str(i)+' direct output')
365
366 return True
367
368 # Read offsets
369 def axi_read_offset(self, leg):
374
375 # Read offsets
376 def axi_hread_offset(self, leg):
381
382 # Get PWM values
383 def axi_get_pwm(self, leg):
387 return [pwm1, pwm2, pwm3] d 1~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
--No lines in buffer--:set pastei -- INSERT (paste) --# -*- coding: utf-8 -*-
2 import numpy as np 3 import mmap 4 ffrom time import sleep as sleep
5 from cordic_model import CORDIC as CORDIC
6 from numeric_conversions import numeric326 self.axi_write(6+leg*3,
off_q2)
328 return True
329 330 # Set read offsets331 def set_default_offsets(self,
print_out=False):
332 for i in range(6):333 offset_q1 =
self.dfloat2hfloat(self.sec2rad(self.j_offs[i][0])).lstrip('x')334
offset_q2 = self.dfloat2hfloat(self.sec2rad(self.j_offs[i][1])).lstrip('x')335
offset_q3 = self.dfloat2hfloat(self.sec2rad(self.j_offs[i][2])).lstrip('x')336
337 if ( print_out ):338 print('Setting leg '+str(i)+'
offset')339 print('\tQ1 offset = 0x'+offset_q1+' |
'+str(self.sec2rad(self.j_offs[i][0])))340 print('\tQ2 offset =
0x'+offset_q2+' | '+str(self.sec2rad(self.j_offs[i][1])))341
print('\tQ3 offset = 0x'+offset_q3+' | '+str(self.sec2rad(self.j_offs[i][2])))342
343 self.axi_set_offset(i, int(offset_q1,16), int(offset_q2,16),
int(offset_q3,16))344 return True345 346 # Set read offsets347
def set_init_position(self, print_out=False):348 for i in range(6):349
q1 = self.dfloat2hfloat(self.sec2rad(self.i_pos[i][0]+self.j_offs[i]
[0])).lstrip('x')350 q2 = self.dfloat2hfloat(self.sec2rad(self.i_pos[i]
[1]+self.j_offs[i][1])).lstrip('x')351 q3 =
self.dfloat2hfloat(self.sec2rad(self.i_pos[i][2]+self.j_offs[i][2])).lstrip('x')352
353 self.config_leg_ctr("one_leg", i)354
self.axi_set_out_mux(i+1)355 self.axi_write_params_in(q1, q2, q3)356
self.axi_write_out_direct()357 358 if ( print_out ):359
[pwm1, pwm2, pwm3] = self.axi_get_pwm(int(i))360 361
print('Setting leg '+str(i)+' direct output')362 print('\tQ1 =
0x'+q1+' | '+str(self.hfloat2dfloat(q1))+' |
'+str(self.rad2sec(self.hfloat2dfloat(q1)))+' | '+str(pwm1))363
print('\tQ2 = 0x'+q1+' | '+str(self.hfloat2dfloat(q2))+' |
'+str(self.rad2sec(self.hfloat2dfloat(q2)))+' | '+str(pwm2))364
print('\tQ3 = 0x'+q1+' | '+str(self.hfloat2dfloat(q3))+' |
'+str(self.rad2sec(self.hfloat2dfloat(q3)))+' | '+str(pwm3))365 366 return
True367 368 # Read offsets369 def axi_read_offset(self, leg):370
off1 = self.axi_read(5+leg*3)371 off2 = self.axi_read(6+leg*3)372
off3 = self.axi_read(7+leg*3)373 return [off1, off2, off3]374 375 #
Read offsets376 def axi_hread_offset(self, leg):377 off1 =
self.axi_hread(5+leg*3)378 off2 = self.axi_hread(6+leg*3)379 off3 =
self.axi_hread(7+leg*3)380 return [off1, off2, off3]381 382 # Get
PWM values383 def axi_get_pwm(self, leg):384 pwm1 =
(int(self.axi_hread(23+leg*3),16) & 0x3FC00) >> 10385 pwm2 =
(int(self.axi_hread(24+leg*3),16) & 0x3FC00) >> 10386 pwm3 =
(int(self.axi_hread(25+leg*3),16) & 0x3FC00) >> 10387 return [pwm1, pwm2,
pwm3][]^[ ::wq!"includes/hexapod_class.py" 387L, 15697C written
root@hexapod_zynq:~/hexapod_project/Modeling/Python# vim
test_locomotion_gait_translation.py
"test_locomotion_gait_translation.py" 228L, 11157C
E823: Not an undo file:
/home/root/hexapod_project/Modeling/Python/.test_locomotion_gait_translation.py.un~
Press ENTER or type command to continue 1 # -*- coding: utf-8 -*-
2 import os, sys
3 from math import sin as sin
4 from math import cos as cos
5 from math import pi as pi
6
7 ###############################################################################
8 #### Append Paths
9 ###############################################################################
10 pathname = os.path.dirname(sys.argv[0])
11 abs_path = os.path.abspath(pathname)
12 print('Appending path'+ abs_path+'/includes')
13 sys.path.append(abs_path+'/includes')
14 from hexapod_class import hexapod_kinematics as hexapod
15
16 ###############################################################################
17 #### Hexapod Configuration
18 ###############################################################################
19 #### Hexapod Object
20 hexapod = hexapod(invoke_axi_ip=True)
21
22 ## Import Parameters
23 hexapod.offsets_file_path= abs_path+'/params/joint_offset.params'
24 hexapod.init_position_file_path = abs_path+'/params/init_position.params'
25 hexapod.init_servo_inv_file_path = abs_path+'/params/init_servo_inv.params'
26
27 ## Initialize Parameters
28 hexapod.import_init_pos()
29 hexapod.import_offsets()
30 hexapod.import_init_servo_invertion()
31 hexapod.set_default_offsets(print_out=False)
32 hexapod.set_init_position(print_out=False)
33
34 ## Set Delay
35 hexapod.delay = 0.01
36
37
38 S = input('Insert S val = ') #10 #y stride length
39 res = input('Insert res = ')#0.1 #iteration resolution
40 xo = 12.38 #x offset
41 yo = float(5)#y offset
42 zo = -10.51 #z offset
43
44 angle = input('Insert alpha : ')
45 alf = angle*pi/180;
46
47 ## Selecci�n de caminata
48 walk = input('Insert Walk : ')
49
50 ###############################################################################
51 #### Traslational Locomotion
52 ###############################################################################
53 n_gaits = input('Insert N gaits : ')
54 for k in range (n_gaits):
55 for j in range(int(S/res)):
56 i=res*j;
57 if(walk==1):
58 ## Primera Etapa: 0<i<=S/2
59 if i<=S/2:
60 x1=xo+i*sin(-alf);y1=yo+i*cos(-alf);
61 x2=xo+(S/2-i)*sin(alf);y2=yo+(S/2-i)*cos(alf);z2=zo;
62 x3=xo+(S/4-i)*sin(-alf);y3=0 +(S/4-i)*cos(-alf);z3=zo;g gg V1 -*- coding: utf-
8 -*- -- VISUAL LINE --1G228167 ## Primera Etapa: 0<i<=2*S/3
168 if i<=2*S/3:
169 x4=xo-0.6*i*sin(alf); y4=0-0.6*i*cos(alf);
z4=zo;
171 if i<=S/6:
172 x5=xo+3*i*sin(-alf); y5=-yo+3*i*cos(-alf);
173 if S!=0:
174 z5=-(8*(3*i-S/4)**2/S-zo-S/2);
175 else:
176 z5=zo+S/2;
177 if i<=S/2:
178 x6=xo-0.6*i*sin(alf); y6=-yo-0.6*i*cos(alf);
z6=zo;
179
180 ## Segunda Etapa: S/3<i<=S/2
181 if i>S/3 and i<=S/2:
182 x1=xo+(3*i-1.2*S)*sin(-alf); y1=yo+(3*i-1.2*S)*cos(-alf);
z1=-(8*((3*i-1.2*S)-S/20)**2/S-zo-S/2);
183 ## Segunda Etapa: 5*S/6<i<=S
184 if i>5*S/6 and i<=S:
185 x2=xo+3*(i-S)*sin(alf); y2=yo+3*(i-S)*cos(alf);
z2=-(8*(3*(i-S)+S/4)**2/S-zo-S/2);
187 if i>S/6 and i<=S/3:
188 x3=xo+(3*i-0.6*S)*sin(-alf); y3=0+(3*i-0.6*S)*cos(-alf);
z3=-(8*((3*i-0.6*S)-3/20*S)**2/S-zo-S/2);
189 if i>2*S/3 and i<=5*S/6:
190 x4=xo+(3*i-2.4*S)*sin(alf); y4=0+(3*i-2.4*S)*cos(alf);
z4=-(8*((3*i-2.4*S)+3/20*S)**2/S-zo-S/2);
191 ## Segunda Etapa: S/6<i<=S
192 if i>S/6 and i<=S:
193 x5=xo+0.6*(S-i)*sin(-alf); y5=-yo+0.6*(S-i)*cos(-alf);
z5=zo;
194 ## Segunda Etapa: S/2<i<=2S/3
195 if i>S/2 and i<=2*S/3:
196 x6=xo+(3*i-1.8*S)*sin(alf); y6=-yo+(3*i-1.8*S)*cos(alf);
z6=-(8*((3*i-1.8*S)+S/20)**2/S-zo-S/2);
197
198 ## Tercera Etapa: S/2<i<=S
200 x1=xo+0.6*(S-i)*sin(-alf); y1=yo+0.6*(S-i)*cos(-alf);
z1=zo;
203 x3=xo+0.6*(S-i)*sin(-alf); y3=0+0.6*(S-i)*cos(-alf);
z3=zo;
204 ## Tercera Etapa: 5*S/6<i<=S
205 if i>5*S/6 and i<=S:
206 x4=xo+0.6*(S-i)*sin(alf); y4=0+0.6*(S-i)*cos(alf);
z4=zo;
207 ## Tercera Etapa: 2S/3<i<=S
208 if i>2*S/3 and i<=S:
209 x6=xo+0.6*(S-i)*sin(alf); y6=-yo+0.6*(S-i)*cos(alf);
z6=zo;
210
211 x1=x1/100; y1=y1/100; z1=z1/100;
212 x2=x2/100; y2=y2/100; z2=z2/100;
213 x3=x3/100; y3=y3/100; z3=z3/100;
214 x4=x4/100; y4=y4/100; z4=z4/100;
215 x5=x5/100; y5=y5/100; z5=z5/100;
216 x6=x6/100; y6=y6/100; z6=z6/100;
217
218 hexapod.joints[0] = [x1, y1, z1]
224 #hexapod.set_step()
225 hexapod.set_step_debug()
226 hexapod.step_delay()
227
228 hexapod.set_init_position(print_out=False) d 1~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
--No lines in buffer--::set pastei -- INSERT (paste) --# -*- coding: utf-8 -*-
2 import os, sys 3 import numpy as np
4 from math import sin as sin 5 from math import cos as cos
6 from math import pi as pi 7 8 ##########################167
x3=xo-0.6*i*sin(-alf);y3=0-0.6*i*cos(-alf);z3=zo;
168 ## Primera Etapa: 0<i<=2*S/3
169 if i<=2*S/3:
170 x4=xo-0.6*i*sin(alf);y4=0-0.6*i*cos(alf);z4=zo;
172 if i<=S/6:173x5=xo+3*i*sin(-alf);y5=-yo+3*i*cos(-alf);
174 if S!=0:175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);176
else:177 z5=zo+S/2;178 if i<=S/2:179
x6=xo-0.6*i*sin(alf); y6=-yo-0.6*i*cos(alf); z6=zo;180
181 ## Segunda Etapa: S/3<i<=S/2182 if i>S/3 and i<=S/2:183
x1=xo+(3*i-1.2*S)*sin(-alf); y1=yo+(3*i-1.2*S)*cos(-alf); z1=-(8*((3*i-
1.2*S)-S/20)**2/S-zo-S/2);184 ## Segunda Etapa: 5*S/6<i<=S185
if i>5*S/6 and i<=S:186 x2=xo+3*(i-S)*sin(alf); y2=yo+3*(i-
S)*cos(alf); z2=-(8*(3*(i-S)+S/4)**2/S-zo-S/2);187 ## Segunda
Etapa: S/6<i<=S/3188 if i>S/6 and i<=S/3:189
x3=xo+(3*i-0.6*S)*sin(-alf); y3=0+(3*i-0.6*S)*cos(-alf); z3=-(8*((3*i-
0.6*S)-3/20*S)**2/S-zo-S/2);190 if i>2*S/3 and i<=5*S/6:191
x4=xo+(3*i-2.4*S)*sin(alf); y4=0+(3*i-2.4*S)*cos(alf); z4=-(8*((3*i-2.4*S)
+3/20*S)**2/S-zo-S/2);192 ## Segunda Etapa: S/6<i<=S193 if
i>S/6 and i<=S:194 x5=xo+0.6*(S-i)*sin(-alf); y5=-yo+0.6*(S-
i)*cos(-alf); z5=zo;195 ## Segunda Etapa: S/2<i<=2S/3196
if i>S/2 and i<=2*S/3:197 x6=xo+(3*i-1.8*S)*sin(alf); y6=-
yo+(3*i-1.8*S)*cos(alf); z6=-(8*((3*i-1.8*S)+S/20)**2/S-zo-S/2);198 199
## Tercera Etapa: S/2<i<=S200 if i>S/2 and i<=S:201
x1=xo+0.6*(S-i)*sin(-alf); y1=yo+0.6*(S-i)*cos(-alf); z1=zo;202
x3=xo+0.6*(S-i)*sin(-alf); y3=0+0.6*(S-i)*cos(-alf); z3=zo;
205 ## Tercera Etapa: 5*S/6<i<=S206 if i>5*S/6 and i<=S:207
x4=xo+0.6*(S-i)*sin(alf); y4=0+0.6*(S-i)*cos(alf); z4=zo;208
## Tercera Etapa: 2S/3<i<=S209 if i>2*S/3 and i<=S:210
x6=xo+0.6*(S-i)*sin(alf); y6=-yo+0.6*(S-i)*cos(alf); z6=zo;211
212 x1=x1/100; y1=y1/100; z1=z1/100;213 x2=x2/100; y2=y2/100;
z2=z2/100;214 x3=x3/100; y3=y3/100; z3=z3/100;215 x4=x4/100;
y4=y4/100; z4=z4/100; 216 x5=x5/100; y5=y5/100; z5=z5/100;217
x6=x6/100; y6=y6/100; z6=z6/100;218 219 hexapod.joints[0] =
np.array([x1, y1, z1], np.float32)220 hexapod.joints[1] = np.array([x2, y2,
z2], np.float32)221 hexapod.joints[2] = np.array([x3, y3, z3],
np.float32)222 hexapod.joints[3] = np.array([x4, y4, z4], np.float32)223
hexapod.joints[4] = np.array([x5, y5, z5], np.float32)224 hexapod.joints[5]
= np.array([x6, y6, z6], np.float32)225 hexapod.set_step()226
hexapod.step_delay()227 228 hexapod.set_init_position(print_out=False)()^[ ::wq!
"test_locomotion_gait_translation.py" 228L, 11267C written
root@hexapod_zynq:~/hexapod_project/Modeling/Python# vim
test_locomotion_gait_translation.pyincludes/hexapod_class.pypy
Appending path/home/root/hexapod_project/Modeling/Python/includes
HEXAPOD CLASS > Import initial possitions successfuly :
/home/root/hexapod_project/Modeling/Python/params/init_position.params
HEXAPOD CLASS > Import offsets successfuly :
/home/root/hexapod_project/Modeling/Python/params/joint_offset.params
HEXAPOD CLASS > Import servo inversions successfuly :
/home/root/hexapod_project/Modeling/Python/params/init_servo_inv.params
Insert S val = 7
Insert res = 0.07
Insert alpha : 0
Insert Walk : 4
Insert N gaits : 1
root@hexapod_zynq:~/hexapod_project/Modeling/Python# v py
test_locomotion_gait_translation.pyvim test_locomotion_gait_translation.py
Press ENTER or type command to continue167 x3=xo-0.6*i*sin(-alf);y3=0-0.6*i*cos(-
alf);z3=zo;
169 if i<=2*S/3:
172 if i<=S/6:
173 x5=xo+3*i*sin(-alf);y5=-yo+3*i*cos(-alf);
174 if S!=0:
175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);
176 else:
177 z5=zo+S/2;
178 if i<=S/2:
179 x6=xo-0.6*i*sin(alf);y6=-yo-0.6*i*cos(alf);z6=zo;
180
182 if i>S/3 and i<=S/2:
183 x1=xo+(3*i-1.2*S)*sin(-alf); y1=yo+(3*i-1.2*S)*cos(-alf); z1=-(8*((3*i-
1.2*S)-S/20)**2/S-zo-S/2);
185 if i>5*S/6 and i<=S:
186 x2=xo+3*(i-S)*sin(alf);y2=yo+3*(i-S)*cos(alf);z2=-(8*(3*(i-S)+S/4)**2/S-zo-
S/2);
188 if i>S/6 and i<=S/3:
189 x3=xo+(3*i-0.6*S)*sin(-alf); y3=0+(3*i-0.6*S)*cos(-alf); z3=-(8*((3*i-
0.6*S)-3/20*S)**2/S-zo-S/2);
190 if i>2*S/3 and i<=5*S/6:
191 x4=xo+(3*i-2.4*S)*sin(alf); y4=0+(3*i-2.4*S)*cos(alf); z4=-(8*((3*i-
2.4*S)+3/20*S)**2/S-zo-S/2);
194 x5=xo+0.6*(S-i)*sin(-alf); y5=-yo+0.6*(S-i)*cos(-alf); z5=zo;
196 if i>S/2 and i<=2*S/3:
197 x6=xo+(3*i-1.8*S)*sin(alf); y6=-yo+(3*i-1.8*S)*cos(alf); z6=-(8*((3*i-
1.8*S)+S/20)**2/S-zo-S/2);
198
201 x1=xo+0.6*(S-i)*sin(-alf); y1=yo+0.6*(S-i)*cos(-alf); z1=zo;
204 x3=xo+0.6*(S-i)*sin(-alf); y3=0+0.6*(S-i)*cos(-alf);z3=zo;
206 if i>5*S/6 and i<=S:
207 x4=xo+0.6*(S-i)*sin(alf);y4=0+0.6*(S-i)*cos(alf);z4=zo;
209 if i>2*S/3 and i<=S:
210 x6=xo+0.6*(S-i)*sin(alf);y6=-yo+0.6*(S-i)*cos(alf); z6=zo;
211
212 x1=x1/100; y1=y1/100; z1=z1/100;
213 x2=x2/100; y2=y2/100; z2=z2/100;
214 x3=x3/100; y3=y3/100; z3=z3/100;
215 x4=x4/100; y4=y4/100; z4=z4/100;
216 x5=x5/100; y5=y5/100; z5=z5/100;
217 x6=x6/100; y6=y6/100; z6=z6/100;
218
219 hexapod.joints[0] = np.array([x1, y1, z1], np.float32)
225 hexapod.set_step()
227
228 hexapod.set_init_position(print_out=False)G ()u
Already at oldest change::226j $ $ ::q!
root@hexapod_zynq:~/hexapod_project/Modeling/Python# clear
root@hexapod_zynq:~/hexapod_project/Modeling/Python# clearvim
alf);z3=zo;
169 if i<=2*S/3:
172 if i<=S/6:
174 if S!=0:
175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);
176 else:
177 z5=zo+S/2;
178 if i<=S/2:
180
182 if i>S/3 and i<=S/2:
1.2*S)-S/20)**2/S-zo-S/2);
185 if i>5*S/6 and i<=S:
S/2);
188 if i>S/6 and i<=S/3:
0.6*S)-3/20*S)**2/S-zo-S/2);
190 if i>2*S/3 and i<=5*S/6:
2.4*S)+3/20*S)**2/S-zo-S/2);
196 if i>S/2 and i<=2*S/3:
1.8*S)+S/20)**2/S-zo-S/2);
198
206 if i>5*S/6 and i<=S:
209 if i>2*S/3 and i<=S:
211
212 x1=x1/100; y1=y1/100; z1=z1/100;
213 x2=x2/100; y2=y2/100; z2=z2/100;
214 x3=x3/100; y3=y3/100; z3=z3/100;
215 x4=x4/100; y4=y4/100; z4=z4/100;
216 x5=x5/100; y5=y5/100; z5=z5/100;
217 x6=x6/100; y6=y6/100; z6=z6/100;
218
227
228 hexapod.set_init_position(print_out=False)g gg 1 # -*- coding: utf-8 -*-
2 import os, sys
7
8 ###############################################################################
9 #### Append Paths
10 ###############################################################################
16
17 ###############################################################################
19 ###############################################################################
22
27
34
35 ## Set Delay
37
38
41 xo = 12.38 #x offset
42 yo = 5#y offset
43 zo = -10.51 #z offset
44
47
50
51 ###############################################################################
53 ###############################################################################
57 i=res*j;
58 if(walk==1):
60 if i<=S/2:
62 x2=xo+(S/2-i)*sin(alf);y2=yo+(S/2-i)*cos(alf);z2=zo; -*- coding: utf-8 -*- --
VISUAL LINE --G228167 x3=xo-0.6*i*sin(-alf); y3=0-
0.6*i*cos(-alf); z3=zo;
169 if i<=2*S/3:
z4=zo;
172 if i<=S/6:
174 if S!=0:
175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);
176 else:
177 z5=zo+S/2;
178 if i<=S/2:
z6=zo;
180
182 if i>S/3 and i<=S/2:
z1=-(8*((3*i-1.2*S)-S/20)**2/S-zo-S/2);
185 if i>5*S/6 and i<=S:
z2=-(8*(3*(i-S)+S/4)**2/S-zo-S/2);
188 if i>S/6 and i<=S/3:
z3=-(8*((3*i-0.6*S)-3/20*S)**2/S-zo-S/2);
190 if i>2*S/3 and i<=5*S/6:
z4=-(8*((3*i-2.4*S)+3/20*S)**2/S-zo-S/2);
z5=zo;
196 if i>S/2 and i<=2*S/3:
z6=-(8*((3*i-1.8*S)+S/20)**2/S-zo-S/2);
198
z1=zo;
z3=zo;
206 if i>5*S/6 and i<=S:
z4=zo;
209 if i>2*S/3 and i<=S:
z6=zo;
211
212 x1=x1/100; y1=y1/100; z1=z1/100;
213 x2=x2/100; y2=y2/100; z2=z2/100;
214 x3=x3/100; y3=y3/100; z3=z3/100;
215 x4=x4/100; y4=y4/100; z4=z4/100;
216 x5=x5/100; y5=y5/100; z5=z5/100;
217 x6=x6/100; y6=y6/100; z6=z6/100;
218
227
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
--No lines in buffer--i -- INSERT (paste) --^[ ::set pastei -- INSERT (paste) --#
-*- coding: utf-8 -*-
169 if i<=2*S/3:
174 if S!=0:175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);176
else:177 z5=zo+S/2;178 if i<=S/2:179
212 x1=x1/100; y1=y1/100; z1=z1/100;213 x2=x2/100; y2=y2/100;
z2=z2/100;214 x3=x3/100; y3=y3/100; z3=z3/100;215 x4=x4/100;
y4=y4/100; z4=z4/100; 216 x5=x5/100; y5=y5/100; z5=z5/100;217
= np.array([x6, y6, z6], np.float32)225 hexapod.set_step()226
hexapod.step_delay_debug()227 228 hexapod.set_init_position(print_out=False)
()^[ ::wq!"test_locomotion_gait_translation.py" 228L, 11273C written
Insert S val = 7
Insert res = 0.07
Insert alpha : 0
Insert Walk : 4
Insert N gaits : 1
Traceback (most recent call last):
File "test_locomotion_gait_translation.py", line 226, in <module>
hexapod.step_delay_debug()
AttributeError: 'hexapod_kinematics' object has no attribute 'step_delay_debug'
test_locomotion_gait_translation.pyvim test_locomotion_gait_translation.py
alf);z3=zo;
169 if i<=2*S/3:
172 if i<=S/6:
174 if S!=0:
175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);
176 else:
177 z5=zo+S/2;
178 if i<=S/2:
180
182 if i>S/3 and i<=S/2:
1.2*S)-S/20)**2/S-zo-S/2);
185 if i>5*S/6 and i<=S:
S/2);
188 if i>S/6 and i<=S/3:
0.6*S)-3/20*S)**2/S-zo-S/2);
190 if i>2*S/3 and i<=5*S/6:
2.4*S)+3/20*S)**2/S-zo-S/2);
196 if i>S/2 and i<=2*S/3:
1.8*S)+S/20)**2/S-zo-S/2);
198
206 if i>5*S/6 and i<=S:
209 if i>2*S/3 and i<=S:
211
212 x1=x1/100; y1=y1/100; z1=z1/100;
213 x2=x2/100; y2=y2/100; z2=z2/100;
214 x3=x3/100; y3=y3/100; z3=z3/100;
215 x4=x4/100; y4=y4/100; z4=z4/100;
216 x5=x5/100; y5=y5/100; z5=z5/100;
217 x6=x6/100; y6=y6/100; z6=z6/100;
218
226 hexapod.step_delay_debug()
227
228 hexapod.set_init_position(print_out=False)g gg 1 # -*- coding: utf-8 -*-
2 import os, sys
7
8 ###############################################################################
9 #### Append Paths
10 ###############################################################################
16
17 ###############################################################################
19 ###############################################################################
22
27
34
35 ## Set Delay
37
38
41 xo = 12.38 #x offset
42 yo = 5#y offset
43 zo = -10.51 #z offset
44
47
50
51 ###############################################################################
53 ###############################################################################
57 i=res*j;
58 if(walk==1):
60 if i<=S/2:
62 x2=xo+(S/2-i)*sin(alf);y2=yo+(S/2-i)*cos(alf);z2=zo;V1 -*- coding: utf-8 -*- --
VISUAL LINE --1G228167 x3=xo-0.6*i*sin(-alf); y3=0-
0.6*i*cos(-alf); z3=zo;
169 if i<=2*S/3:
z4=zo;
172 if i<=S/6:
174 if S!=0:
175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);
176 else:
177 z5=zo+S/2;
178 if i<=S/2:
z6=zo;
180
182 if i>S/3 and i<=S/2:
z1=-(8*((3*i-1.2*S)-S/20)**2/S-zo-S/2);
185 if i>5*S/6 and i<=S:
z2=-(8*(3*(i-S)+S/4)**2/S-zo-S/2);
188 if i>S/6 and i<=S/3:
z3=-(8*((3*i-0.6*S)-3/20*S)**2/S-zo-S/2);
190 if i>2*S/3 and i<=5*S/6:
z4=-(8*((3*i-2.4*S)+3/20*S)**2/S-zo-S/2);
z5=zo;
196 if i>S/2 and i<=2*S/3:
z6=-(8*((3*i-1.8*S)+S/20)**2/S-zo-S/2);
198
z1=zo;
z3=zo;
206 if i>5*S/6 and i<=S:
z4=zo;
209 if i>2*S/3 and i<=S:
z6=zo;
211
212 x1=x1/100; y1=y1/100; z1=z1/100;
213 x2=x2/100; y2=y2/100; z2=z2/100;
214 x3=x3/100; y3=y3/100; z3=z3/100;
215 x4=x4/100; y4=y4/100; z4=z4/100;
216 x5=x5/100; y5=y5/100; z5=z5/100;
217 x6=x6/100; y6=y6/100; z6=z6/100;
218
226 hexapod.step_delay_debug()
227
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
~
--No lines in buffer--::set pastei -- INSERT (paste) --# -*- coding: utf-8 -*-
169 if i<=2*S/3:
174 if S!=0:175 z5=-(8*(3*i-S/4)**2/S-zo-S/2);176
else:177 z5=zo+S/2;178 if i<=S/2:179
212 x1=x1/100; y1=y1/100; z1=z1/100;213 x2=x2/100; y2=y2/100;
z2=z2/100;214 x3=x3/100; y3=y3/100; z3=z3/100;215 x4=x4/100;
y4=y4/100; z4=z4/100; 216 x5=x5/100; y5=y5/100; z5=z5/100;217
= np.array([x6, y6, z6], np.float32)225 hexapod.set_step_debug()226
hexapod.step_delay()227 228 hexapod.set_init_position(print_out=False)()^[ ::wq!
"test_locomotion_gait_translation.py" 228L, 11273C written
Insert S val = 7
Insert res = 0.07
Insert alpha : 0
Insert Walk : 4
Insert N gaits : 1
[ 0.1238 0.05 -0.1051]
(21.982499390822479, -0.18009842803489623, -84.552714905528731)
[ 0.1238 0.04958 -0.1051 ]
(21.825131832345384, -0.19233812702755906, -84.633147213194803)
[ 0.1238 0.04916 -0.1051 ]
(21.650278989593058, -0.17834989960737296, -84.769532430541616)
[ 0.1238 0.04874 -0.1051 ]
(21.484168788978348, -0.152121973194524, -84.808000055947119)
[ 0.1238 0.04832 -0.1051 ]
(21.307567417798499, -0.13988227420186114, -84.9338941027288)
[ 0.1238 0.0479 -0.1051]
(21.150199859321404, -0.13638521734681461, -85.024817580960004)
[ 0.1238 0.04748 -0.1051 ]
(20.975347016569078, -0.15037344476700074, -85.10874694548113)
[ 0.1238 0.04706 -0.1051 ]
(20.812733872809414, -0.12764257520919828, -85.194424838429768)
[ 0.1238 0.04664 -0.1051 ]
(20.64487514376718, -0.16785872904223337, -85.287096845088499)
[ 0.1238 0.04622 -0.1051 ]
(20.461279658877238, -0.16785872904223337, -85.330810055776581)
[ 0.1238 0.0458 -0.1051]
(20.298666515117574, -0.14862491633947747, -85.418236477152746)
[ 0.1238 0.04538 -0.1051 ]
(20.125562200792771, -0.14512785948443094, -85.503914370101384)
[ 0.1238 0.04496 -0.1051 ]
(19.96469758546063, -0.13113963206424481, -85.591340791477549)
[ 0.1238 0.04454 -0.1051 ]
(19.774107986860596, -0.13988227420186114, -85.678767212853714)
[ 0.1238 0.04412 -0.1051 ]
(19.613243371528455, -0.12239698992662849, -85.759199520519786)
[ 0.1238 0.0437 -0.1051]
(19.438390528776129, -0.097917591941302798, -85.851871527178517)
[ 0.1238 0.04328 -0.1051 ]
(19.265286214451326, -0.11015729093396565, -85.937549420127155)
[ 0.1238 0.04286 -0.1051 ]
(19.093930428554046, -0.073438193955977102, -86.02497584150332)
[ 0.1238 0.04244 -0.1051 ]
(18.91383200051915, -0.08567789294863995, -86.114150791306997)
[ 0.1238 0.04202 -0.1051 ]
(18.745973271476917, -0.087426421376163216, -86.194583098973069)
[ 0.1238 0.0416 -0.1051]
(18.571120428724591, -0.071689665528453836, -86.245290423371245)
[ 0.1238 0.04118 -0.1051 ]
(18.408507284964926, -0.071689665528453836, -86.369435941725399)
[ 0.1238 0.04076 -0.1051 ]
(18.237151499067647, -0.076935250811023634, -86.411400623985955)
[ 0.1238 0.04034 -0.1051 ]
(18.039567786757516, -0.036719096977988551, -86.500575573789646)
[ 0.1238 0.03992 -0.1051 ]
(17.871709057715282, -0.066444080245884038, -86.539043199195163)
[ 0.1238 0.0395 -0.1051]
(17.695107686535433, -0.06294702339083752, -86.670182831259396)
[ 0.1238 0.03908 -0.1051 ]
(17.5272489574932, -0.043713210688081608, -86.757609252635561)
[ 0.1238 0.03866 -0.1051 ]
(17.343653472603258, -0.03147351169541876, -86.80307099175117)
[ 0.1238 0.03824 -0.1051 ]
(17.165303572995885, -0.041964682260558342, -86.887000356272281)
[ 0.1238 0.03782 -0.1051 ]
(16.990450730243559, -0.048958795970651399, -86.930713566960364)
[ 0.1238 0.0374 -0.1051]
(16.81384935906371, -0.017485284275232642, -87.063601727452138)
[ 0.1238 0.03698 -0.1051 ]
(16.638996516311384, -0.061198494963314247, -87.063601727452138)
[ 0.1238 0.03656 -0.1051 ]
(16.448406917711345, -0.01223969899266285, -87.145782563545723)
[ 0.1238 0.03614 -0.1051 ]
(16.270057018103973, -0.01573675584770938, -87.194741359516385)
[ 0.1238 0.03572 -0.1051 ]
(16.095204175351647, 0.0017485284275232642, -87.280419252465023)
[ 0.1238 0.0353 -0.1051]
(15.913357218889228, -0.01223969899266285, -87.362600088558608)
[ 0.1238 0.03488 -0.1051 ]
(15.738504376136902, -0.01573675584770938, -87.408061827674217)
[ 0.1238 0.03446 -0.1051 ]
(15.554908891246958, -0.0052455852825697927, -87.455272095217353)
[ 0.1238 0.03404 -0.1051 ]
(15.376558991639586, 0.0052455852825697927, -87.535704402883411)
[ 0.1238 0.03362 -0.1051 ]
(15.19646056360469, 0.0052455852825697927, -87.619633767404537)
[ 0.1238 0.0332 -0.1051]
(15.002373908149607, 0.033222040122942019, -87.710557245635741)
[ 0.1238 0.03278 -0.1051 ]
(14.841509292817467, -0.006994113710093057, -87.752521927896296)
[ 0.1238 0.03236 -0.1051 ]
(14.643925580507338, 0.038467625405511817, -87.836451292417422)
[ 0.1238 0.03194 -0.1051 ]
(14.467324209327488, 0.01223969899266285, -87.836451292417422)
[ 0.1238 0.03152 -0.1051 ]
(14.278483139154975, 0.0087426421376163212, -87.880164503105505)
[ 0.1238 0.0311 -0.1051]
(14.108875881685218, 0.006994113710093057, -87.969339452909182)
[ 0.1238 0.03068 -0.1051 ]
(13.907795112520043, -0.01223969899266285, -88.013052663597264)
[ 0.1238 0.03026 -0.1051 ]
(13.743433440332858, 0.01573675584770938, -88.100479084973429)
[ 0.1238 0.02984 -0.1051 ]
(13.547598256450252, 0.027976454840372228, -88.145940824089038)
[ 0.1238 0.02942 -0.1051 ]
(13.364002771560308, 0.027976454840372228, -88.186156977922067)
[ 0.1238 0.029 -0.1051]
(13.190898457235505, 0.027976454840372228, -88.268337814015666)
[ 0.1238 0.02858 -0.1051 ]
(12.998560330207946, 0.03147351169541876, -88.310302496276222)
[ 0.1238 0.02816 -0.1051 ]
(12.821958959028096, 0.020982341130279171, -88.355764235391831)
[ 0.1238 0.02774 -0.1051 ]
(12.626123775145491, 0.013988227420186114, -88.402974502934953)
[ 0.1238 0.02732 -0.1051 ]
(12.435534176545456, 0.05420438125322119, -88.485155339028552)
[ 0.1238 0.0269 -0.1051]
(12.251938691655512, 0.048958795970651399, -88.485155339028552)
[ 0.1238 0.02648 -0.1051 ]
(12.075337320475663, 0.029724983267895494, -88.57083323197719)
[ 0.1238 0.02606 -0.1051 ]
(11.88125066502058, 0.064695551818360772, -88.658259653353355)
[ 0.1238 0.02564 -0.1051 ]
(11.69415812327559, 0.017485284275232642, -88.619792027947838)
[ 0.1238 0.02522 -0.1051 ]
(11.517556752095741, 0.036719096977988551, -88.742189017874466)
[ 0.1238 0.0248 -0.1051]
(11.333961267205799, 0.036719096977988551, -88.742189017874466)
[ 0.1238 0.02438 -0.1051 ]
(11.139874611750717, 0.043713210688081608, -88.792896342272641)
[ 0.1238 0.02396 -0.1051 ]
(10.961524712143344, 0.052455852825697931, -88.84185513824329)
[ 0.1238 0.02354 -0.1051 ]
(10.756946886123123, -0.01223969899266285, -88.880322763648806)
[ 0.1238 0.02312 -0.1051 ]
(10.585591100225841, -0.006994113710093057, -88.924035974336888)
[ 0.1238 0.0227 -0.1051]
(10.389755916343237, 0.006994113710093057, -88.964252128169917)
[ 0.1238 0.02228 -0.1051 ]
(10.20441190302577, 0.013988227420186114, -89.009713867285527)
[ 0.1238 0.02186 -0.1051 ]
(10.013822304425734, 0.036719096977988551, -89.048181492691043)
[ 0.1238 0.02144 -0.1051 ]
(9.8197356489706529, 0.047210267543128133, -89.095391760234165)
[ 0.1238 0.02102 -0.1051 ]
(9.6361401640807092, 0.048958795970651399, -89.13735644249472)
[ 0.1238 0.0206 -0.1051]
(9.4525446791907672, 0.057701438108267722, -89.17932112475529)
[ 0.1238 0.02018 -0.1051 ]
(9.2567094953081615, 0.064695551818360772, -89.217788750160793)
[ 0.1238 -0.0088 -0.34726456]
(-4.061831537136543, -81.070520542116142, 108.47083526561946)
[ 0.1238 -0.0067 -0.32434458]
(-3.0948953167161779, -64.092309510865249, -121.250829542387)
[ 0.1238 -0.0046 -0.30243257]
(-2.1192164541581962, -73.992477467501971, 8.1717476060299763)
[ 0.1238 -0.0025 -0.28152856]
(-1.1435375916002148, -72.011394759118119, 3.8511338616199895)
[ 0.1238 -0.0004 -0.26163256]
(-0.185344013317466, -69.276696298471734, -1.737162992744363)
[ 0.1238 0.0017 -0.24274457]
(0.78683779238546891, -68.370958573014676, -0.074312458169738735)
[ 0.1238 0.0038 -0.22486457]
(1.7537740128058341, -66.790288874533644, -0.0026227926412848964)
[ 0.1238 0.0059 -0.20799257]
(2.7294528753638154, -65.139678038951686, 0.0026227926412848964)
[ 0.1238 0.008 -0.19212857]
(3.6981376242117041, -63.338693758602723, -0.0026227926412848964)
[ 0.1238 0.0101 -0.17727257]
(4.6545826740669298, -61.368102220784003, 0.0026227926412848964)
[ 0.1238 0.0122 -0.16342457]
(5.6232674229148181, -39.000926575906412, -38.928362646164196)
[ 0.1238 0.0143 -0.15058458]
(6.5867065864801368, -28.583194204722801, -54.565452373504748)
[ 0.1238 0.0164 -0.13875256]
(7.5396545794803158, -20.560945779246065, -65.539216784640757)
[ 0.1238 0.0185 -0.12792857]
(8.5030937430456337, -13.704965814927345, -74.075532567809333)
[ 0.1238 0.0206 -0.11811257]
(9.4525446791907672, -7.7354897633629207, -80.889547849867483)
[ 0.1238 0.0227 -0.10930457]
(10.389755916343237, -2.4496883269600933, -86.411400623985955)
[ 0.1238 0.0248 -0.10150457]
(11.333961267205799, 2.1506899658536152, -90.824686375054682)
[ 0.1238 0.0269 -0.09471257]
(12.251938691655512, 6.216018559845204, -94.363707912361761)
[ 0.1238 0.029 -0.08892857]
(13.190898457235505, 9.6746077894862204, -97.142119583696228)
[ 0.1238 0.0311 -0.08415257]
(14.108875881685218, 12.522960597921619, -99.194891957608547)
[ 0.1238 0.0332 -0.08038457]
(15.002373908149607, 14.834515179107374, -100.73359697382901)
[ 0.1238 0.0353 -0.07762457]
(15.913357218889228, 16.5725524360655, -101.65681998356131)
[ 0.1238 0.0374 -0.07587257]
(16.81384935906371, 17.663634174840016, -102.09744914729716)
[ 0.1238 0.0395 -0.07512857]
(17.695107686535433, 18.034322201474946, -102.05723299346413)
[ 0.1238 0.0416 -0.07539257]
(18.571120428724591, 17.786031164766644, -101.48021861238145)
[ 0.1238 0.0437 -0.07666457]
(19.438390528776129, 16.887287553019686, -100.51153386353356)
[ 0.1238 0.0458 -0.07894457]
(20.298666515117574, 15.41677514547262, -99.067249382399339)
root@hexapod_zynq:~/hexapod_project/Modeling/Python#

Upd2 Pentapod Pata2

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Upd2 Pentapod Pata2

Uploaded by

Copyright:

Available Formats

=~=~=~=~=~=~=~=~=~=~=~= PuTTY log 2020.04.

You might also like