def next_point(dx, dy, dz, mt):
ports = pypot.dynamixel.get_available_ports()
with pypot.dynamixel.DxlIO(ports[0], baudrate=1000000) as dxl_io:
global a
#Recuperation de la position en x,y,z d'une patte
for i in [1, 3, 5]:
ids = i
x = dxl_io.get_present_position([ids * 10 + 1])
y = dxl_io.get_present_position([ids * 10 + 2])
z = dxl_io.get_present_position([ids * 10 + 3])
x = x[0]
y = y[0]
z = z[0]
kinematics.computeDK(x, y, z)
#Ajout de la derivee a la position de la patte
#Exemple: ny= nouvelle valeur, y=valeur lue sur la patte, dy=valeur a ajouter
#mutliplication par sin(mt)
# Si mt=0--> nx=x+dx*math.sin(mt) => nx=x
nx = x + dx * math.sin(mt)
ny = y + dy * math.sin(mt)
nz = z + dz * math.sin(mt)
print("nx=", nx)
print("ny=", ny)
print("nz=", nz)
#Envoi des nouvelles positions pour le deplacement du robot
calcul.set_leg_pos_robot_frame(ids, nx, ny, nz)
def set_goal_position():
dx = leg_init_pos[0]
dy = leg_init_pos[1]
dz = leg_init_pos[2]
print("dx=", dx, "dy=", dy, "dz=", dz)
for i in [1, 2, 3, 4, 5, 6]:
leg_id = i
calcul.set_leg_pos_robot_frame(leg_id, dx, dy, dz)
def next_point(ids, dx, dy, dz, mt):
#Recuperation de la position en x,y,z d'une patte
x = dxl_io.get_present_position([ids * 10 + 1])
y = dxl_io.get_present_position([ids * 10 + 2])
z = dxl_io.get_present_position([ids * 10 + 3])
x = x[0]
y = y[0]
z = z[0]
kinematics.computeDK(x, y, z)
global a
#Ajout de la derivee a la position de la patte
if mt == 0:
if a == 0:
print("Boucle 1")
nx = x + dx * math.sin(mt)
ny = y + dy * math.sin(mt)
nz = z + dz
if y > -0.6 and y < 0.4:
a = 1
return a
if x > 154.7 and x < 155.3:
c = 1
return c
if a == 1 or c == 1:
ny = y + dy * math.sin(mt)
nz = z - dz
print("Boucle 2")
if y > 39.6 and y < 40.4:
a = 2
return a
if x > 170.7 and x < 171.3:
c = 2
return c
if a == 2:
ny = y - 2 * dy
print("Boucle 3")
if y > -40.6 and y < -39.8:
a = 0
return a
if c == 2:
nx = x - 2 * dx
if x > 139.7 and x < 1410.3:
c = 0
return c
#Envoi des nouvelles positions pour le deplacement du robot
calcul.set_leg_pos_robot_frame(ids, nx, ny, nz)
def set_goal_position2():
dx = leg_init_pos[0]
dy = leg_init_pos[1]
dz = leg_init_pos[2]
print("dx=", dx, "dy=", dy, "dz=", dz)
for i in [2, 4, 6]:
leg_id = i
calcul.set_leg_pos_robot_frame(leg_id, dx, dy, dz)
# ########### Go left ###########
# if mt==2:
# if b==0:
# print("Boucle 1")
# print("Leg_init_pos=",leg_init_pos)
# leg_delta=[ dx , dy , -dz]
# leg_init_pos[0]=leg_init_pos[0]+(leg_delta[0])
# leg_init_pos[1]=leg_init_pos[1]+(leg_delta[1])
# leg_init_pos[2]=leg_init_pos[2]+(leg_delta[2])
# x=leg_init_pos[0]
# z=leg_init_pos[2]
# if x==155 :
# print("Leg_init_pos=",leg_init_pos)
# b=1
# if b==1:
# b=1
# print("Boucle 2")
# print("Leg_init_pos=",leg_init_pos)
# leg_delta=[ dx , dy , dz]
# leg_init_pos[0]=leg_init_pos[0]+(leg_delta[0])
# leg_init_pos[1]=leg_init_pos[1]+(leg_delta[1])
# leg_init_pos[2]=leg_init_pos[2]+(leg_delta[2])
# x=leg_init_pos[0]
# z=leg_init_pos[2]
# if x==170 and z==90:
# print("Leg_init_pos=",leg_init_pos)
# b=2
# if b==2:
# b=2
# print("Boucle 3")
# print("Leg_init_pos=",leg_init_pos)
# leg_delta=[ -3*dx , dy , 0]
# leg_init_pos[0]=leg_init_pos[0]+(leg_delta[0])
# leg_init_pos[1]=leg_init_pos[1]+(leg_delta[1])
# leg_init_pos[2]=leg_init_pos[2]+(leg_delta[2])
# x=leg_init_pos[0]
# z=leg_init_pos[2]
# if x==140 and z==90:
# print("Leg_init_pos=",leg_init_pos)
# b=0
# return b"""########### Go right ###########
# if mt==3:
# if a==0:
# print("Boucle 1")
# leg_delta=[ dx , dy , -dz]
# leg_init_pos[0]=leg_init_pos[0]+(leg_delta[0])
# leg_init_pos[1]=leg_init_pos[1]+(leg_delta[1])
# leg_init_pos[2]=leg_init_pos[2]+(leg_delta[2])
# x=leg_init_pos[0]
# z=leg_init_pos[2]
# if x==155 :
# print("Leg_init_pos=",leg_init_pos)
# a=1
# if a==1:
# a=1
# print("Boucle 2")
# leg_delta=[ dx , dy , dz]
# leg_init_pos[0]=leg_init_pos[0]+(leg_delta[0])
# leg_init_pos[1]=leg_init_pos[1]+(leg_delta[1])
# leg_init_pos[2]=leg_init_pos[2]+(leg_delta[2])
# x=leg_init_pos[0]
# z=leg_init_pos[2]
# if x==40 and z==90:
# print("Leg_init_pos=",leg_init_pos)
# a=2
# if a==2:
# a=2
# print("Boucle 3")
# leg_delta=[ dx , -2*dy , 0]
# leg_init_pos[0]=leg_init_pos[0]+(leg_delta[0])
# leg_init_pos[1]=leg_init_pos[1]+(leg_delta[1])
# leg_init_pos[2]=leg_init_pos[2]+(leg_delta[2])
# x=leg_init_pos[0]
# z=leg_init_pos[2]
# if x==-40 and z==90:
# print("Leg_init_pos=",leg_init_pos)
# a=0
# return a
# if x<=155 and z<=0:
# leg_delta=[ x , y , -z ]
# leg_final_pos[j]=leg_init_pos[i][j]+(leg_delta[j])
# if x<=170 and z<=90:
# leg_delta=[ x , y , z ]
# leg_final_pos[j]=leg_init_pos[i][j]+(leg_delta[j])
# if z>90:
# leg_delta=[ -3*x , y , 0 ]
# leg_final_pos[j]=leg_init_pos[i][j]+(leg_delta[j])
# if x==140 and z<90:
# leg_delta=[ x , y , -z ]
# leg_final_pos[j]=leg_init_pos[i][j]+(leg_delta[j])