private Vector3 GetCurrentLegPosition(LegConfiguration legConfig)
{
MotorGoalPositions positions;
lock (_driver.SyncLock)
{
float coxa = _driver.GetPresentPositionInDegrees(legConfig.CoxaId);
float femur = _driver.GetPresentPositionInDegrees(legConfig.FemurId);
float tibia = _driver.GetPresentPositionInDegrees(legConfig.TibiaId);
positions = new MotorGoalPositions(coxa, femur, tibia);
}
return(CalculateFkForLeg(positions, legConfig));
}
private static Vector3 CalculateFkForLeg(MotorGoalPositions currentPsoitions, LegConfiguration legConfig)
{
float femurAngle = Math.Abs(currentPsoitions.Femur - Math.Abs(legConfig.FemurCorrection));
float tibiaAngle = Math.Abs(currentPsoitions.Tibia - Math.Abs(legConfig.TibiaCorrection));
float coxaAngle = 150 - currentPsoitions.Coxa - legConfig.AngleOffset;
float baseX = (float)Math.Cos(coxaAngle.DegreeToRad());
float baseY = (float)Math.Sin(coxaAngle.DegreeToRad());
Vector3 coxaVector = new Vector3(baseX, baseY, 0) * CoxaLength;
float femurX = (float)Math.Sin((femurAngle - 90).DegreeToRad()) * FemurLength;
float femurY = (float)Math.Cos((femurAngle - 90).DegreeToRad()) * FemurLength;
Vector3 femurVector = new Vector3(baseX * femurY, baseY * femurY, femurX);
// to calculate tibia we need angle between tibia and a vertical line
// we get this by calculating the angles formed by a horizontal line from femur, femur and part of fibia by knowing that the sum of angles is 180
// than we just remove this from teh tibia andgle and done
float angleForTibiaVector = tibiaAngle - (180 - 90 - (femurAngle - 90));
float tibiaX = (float)Math.Sin(angleForTibiaVector.DegreeToRad()) * TibiaLength;
float tibiaY = (float)Math.Cos(angleForTibiaVector.DegreeToRad()) * TibiaLength;
Vector3 tibiaVector = new Vector3(baseX * tibiaX, baseY * tibiaX, -tibiaY);
return(legConfig.CoxaPosition + coxaVector + femurVector + tibiaVector);
}
