public Angle[] Reset(ArmCursor orig) { Angle[] a = poseCalculator.PoseToAngles(orig.Pos, orig.Dir); for (int i = 0; i < a.Length; i++) { servos[i].Set(a[i]); } return(a); }
public void ptaTPose() { Quaternion dir = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Math.PI / 2f); Vector3 pos = Vector3.Zero; pos.X = config.SHOULDER + config.FOREARM; pos.Z = config.UPPER_ARM - config.HAND; Angle[] ans = calc.PoseToAngles(pos, dir); Angle[] expected = new Angle[] { 0, PI / 2, PI / 2, 0, -PI / 2, 0 }; testAngleSet(expected, ans); }