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); }