def StepCut_Leg5_Link(x0,y0,z0,x1,y1,z1,tx,ty,tz,s,step,date):
H = 50
if (s<=step):
x = (x1-tx)/step*s + tx
y = (y1-ty)/step*s + ty
z = (z1-tz)/step*s + tz
stepdrive.InverseKinematics_Leg5(x,y,z,date)
else:
x = (x0-x1)/step*(s-step) + x1
y = (y0-y1)/step*(s-step) + y1
z = (z0-z1)/step*(s-step) + z1 + (-H)*4/(step*step)*(s-step)*(s-2*step)
stepdrive.InverseKinematics_Leg5(x,y,z,date)
def StepCut_Leg5(x0, y0, z0, x1, y1, z1, s, step, date):
H = 60
if (s <= step):
x = (x1 - x0) / step * s + x0
y = (y1 - y0) / step * s + y0
z = (z1 - z0) / step * s + z0
stepdrive.InverseKinematics_Leg5(x, y, z, date)
else:
x = (x0 - x1) / step * (s - step) + x1
y = (y0 - y1) / step * (s - step) + y1
z = (z0 - z1) / step * (s - step) + z1 + (-H) * 4 / (step * step) * (
s - step) * (s - 2 * step)
stepdrive.InverseKinematics_Leg5(x, y, z, date)
def Robot_Init():
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(180, 240, -95, date)
IK.InverseKinematics_Leg2(230, 0, -95, date)
IK.InverseKinematics_Leg3(180, -240, -95, date)
IK.InverseKinematics_Leg4(-180, 240, -95, date)
IK.InverseKinematics_Leg5(-230, 0, -95, date)
IK.InverseKinematics_Leg6(-180, -240, -95, date)
Uart.SendDate(date)
time.sleep(0.5)
def Robot_Init():
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(GP.Leg1_Pos[0],GP.Leg1_Pos[1],GP.Leg1_Pos[2],date)
IK.InverseKinematics_Leg2(GP.Leg2_Pos[0],GP.Leg2_Pos[1],GP.Leg2_Pos[2],date)
IK.InverseKinematics_Leg3(GP.Leg3_Pos[0],GP.Leg3_Pos[1],GP.Leg3_Pos[2],date)
IK.InverseKinematics_Leg4(GP.Leg4_Pos[0],GP.Leg4_Pos[1],GP.Leg4_Pos[2],date)
IK.InverseKinematics_Leg5(GP.Leg5_Pos[0],GP.Leg5_Pos[1],GP.Leg5_Pos[2],date)
IK.InverseKinematics_Leg6(GP.Leg6_Pos[0],GP.Leg6_Pos[1],GP.Leg6_Pos[2],date)
Uart.SendDate(date)
time.sleep(0.5)
def robot_stand():
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(145, 155, -90, date)
IK.InverseKinematics_Leg3(145, -155, -90, date)
IK.InverseKinematics_Leg5(-185, 0, -90, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(145, 155, -95, date)
IK.InverseKinematics_Leg3(145, -155, -95, date)
IK.InverseKinematics_Leg5(-185, 0, -95, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg2(185, 0, -90, date)
IK.InverseKinematics_Leg4(-145, 155, -90, date)
IK.InverseKinematics_Leg6(-145, -155, -90, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x20\x03\x09')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg2(185, 0, -95, date)
IK.InverseKinematics_Leg4(-145, 155, -95, date)
IK.InverseKinematics_Leg6(-145, -155, -95, date)
Uart.SendDate(date)
time.sleep(0.1)
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1( 145, 155,-255, date)
IK.InverseKinematics_Leg2( 185, 0, -255, date)
IK.InverseKinematics_Leg3( 145,-155,-255, date)
IK.InverseKinematics_Leg4(-145, 155,-255, date)
IK.InverseKinematics_Leg5(-185, 0, -255, date)
IK.InverseKinematics_Leg6(-145,-155,-255, date)
Uart.SendDate(date)
time.sleep(0.5)
def test():
Xa = 130
Ya = 130
Za = -90
Xb = 250
Yb = 0
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(Xa, Ya, Za, date)
IK.InverseKinematics_Leg2(Xb, Yb, Za, date)
IK.InverseKinematics_Leg3(Xa, -Ya, Za, date)
IK.InverseKinematics_Leg4(-Xa, Ya, Za, date)
IK.InverseKinematics_Leg5(-Xb, Yb, Za, date)
IK.InverseKinematics_Leg6(-Xa, -Ya, Za, date)
Uart.SendDate(date)
def Robot_Init():
x1 = 200
y1 = 240
z1 = -130
x2 = 250
y2 = 0
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
IK.InverseKinematics_Leg1(x1, y1, z1, date)
IK.InverseKinematics_Leg2(x2, y2, z1, date)
IK.InverseKinematics_Leg3(x1, -y1, z1, date)
IK.InverseKinematics_Leg4(-x1, y1, z1, date)
IK.InverseKinematics_Leg5(-x2, y2, z1, date)
IK.InverseKinematics_Leg6(-x1, -y1, z1, date)
Uart.SendDate(date)
time.sleep(0.5)
def Test_Gait(L, A):
angle = A / 180 * PI
for i in range(17):
x4 = Xa + i * (L * math.cos(angle)) / 16
y4 = Ya + i * (L * math.sin(angle)) / 16
z4 = Za + H * He[i]
GP.Leg4_Pos[0] = x4
GP.Leg4_Pos[1] = y4
GP.Leg4_Pos[2] = z4
IK.InverseKinematics_Leg4(x4, y4, z4, date)
#MOVE1
if (Index1 == 0):
break
for i in range(17):
x1 = Xf + i * (L * math.cos(angle)) / 16
y1 = Yf + i * (L * math.sin(angle)) / 16
z1 = Zf + H * He[i]
GP.Leg1_Pos[0] = x1
GP.Leg1_Pos[1] = y1
GP.Leg1_Pos[2] = z1
IK.InverseKinematics_Leg1(x1, y1, z1, date)
#MOVE1
if (Index1 == 0):
break
if (GP.Leg4_Pos[2] > Za):
q = Za - GP.Leg4_Pos[2]
GP.Leg4_Pos[2] = Za
GP.Leg1_Pos[2] = GP.Leg1_Pos[2] - q * ((GP.Leg1_Pos[1] + 155) /
(GP.Leg4_Pos[1] + 155))
GP.Leg2_Pos[2] = GP.Leg2_Pos[2] - q * ((155) / (GP.Leg4_Pos[1] + 155))
GP.Leg5_Pos[2] = GP.Leg5_Pos[2] - q * ((155) / (GP.Leg4_Pos[1] + 155))
IK.InverseKinematics_Leg1(GP.Leg1_Pos[0], GP.Leg1_Pos[1],
GP.Leg1_Pos[2], date)
IK.InverseKinematics_Leg2(GP.Leg2_Pos[0], GP.Leg2_Pos[1],
GP.Leg2_Pos[2], date)
IK.InverseKinematics_Leg4(GP.Leg4_Pos[0], GP.Leg4_Pos[1],
GP.Leg4_Pos[2], date)
IK.InverseKinematics_Leg5(GP.Leg5_Pos[0], GP.Leg5_Pos[1],
GP.Leg5_Pos[2], date)
#MOVE
x1 = GP.Leg1_Pos[0] - L / 3 * math.cos(angle)
y1 = GP.Leg1_Pos[1] - L / 3 * math.sin(angle)
z1 = GP.Leg1_Pos[2]
x2 = GP.Leg2_Pos[0] - L / 3 * math.cos(angle)
y2 = GP.Leg2_Pos[1] - L / 3 * math.sin(angle)
z2 = GP.Leg2_Pos[2]
x3 = GP.Leg3_Pos[0] - L / 3 * math.cos(angle)
y3 = GP.Leg3_Pos[1] - L / 3 * math.sin(angle)
z3 = GP.Leg3_Pos[2]
x4 = GP.Leg4_Pos[0] - L / 3 * math.cos(angle)
y4 = GP.Leg4_Pos[1] - L / 3 * math.sin(angle)
z4 = GP.Leg4_Pos[2]
x5 = GP.Leg5_Pos[0] - L / 3 * math.cos(angle)
y5 = GP.Leg5_Pos[1] - L / 3 * math.sin(angle)
z5 = GP.Leg5_Pos[2]
x6 = GP.Leg6_Pos[0] - L / 3 * math.cos(angle)
y6 = GP.Leg6_Pos[1] - L / 3 * math.sin(angle)
z6 = GP.Leg6_Pos[2]
GP.Leg1_Pos[0] = x1
GP.Leg1_Pos[1] = y1
GP.Leg1_Pos[2] = z1
GP.Leg2_Pos[0] = x2
GP.Leg2_Pos[1] = y2
GP.Leg2_Pos[2] = z2
GP.Leg3_Pos[0] = x3
GP.Leg3_Pos[1] = y3
GP.Leg3_Pos[2] = z3
GP.Leg4_Pos[0] = x4
GP.Leg4_Pos[1] = y4
GP.Leg4_Pos[2] = z4
GP.Leg5_Pos[0] = x5
GP.Leg5_Pos[1] = y5
GP.Leg5_Pos[2] = z5
GP.Leg6_Pos[0] = x6
GP.Leg6_Pos[1] = y6
GP.Leg6_Pos[2] = z6
for i in range(17):
x5 = Xb + i * (L * math.cos(angle)) / 16
y5 = Yb + i * (L * math.sin(angle)) / 16
z5 = Zb + H * He[i]
GP.Leg5_Pos[0] = x5
GP.Leg5_Pos[1] = y5
GP.Leg5_Pos[2] = z5
#MOVE1
if (Index1 == 0):
break
for i in range(17):
x2 = Xe + i * (L * math.cos(angle)) / 16
y2 = Ye + i * (L * math.sin(angle)) / 16
z2 = Ze + H * He[i]
GP.Leg2_Pos[0] = x2
GP.Leg2_Pos[1] = y2
GP.Leg2_Pos[2] = z2
#MOVE1
x1 = GP.Leg1_Pos[0] - L / 3 * math.cos(angle)
y1 = GP.Leg1_Pos[1] - L / 3 * math.sin(angle)
z1 = GP.Leg1_Pos[2]
x2 = GP.Leg2_Pos[0] - L / 3 * math.cos(angle)
y2 = GP.Leg2_Pos[1] - L / 3 * math.sin(angle)
z2 = GP.Leg2_Pos[2]
x3 = GP.Leg3_Pos[0] - L / 3 * math.cos(angle)
y3 = GP.Leg3_Pos[1] - L / 3 * math.sin(angle)
z3 = GP.Leg3_Pos[2]
x4 = GP.Leg4_Pos[0] - L / 3 * math.cos(angle)
y4 = GP.Leg4_Pos[1] - L / 3 * math.sin(angle)
z4 = GP.Leg4_Pos[2]
x5 = GP.Leg5_Pos[0] - L / 3 * math.cos(angle)
y5 = GP.Leg5_Pos[1] - L / 3 * math.sin(angle)
z5 = GP.Leg5_Pos[2]
x6 = GP.Leg6_Pos[0] - L / 3 * math.cos(angle)
y6 = GP.Leg6_Pos[1] - L / 3 * math.sin(angle)
z6 = GP.Leg6_Pos[2]
GP.Leg1_Pos[0] = x1
GP.Leg1_Pos[1] = y1
GP.Leg1_Pos[2] = z1
GP.Leg2_Pos[0] = x2
GP.Leg2_Pos[1] = y2
GP.Leg2_Pos[2] = z2
GP.Leg3_Pos[0] = x3
GP.Leg3_Pos[1] = y3
GP.Leg3_Pos[2] = z3
GP.Leg4_Pos[0] = x4
GP.Leg4_Pos[1] = y4
GP.Leg4_Pos[2] = z4
GP.Leg5_Pos[0] = x5
GP.Leg5_Pos[1] = y5
GP.Leg5_Pos[2] = z5
GP.Leg6_Pos[0] = x6
GP.Leg6_Pos[1] = y6
GP.Leg6_Pos[2] = z6
for i in range(17):
x6 = Xc + i * (L * math.cos(angle)) / 16
y6 = Yc + i * (L * math.sin(angle)) / 16
z6 = Zc + H * He[i]
GP.Leg6_Pos[0] = x6
GP.Leg6_Pos[1] = y6
GP.Leg6_Pos[2] = z6
#MOVE1
if (Index1 == 0):
break
for i in range(17):
x3 = Xd + i * (L * math.cos(angle)) / 16
y3 = Yd + i * (L * math.sin(angle)) / 16
z3 = Zd + H * He[i]
GP.Leg3_Pos[0] = x3
GP.Leg3_Pos[1] = y3
GP.Leg3_Pos[2] = z3
def twist_all(x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4, x5, y5, z5, x6,
y6, z6, gamma, beta, zc, leg_id):
Rz = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
Ry = np.array([[m.cos(beta), 0, m.sin(beta)], [0, 1, 0],
[-m.sin(beta), 0, m.cos(beta)]])
Rx = np.array([[1, 0, 0], [0, m.cos(gamma), -m.sin(gamma)],
[0, m.sin(gamma), m.cos(gamma)]])
R1 = np.dot(
Rz,
Ry,
)
R = np.dot(R1, Rx)
Rp = np.array([[R[0][0], R[0][1], R[0][2], 0],
[R[1][0], R[1][1], R[1][2], 0],
[R[2][0], R[2][1], R[2][2], zc], [0, 0, 0, 1]])
# one = np.array([[-114.94], [261.0668], [-140], [1]])
# two = np.array([[-276], [0], [-140], [1]])
# three = np.array([[-114.94], [-261.0668], [-140], [1]])
# four = np.array([[114.94], [-261.06680], [-140], [1]])
# five = np.array([[276], [0], [-140], [1]])
# six = np.array([[114.94], [261.0668], [-140], [1]])
one = np.array([[x1], [y1], [z1], [1]])
two = np.array([[x2], [y2], [z2], [1]])
three = np.array([[x3], [y3], [z3], [1]])
four = np.array([[x4], [y4], [z4], [1]])
five = np.array([[x5], [y5], [z5], [1]])
six = np.array([[x6], [y6], [z6], [1]])
P_one = np.dot(Rp, one)
P_two = np.dot(Rp, two)
P_three = np.dot(Rp, three)
P_four = np.dot(Rp, four)
P_five = np.dot(Rp, five)
P_six = np.dot(Rp, six)
date = bytearray(b'\x55\x55\x3B\x03\x12')
date.extend(Uart.AddTime(65))
#print(P_one[0],P_one[1],z1)
#stepdrive.InverseKinematics_Leg1(P_one[0],P_one[1],P_one[2],date)
#stepdrive.InverseKinematics_Leg2(P_two[0],P_two[1],P_two[2],date)
#stepdrive.InverseKinematics_Leg3(P_three[0],P_three[1],P_three[2],date)
#stepdrive.InverseKinematics_Leg4(P_four[0],P_four[1],P_four[2],date)
#stepdrive.InverseKinematics_Leg5(P_five[0],P_five[1],P_five[2],date)
#stepdrive.InverseKinematics_Leg6(P_six[0],P_six[1],P_six[2],date)
if leg_id == 1:
stepdrive.InverseKinematics_Leg1(P_one[0], P_one[1], z1, date)
else:
stepdrive.InverseKinematics_Leg1(P_one[0], P_one[1], P_one[2], date)
if leg_id == 2:
stepdrive.InverseKinematics_Leg2(P_two[0], P_two[1], z2, date)
else:
stepdrive.InverseKinematics_Leg2(P_two[0], P_two[1], P_two[2], date)
if leg_id == 3:
stepdrive.InverseKinematics_Leg3(P_three[0], P_three[1], z3, date)
else:
stepdrive.InverseKinematics_Leg3(P_three[0], P_three[1], P_three[2],
date)
if leg_id == 4:
stepdrive.InverseKinematics_Leg4(P_four[0], P_four[1], z4, date)
else:
stepdrive.InverseKinematics_Leg4(P_four[0], P_four[1], P_four[2], date)
if leg_id == 5:
stepdrive.InverseKinematics_Leg5(P_five[0], P_five[1], z5, date)
else:
stepdrive.InverseKinematics_Leg5(P_five[0], P_five[1], P_five[2], date)
if leg_id == 6:
stepdrive.InverseKinematics_Leg6(P_six[0], P_six[1], z6, date)
else:
stepdrive.InverseKinematics_Leg6(P_six[0], P_six[1], P_six[2], date)
#print(P_two)
Uart.SendDate(date)
