Пример #1
0
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
Пример #2
0
def findFootPoseStaticMode(indLeg, comPos, nextLeft):
    """Compute the foot positions for the swing legs when the character starts to slow down and stay static
    
    """
    tri = tr.createNewTriTemplate(comPos, nextLeft)
    return tri[int(indLeg/2)]