def turnRight():
T = np.linspace(0, step_periode, calc_rate * step_periode)
for t in T: #left foot side
x = walkingSinX(t)
dataPub = FullBodyIK()
dataPub.right_leg.x = x
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = -x
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
for t in T: # right foot up
z = walkingSinZ(t)
y = t/step_periode * step_distance
rotate = t/step_periode * rotate_angle
dataPub = FullBodyIK()
dataPub.right_leg.x = side_distance
dataPub.right_leg.y = y
dataPub.right_leg.z = 26.0 - z
dataPub.right_leg.rotate = rotate
dataPub.left_leg.x = -side_distance
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
for t in T: #left foot side
x = walkingSinX(t)
dataPub = FullBodyIK()
dataPub.right_leg.x = -x
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = x
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
for t in T: # left foot up
z = walkingSinZ(t)
y = t/step_periode * step_distance
rotate = t/step_periode * rotate_angle
dataPub = FullBodyIK()
dataPub.right_leg.x = -side_distance
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = side_distance
dataPub.left_leg.y = y
dataPub.left_leg.z = 26.0 - z
dataPub.left_leg.rotate = -rotate/2
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
def walk():
T = np.linspace(0, step_periode, rate * step_periode)
for t in T:
dataPub = FullBodyIK()
if (0 <= t < dsp_1_end):
x = walkSin(t, dsp_ratio * step_periode * 2,
0 + dsp_ratio * step_periode / 4, swap_distance / 2,
swap_distance / 2)
if (t < dsp_1_end / 4):
z = 0
else:
z = walkSin(t - dsp_1_end / 4, ssp_ratio * step_periode,
0 + dsp_ratio * step_periode / 4, walk_height / 2,
walk_height / 2)
y = (t - dsp_1_end) / (ssp_ratio * step_periode) * walk_distance
dataPub.right_leg.x = x
dataPub.right_leg.z = 26.0 - z
dataPub.right_leg.y = y
dataPub.left_leg.x = -x
dataPub.left_leg.z = 26.0
if (dsp_1_end <= t < ssp_1_end):
z = walkSin(t, ssp_ratio * step_periode,
dsp_1_end + dsp_ratio * step_periode / 4,
walk_height / 2, walk_height / 2)
y = (t - dsp_1_end) / (ssp_ratio * step_periode) * walk_distance
dataPub.right_leg.x = swap_distance
dataPub.right_leg.y = y
dataPub.right_leg.z = 26.0 - z
dataPub.left_leg.x = -swap_distance
dataPub.left_leg.z = 26.0
if (ssp_1_end <= t < dsp_2_end):
x = walkSin(t, dsp_ratio * step_periode * 2,
ssp_1_end + dsp_ratio * step_periode / 4,
swap_distance / 2, swap_distance / 2)
dataPub.right_leg.x = -x
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = x
dataPub.left_leg.z = 26.0
if (dsp_2_end <= t <= ssp_2_end):
z = walkSin(t, ssp_ratio * step_periode,
dsp_2_end + dsp_ratio * step_periode / 4,
walk_height / 2, walk_height / 2)
y = (t - dsp_2_end) / (ssp_ratio * step_periode) * walk_distance
dataPub.right_leg.x = -swap_distance
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = swap_distance
dataPub.left_leg.y = y
dataPub.left_leg.z = 26.0 - z
fullBodyIK_pub.publish(dataPub)
sleep(1 / rate)
def walkSteady():
T = np.linspace(0, step_periode, calc_rate * step_periode)
for t in T: #left foot side
x = walkingSinX(t)
dataPub = FullBodyIK()
dataPub.right_leg.x = x
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = -x
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
for t in T: # right foot up
z = walkingSinZ(t)
dataPub = FullBodyIK()
dataPub.right_leg.x = side_distance
dataPub.right_leg.z = 26.0 - z
dataPub.left_leg.x = -side_distance
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
for t in T: #left foot side
x = walkingSinX(t)
dataPub = FullBodyIK()
dataPub.right_leg.x = -x
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = x
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
for t in T: # left foot up
z = walkingSinZ(t)
dataPub = FullBodyIK()
dataPub.right_leg.x = -side_distance
dataPub.right_leg.z = 26.0
dataPub.left_leg.x = side_distance
dataPub.left_leg.z = 26.0 - z
fullBodyIK_pub.publish(dataPub)
sleep(1/calc_rate)
def startPose():
dataPub = FullBodyIK()
dataPub.right_leg.z = 26.0
dataPub.left_leg.z = 26.0
fullBodyIK_pub.publish(dataPub)
sleep(1 / rate)