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