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], # RAThe 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()
@Gero very good attempt. I was interested to see the feet trajectory of your work, thus I mapped the horizontal longitudinal movement and the vertical movement. As expected, it is a simple sine curve with flat base:
Reversely, based on your derived gait numbers, I applied "forward kinematics" to verify that I can also work backwards to arrive at the same trajectory without using your original sine / cosine equations:
The reason I'm keen to dig deeper, is that I noticed in the original Bittle InstinctBittle.h, the gait numbers provided tend to result in Bittle dragging its feet when it walks (thus @Rongzhong Li's recommendation of running Bittle without socks, which doesn't make sense in normal circumstances). The feet doesn't seem to "push" Bittle up when it touches the ground, resulting in unstable Bittle movement. As Li did not disclose the original equations of how the numbers in InstinctBittle.h were derived, by applying the same forward kinematics approach, I mapped out the original Bittle's walk, run, crawl and trot feet trajectory:
Similar to your work, Li's given gait numbers all have a "flat" trajectory base, i.e. Bittle does not "push" itself up when the feet touches the ground, resulting in unstable forward-moving gestures. In a separate quadruped project, I came across this paper of a more sophisticated feet trajectory:
As one can see, during the stance phase, the feet trajectory is slightly convex downwards, which has a number of beneficiaries according to the authors. As I am still awaiting a few parts coming from eBay on my other project, I would work on applying this revised trajectory on Bittle, and see if it would result in better movement.