/** * this method gets called every time the controller transitions to a new state */ virtual public void conTransitionPlan() { lowLCon.updateSwingAndStanceReferences(); lowLCon.updateDAndV(); standFootStartPos = lowLCon.getStanceFoot().getWorldBottomPosition(); swingFootStartPos = lowLCon.getSwingFoot().getWorldBottomPosition(); //now prepare the step information for the following step: lowLCon.swingFootHeightTrajectory.clear(); //lowLCon.standFootHeightTrajectory.clear(); lowLCon.swingFootCoronalTrajectory.clear(); lowLCon.standFootCoronalTrajectory.clear(); lowLCon.swingFootSagittalTrajectory.clear(); lowLCon.standFootSagittalTrajectory.clear(); stepRaycastHits.Clear(); if (lowLCon.getIsComputeIPStep()) { lowLCon.swingFootHeightTrajectory.addKnot(0, 0); lowLCon.swingFootHeightTrajectory.addKnot(0.5f, lowLCon.getStepHeight()); lowLCon.swingFootHeightTrajectory.addKnot(1, 0); lowLCon.swingFootSagittalTrajectory.addKnot(0, 0); lowLCon.swingFootSagittalTrajectory.addKnot(1, 0); lowLCon.swingFootCoronalTrajectory.addKnot(0, 0); lowLCon.swingFootCoronalTrajectory.addKnot(1, 0); } else { if (lowLCon.getIsAddFootJoint()) { lowLCon.getSwingFoot().removeFixedPositionJoint(); lowLCon.getStanceFoot().removeFixedPositionJoint(); } lowLCon.swingFootHeightTrajectory.addKnot(0, 0); lowLCon.swingFootHeightTrajectory.addKnot(0.5f, lowLCon.getStepHeight()); lowLCon.swingFootHeightTrajectory.addKnot(1, 0); float w = lowLCon.getLocalLeftAxisSign() * sagittalStepWidth; w = (lowLCon.getStance() == SimGlobals.LEFT_STANCE)?w:-w; lowLCon.swingFootSagittalTrajectory.addKnot(0, 0 + w); lowLCon.swingFootSagittalTrajectory.addKnot(1, lowLCon.getSwingStepLength() + w); lowLCon.standFootSagittalTrajectory.addKnot(0, 0 - w); lowLCon.standFootSagittalTrajectory.addKnot(1, lowLCon.getStandStepLength() - w); w = lowLCon.getLocalLeftAxisSign() * coronalStepWidth; w = (lowLCon.getStance() == SimGlobals.LEFT_STANCE)?w:-w; lowLCon.swingFootCoronalTrajectory.addKnot(0, 0 + w); lowLCon.swingFootCoronalTrajectory.addKnot(1, 0 + w); //lowLCon.standFootCoronalTrajectory.addKnot(0, 0); //lowLCon.standFootCoronalTrajectory.addKnot(1, 0); //float contactPosDis = (lowLCon.getStanceFoot().getWorldBottomPosition() - lowLCon.swingFootBasePosition).magnitude; //if(Vector2.Dot(lowLCon.standFootBaseNormal, -1*Physics.gravity.normalized) > 0.7f/* && contactPosDis < 0.5f*/){ // lowLCon.getStanceFoot().addFixedPositionJoint(lowLCon.swingFootBasePosition, 800); //} //lowLCon.standFootHeightTrajectory.addKnot(0, lowLCon.getLegLength()); //lowLCon.standFootHeightTrajectory.addKnot(1, lowLCon.getLegLength()); } }