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