def testOsci(self):
print("start")
y0 = [1, 0]
timestep = 10
odeArgs = (5, -1, 1, 10)
yo = oscillator.oscillatorUnit(y0, timestep, DEBUG=True, odeArgs=odeArgs)
plt.plot(yo[:, 0], yo[:, 1])
plt.show()
def computeForce(indAnt, legJointAngleArray, legJointVelArray, timestep, optParam=None, flutParam=None):
"""Compute the force on each leg
"""
forces = []
torqueArray = []
# for each leg
indLeg = 0
indJoint = 0
for legJointAngle, legJointVel in zip(legJointAngleArray, legJointVelArray):
indJoint = 0
angles = []
torques = []
# if it is the swing mode
if np.array_equal(legJointAngle, [0, 0, 0]):
forces.append(np.matrix([0, 0, 0]).T)
torqueArray.append([0]*3)
continue
# for each joint
for angle, vel in zip(legJointAngle, legJointVel):
# clamp the rotational velocity
if vel > 5 :
vel = 5
elif vel < -5:
vel = -5
# de-transform the joint angles with operators
indDOF = indJoint + indLeg*3
q_o, dq_o = ol.operators(angle, vel, indDOF, optParam=optParam, forward=False, flutParam=flutParam)
# read the sigma parameter
sigma = up.readSigma(indAnt)
odeArgs = (5, sigma, 1, 10)
# call the oscillator, do the integration
y_o = ol.oscillatorUnit([q_o, dq_o], timestep=timestep, odeArgs=odeArgs)
# transform the joint angles with operators
nextAngle, nextVel = ol.operators(y_o[-1][0], y_o[-1][1], indDOF, optParam=optParam, forward=True, flutParam=flutParam)
angles.append(nextAngle)
# compute the torque
tq = pdservo(nextAngle, angle, nextVel, indDOF, optParam=optParam)
torques.append(tq)
indJoint = indJoint + 1
# convert the joint torques into contact force
torqueArray.append(torques)
jac = jacobian(angles[0], angles[1], angles[2], indLeg)
force = jac*np.matrix(torques).transpose()
if np.isnan(force).any():
force = np.array([0, 0, 0])
forces.append(force)
indLeg = indLeg + 1
totalForce = np.sum(np.array(forces), 0)
return totalForce, forces, torqueArray
