def firstStep(indAnt, curve, vel, isFlat=True, ground=None):
"""Initialise the simulation
This function initialises the settings at the beginning of the simulation.
"""
speed = np.linalg.norm(vel)
curveStartPoint = up.getPosCurvePoint(curve, 0.01)
tan = up.getTanCurvePoint(curve, 0.01)
yAngle = up.getAngle(tan)
comTrajList = []
comTrajList.append(curveStartPoint)
triList = []
newTri = tr.createNewTriTemplate(curveStartPoint, True, yRot=yAngle)
newTri = tr.plotTri(newTri, isFlat=isFlat, ground=ground)
triList.append(newTri)
strideLen = tr.triangleSpeed(speed)
newTri = tr.createNewTriTemplate(curveStartPoint + strideLen/2*tan, False, yRot=yAngle)
newTri = tr.plotTri(newTri, isFlat=isFlat, ground=ground)
triList.append(newTri)
diffCOM = np.average(triList[1], 0) - np.average(triList[0], 0)
curTriRight = triList[1] - diffCOM
up.animateBodyLeg(indAnt, curveStartPoint, up.mergeTwoTripod(triList[0], curTriRight))
jointLTList = []
jointRTList = []
jointLT, jointRT = fe.solveJointAngles(curveStartPoint + [0, 0, -0.1], triList[0], triList[1], True)
jointLTList.append(jointLT)
jointRTList.append(jointRT)
return comTrajList, triList, jointLTList, jointRTList
def triangleCurve(triShape, lastTriCOM, dirVec, nextCurvePoint, speed):
"""Compute the location of the triangle given the trajectory path
"""
strideLen = triangleSpeed(speed)
dirVec[1] = 0
normDirVec = np.sqrt(sum(dirVec**2))
tri_tangent_com = lastTriCOM + dirVec/normDirVec*strideLen
tri_curve_com = nextCurvePoint[0:3]
angle_tan = up.getAngle(dirVec)
angle_cur = up.getAngle(nextCurvePoint[3:6])
triVT = rotateTri(triShape, angle_tan)
triVC = rotateTri(triShape, angle_cur)
tri_tangent = triVT + tri_tangent_com
tri_curve = triVC + tri_curve_com
curvature = nextCurvePoint[-1]
weight = np.exp(-curvature*speed*speed/9.8)
newTri = (1 - weight) * tri_tangent + weight * tri_curve
return newTri
