Creating gaits, especially walking, trotting etc. is quite a challenging task, if you do this manually. It is therefore favorable to have a kinematic model of our cat. I wanted to show you my approach to create a new gait via Inverse Kinematics, where I'm using the IKPY library (https://github.com/Phylliade/ikpy).
Inverse kinematics at a glance is that you provide a point in space and the angles of the motors are calculated. In IKPY this is achieved by creating "chains" e.g. upper_arm--lower_arm--paw. Then you decide where to place the paw in space. Below you can see an image of Nybbles Kinematic model, to give you a first impression.
For the first tests I created an alternative walking gait that follows as swing-stance-pattern of this form:
# Walking pattern (1 == swing)
# [0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1], # LL
# [0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0], # RL
# [0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0], # LA
# [1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0], # RA
The longitudinal movement (blue) and the lift (orange) is based on sine/cosine-functions, that follow the pattern phase shift as shown above.
This pattern in combination with the IK-model will lead to this movement:
I also tried this new gait on Nybble for validation and it works quite well. The lift is 1 cm in amplitude, so Nybble can now step over a small obstacle.
Further updates and the Bittle-Version can be found here: https://github.com/ger01d/kinematic-model-opencat
So here's the code for the version above:
import ikpy
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
import numpy as np
from numpy import sin, cos, pi
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.axes3d as p3
deg2rad = pi/180
# Values in cm
armLength = 5
bodyLength = 12
bodyWidth = 10
distanceFloor = 7
stepLength = 5
swingHeight = 1
leanLeft = 0 # Not working, yet
leanRight = -leanLeft # Not working, yet
leanForward = 0 # cm
left_arm = Chain(name='left_arm', links=[
URDFLink(
name="center",
translation_vector=[leanForward, 0, distanceFloor],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="shoulder",
translation_vector=[0, bodyWidth/2, leanLeft],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0.1*deg2rad, 179*deg2rad),
),
URDFLink(
name="upperArm",
translation_vector=[armLength, 0, 0 ],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(-179*deg2rad, -0.1*deg2rad),
),
URDFLink(
name="lowerArm",
translation_vector=[armLength, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
])
right_arm = Chain(name='right_arm', links=[
URDFLink(
name="center",
translation_vector=[leanForward, 0, distanceFloor],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="shoulder",
translation_vector=[0, -bodyWidth/2, leanRight],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0.1*deg2rad, 179*deg2rad),
),
URDFLink(
name="upperArm",
translation_vector=[armLength, 0, 0],
orientation=[0,0,0],
rotation=[0,1,0],
bounds=(-179*deg2rad, -1*deg2rad),
),
URDFLink(
name="lowerArm",
translation_vector=[armLength, 0, 0 ],
orientation=[0,0,0],
rotation=[0, 1, 0],
)
])
left_leg = Chain(name='left_leg', links=[
URDFLink(
name="center",
translation_vector=[leanForward, 0, distanceFloor],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="butt",
translation_vector=[-bodyLength, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="hip",
translation_vector=[0, bodyWidth/2, leanLeft],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0.1*deg2rad, 100*deg2rad),
),
URDFLink(
name="upperLeg",
translation_vector=[armLength, 0, 0 ],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(1*deg2rad, 179*deg2rad),
),
URDFLink(
name="lowerLeg",
translation_vector=[armLength, 0, 0 ],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
)
])
right_leg = Chain(name='right_leg', links=[
URDFLink(
name="center",
translation_vector=[leanForward, 0, distanceFloor],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="butt",
translation_vector=[-bodyLength, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
),
URDFLink(
name="hip",
translation_vector=[ 0, -bodyWidth/2, leanRight],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(0.1*deg2rad, 100*deg2rad),
),
URDFLink(
name="upperLeg",
translation_vector=[armLength, 0, 0 ],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
bounds=(1*deg2rad, 179*deg2rad),
),
URDFLink(
name="lowerLeg",
translation_vector=[armLength, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
)
])
def buildGait(frames):
frame = np.arange(0,frames)
swingEnd = pi/2
# longitudinalMovement
swing = -cos(2*(frame*2*pi/frames))
stance = cos(2/3*(frame*2*pi/frames-swingEnd))
swingSlice = np.less_equal(frame,swingEnd/(2*pi/frames))
stanceSlice = np.invert(swingSlice)
longitudinalMovement = np.concatenate((swing[swingSlice], stance[stanceSlice]))
longitudinalMovement = np.concatenate((longitudinalMovement,longitudinalMovement,longitudinalMovement, longitudinalMovement))
# verticalMovement
lift = sin(2*(frame*2*pi/frames))
liftSlice = swingSlice
verticalMovement = np.concatenate((lift[liftSlice], np.zeros(np.count_nonzero(stanceSlice))))
verticalMovement = np.concatenate((verticalMovement, verticalMovement, verticalMovement, verticalMovement))
return longitudinalMovement, verticalMovement
frames = 43
longitudinalMovement, verticalMovement = buildGait(frames)
# Walking pattern
# [0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1], # LL
# [0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0], # RL
# [0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0], # LA
# [1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0], # RA
# Phase shift between arms/legs, [RA, RL, LA, LL]
shiftFrame = np.round(np.array([0, pi/2, pi, 3*pi/2])/2/pi*frames)
shiftFrame = shiftFrame.astype(int)
for frame in range(0, frames):
right_arm_angles = 180/pi*right_arm.inverse_kinematics(target_position = np.array([stepLength*longitudinalMovement[frame], -bodyWidth/2 ,swingHeight*verticalMovement[frame]]))
right_arm_correction = np.array([0, -90, 90, 0])
right_arm_angles = np.round(right_arm_angles+right_arm_correction)
right_arm_angles = np.delete(right_arm_angles, np.s_[0,3], axis=0)
right_arm_angles = right_arm_angles.astype(int)
right_leg_angles = 180/pi*right_leg.inverse_kinematics(target_position = np.array([-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[1]], -bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[1]]]))
right_leg_correction = np.array([0, 0, -90, -90, 0])
right_leg_angles = np.round(right_leg_angles+right_leg_correction)
right_leg_angles = np.delete(right_leg_angles, np.s_[0,1,4], axis=0)
right_leg_angles = right_leg_angles.astype(int)
left_arm_angles = 180/pi*left_arm.inverse_kinematics(target_position = np.array([stepLength*longitudinalMovement[frame+shiftFrame[2]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[2]]]))
left_arm_correction = np.array([0, -90, 90, 0])
left_arm_angles = np.round(left_arm_angles+left_arm_correction)
left_arm_angles = np.delete(left_arm_angles, np.s_[0,3], axis=0)
left_arm_angles = left_arm_angles.astype(int)
left_leg_angles = 180/pi*left_leg.inverse_kinematics(target_position = np.array([-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[3]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[3]]]))
left_leg_correction = np.array([0, 0, -90, -90, 0])
left_leg_angles = np.round(left_leg_angles+left_leg_correction)
left_leg_angles = np.delete(left_leg_angles, np.s_[0,1,4],axis=0)
left_leg_angles = left_leg_angles.astype(int)
# Writing sequence to file
gait_sequence = np.concatenate((left_arm_angles, right_arm_angles, right_leg_angles, left_leg_angles))
print(frame," ",gait_sequence)
f = open("gait_sequence.csv", "a")
f.write("#")
np.savetxt(f, gait_sequence[[0,2,4,6,1,3,5,7]], fmt='%3.1i', delimiter=',', newline=", ")
f.write("+")
f.write("\n")
f.close()
# Create plot and image for each frame
fig = plt.figure()
ax = p3.Axes3D(fig)
ax.set_box_aspect([3, 3/2,1])
ax.set_xlim3d([-20, 10])
ax.set_xlabel('X')
ax.set_ylim3d([-10, 10])
ax.set_ylabel('Y')
ax.set_zlim3d([0.0, 10])
ax.set_zlabel('Z')
right_arm.plot(right_arm.inverse_kinematics(target_position = np.array([stepLength*longitudinalMovement[frame], -bodyWidth/2 ,swingHeight*verticalMovement[frame]])), ax)
right_leg.plot(right_leg.inverse_kinematics(target_position = np.array([-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[1]], -bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[1]]])), ax)
left_arm.plot(left_arm.inverse_kinematics(target_position = np.array([stepLength*longitudinalMovement[frame+shiftFrame[2]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[2]]])), ax)
left_leg.plot(left_leg.inverse_kinematics(target_position = np.array([-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[3]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[3]]])), ax)
figureName = "Nybble_" + str(frame)
plt.savefig(figureName)
#plt.show()
Nice work Gero, I am very new here, I just started to know and learn Arduino a couple of months ago, and I found this project and your kinematic tool, very interesting and useful. I just noticed that in your code, you placed four times of longitudinalMovement and verticalMovement into list, as the gait pattern for each leg/arm only starts from 0,1/4,/1/2,3/4 frames, to put two times of the original longitudinalMovement\verticalMovement is enough.
Jason
aijnec@163.com