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