private static MotorGoalPositions CalculateIkForLeg(Vector3 target, LegConfiguration legConfig)
{
Vector3 relativeVector = target - legConfig.CoxaPosition;
float targetAngle = (float)(Math.Atan2(relativeVector.Y, relativeVector.X).RadToDegree() + legConfig.AngleOffset);
float horizontalDistanceToTarget = (float)Math.Sqrt(Math.Pow(relativeVector.X, 2) + Math.Pow(relativeVector.Y, 2));
float horizontalDistanceWithoutCoxa = horizontalDistanceToTarget - CoxaLength;
float absoluteDistanceToTargetWithoutCoxa = (float)Math.Sqrt(Math.Pow(horizontalDistanceWithoutCoxa, 2) + Math.Pow(relativeVector.Z, 2));
// use sss triangle solution to calculate angles
// use law of cosinus to get angles in two corners
float angleByTibia = (float)GetAngleByA(absoluteDistanceToTargetWithoutCoxa, FemurLength, TibiaLength);
float angleByFemur = (float)GetAngleByA(TibiaLength, FemurLength, absoluteDistanceToTargetWithoutCoxa);
// we have angles of the SSS triangle. now we need angle for the servos
float groundToTargetAngleSize = (float)Math.Atan2(horizontalDistanceWithoutCoxa, -relativeVector.Z).RadToDegree();
if (targetAngle >= 90 || targetAngle <= -90)
{
// target is behind me
// can still happen if target is right bellow me
throw new NotSupportedException($"Target angle is {targetAngle}");
}
float femurAngle = angleByFemur + groundToTargetAngleSize;
// these angles need to be converted to the dynamixel angles
// in other words horizon is 150 for the dynamixel angles so we need to recalcualate them
float correctedFemur = Math.Abs(legConfig.FemurCorrection + femurAngle);
float correctedTibia = Math.Abs(legConfig.TibiaCorrection + angleByTibia);
float correctedCoxa = 150f + targetAngle;
return(new MotorGoalPositions(correctedCoxa, correctedFemur, correctedTibia));
}
private void MoveLeg(Vector3 target, LegConfiguration legConfig)
{
var legGoalPositions = CalculateIkForLeg(target, legConfig);
lock (_driver.SyncLock)
{
_driver.SetGoalPositionInDegrees(legConfig.CoxaId, legGoalPositions.Coxa);
_driver.SetGoalPositionInDegrees(legConfig.FemurId, legGoalPositions.Femur);
_driver.SetGoalPositionInDegrees(legConfig.TibiaId, legGoalPositions.Tibia);
}
}
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);
}
