void updateFeetPositions(float p_phi)
{
for (int i = 0; i < m_foot.Length; i++)
{
bool inStance = m_stepCycles[i].isInStance(p_phi);
//
if (!inStance)
{
float swingPhi = m_stepCycles[i].getSwingPhase(p_phi);
// The height offset, ie. the "lift" that the foot makes between stepping points.
Vector3 heightOffset = new Vector3(0.0f, m_stepHeightTraj.getValAt(swingPhi), 0.0f);
float flip = (i * 2.0f) - 1.0f;
Vector3 wpos = Vector3.Lerp(m_liftPos[i],
transform.position + new Vector3(flip * m_stepLength.x, 0.0f, m_stepLength.y * 0.5f),
swingPhi);
wpos = new Vector3(wpos.x, 0.0f, wpos.z);
m_foot[i].position = wpos + heightOffset;
}
else
{
m_liftPos[i] = m_foot[i].position;
Debug.DrawLine(m_foot[i].position, m_foot[i].position + Vector3.up, Color.magenta - new Color(0.3f, 0.3f, 0.3f, 0.0f), 10.0f);
}
Color debugColor = Color.red;
if (i == 1)
{
debugColor = Color.green;
}
Debug.DrawLine(m_oldFootPos[i], m_foot[i].position, debugColor, 30.0f);
m_oldFootPos[i] = m_foot[i].position;
}
}
public void calculateFh(float p_phi, float p_currentH, float p_dt, Vector3 p_up)
{
float hLF = m_tuneLFHeightTraj.getValAt(p_phi);
m_Fh = p_up * m_heightForceCalc.drive(hLF - p_currentH, p_dt);
int i = 0;
}
public void calculateFsw(int p_legId, float p_phi)
{
float swing = m_tuneStepCycles[p_legId].getSwingPhase(p_phi);
float Kft = m_tunePropGainFootTrackingKft.getValAt(swing);
m_FootTrackingSpringDamper.m_Kp = Kft;
Vector3 diff = m_feet[p_legId].transform.position - m_footStrikePlacement[p_legId];
float error = Vector3.Magnitude(diff);
m_Fsw[p_legId] = -diff.normalized * m_FootTrackingSpringDamper.drive(error, Time.deltaTime);
Debug.DrawLine(m_feet[p_legId].transform.position, m_feet[p_legId].transform.position + m_Fsw[p_legId], Color.yellow);
}
public void updateReferenceFeetPositions(float p_phi, float p_t, Vector3 p_goalVelocity)
{
m_optimalProgress = p_goalVelocity.z * p_t;
for (int i = 0; i < m_referenceFootPos.Length; i++)
{
bool inStance = m_tuneStepCycles[i].isInStance(p_phi);
//
if (!inStance)
{
float swingPhi = m_tuneStepCycles[i].getSwingPhase(p_phi);
// The height offset, ie. the "lift" that the foot makes between stepping points.
Vector3 heightOffset = new Vector3(0.0f, m_tuneStepHeightTraj.getValAt(swingPhi), 0.0f);
float flip = (i * 2.0f) - 1.0f;
float legOptimalProgress = m_optimalProgress - m_referenceLiftPos[i].z;
Vector3 offset = transform.position.x * Vector3.right + m_referenceLiftPos[i].z * Vector3.forward + Vector3.forward * legOptimalProgress;
Vector3 wpos = Vector3.Lerp(m_referenceLiftPos[i],
offset + new Vector3(flip * m_tuneStepLength.x, 0.0f, m_tuneStepLength.y * 0.5f),
swingPhi);
wpos = new Vector3(wpos.x, 0.0f, wpos.z);
m_referenceFootPos[i] = wpos + heightOffset;
}
else
{
m_referenceLiftPos[i] = m_referenceFootPos[i];
Debug.DrawLine(m_referenceFootPos[i] + Vector3.forward * m_startPos.z, m_referenceFootPos[i] + Vector3.up + Vector3.forward * m_startPos.z, Color.magenta - new Color(0.3f, 0.3f, 0.3f, 0.0f), 10.0f);
}
Color debugColor = Color.red;
if (i == 1)
{
debugColor = Color.green;
}
Debug.DrawLine(m_referenceOldFootPos[i] + Vector3.forward * m_startPos.z, m_referenceFootPos[i] + Vector3.forward * m_startPos.z, debugColor, 10.0f);
m_referenceOldFootPos[i] = m_referenceFootPos[i];
}
}
// Get the phase value in the foot transition based on
// swing phase. Note the phi variable here is the fraction
// of the swing phase!
private float getFootTransitionPhase(float p_swingPhi)
{
return(m_tuneFootTransitionEase.getValAt(p_swingPhi));
}