/// <summary>首の角度を設定 FIXME: 現状はヨー角のみ</summary> private static void SetHead(RobotJointAngles robot, Body body) { var neck = QuartanionFactory.FromVector4(body.JointOrientations[JointType.Neck].Orientation); var spine = QuartanionFactory.FromVector4(body.JointOrientations[JointType.SpineShoulder].Orientation); var neckZ = Quartanion.UnitZ.RotateBy(neck); //正規直交基底に使う var spineX = Quartanion.UnitX.RotateBy(spine); var spineY = Quartanion.UnitY.RotateBy(spine); var spineZ = Quartanion.UnitZ.RotateBy(spine); var neckZfromSpine = new Quartanion { X = neckZ.Product(spineX), Y = neckZ.Product(spineY), Z = neckZ.Product(spineZ) }; robot.HeadYaw = (float)Math.Asin(neckZfromSpine.X); }
/// <summary>右肩から右肘にかけての関節角度指示値を設定</summary> private static void SetRArmAngles(RobotJointAngles robot, Body body) { #region 諸元の準備 //右肩の基準座標系を取得 var rShoulder = GetOrientation(body, JointType.ShoulderRight); var shoulderX = Quartanion.UnitX.RotateBy(rShoulder); var shoulderY = Quartanion.UnitY.RotateBy(rShoulder); var shoulderZ = Quartanion.UnitZ.RotateBy(rShoulder); //ヒジの位置とヒジ回転軸ベクトルを取得 var elbowOrientation = GetOrientation(body, JointType.ElbowRight); var elbowZ = Quartanion.UnitZ.RotateBy(elbowOrientation); var elbowY = Quartanion.UnitY.RotateBy(elbowOrientation); //ヒジ-手首方向の単位ベクトルを取得 var wristY = Quartanion.UnitY.RotateBy(GetOrientation(body, JointType.WristRight)); //肩座標を基準にするため成分を再取得 var elbowZFromShoulder = new Quartanion { X = elbowZ.Product(shoulderX), Y = elbowZ.Product(shoulderY), Z = elbowZ.Product(shoulderZ) }; var elbowPosFromShoulder = new Quartanion { X = elbowY.Product(shoulderX), Y = elbowY.Product(shoulderY), Z = elbowY.Product(shoulderZ) }; #endregion #region 換計算 //NOTE: 変換計算の導出は手元のノートで手計算により事前に行われている。ソースだけ見ても分かりづらい //肩から見たヒジの位置情報をもとにShoulderRollおよびShoulderPitchを一意に特定 float rShoulderRoll = (float)Math.Asin(elbowPosFromShoulder.Y); float rShoulderPitch = 0.0f; if (rShoulderRoll > -1.5f && rShoulderRoll < 1.5f) { //算術的チェック: X / Cos() が1越えてるとAsin関数がNaNを吐くのを防止 if (Math.Abs(elbowPosFromShoulder.X) > Math.Abs(Math.Cos(rShoulderRoll))) { rShoulderPitch = 0.5f * 3.14f; } else { rShoulderPitch = (float)Math.Asin(elbowPosFromShoulder.X / Math.Cos(rShoulderRoll)); //追加処理: Pitchは360度分の自由度があるのだがAsinだと180度しか分解能ないので //Z座標の正負情報を使って360度に対応させる //具体的にはZ < 0 のときピッチは[-180, -90]か[90, 180]の範囲に入る if (elbowPosFromShoulder.Z < 0) { if (rShoulderPitch > 0) { rShoulderPitch = (float)Math.PI - rShoulderPitch; } else { rShoulderPitch = (float)(-Math.PI) - rShoulderPitch; } } } } //ヒジのZベクトルつまりヒジの回転軸ベクトルの向き(と上のShoulderPitch)を用いてElbowYawを特定 //このnoRollRElbowはElbowYaw=0の場合のヒジZベクトルの向きである(導出は手計算) var noRollRElbow = new Quartanion { X = (float)Math.Cos(rShoulderPitch), Z = -(float)Math.Sin(rShoulderPitch) }; //noRollRElbowと実際のヒジZベクトルのズレはElbowYaw回転によって説明される、というノリで計算。 //外積を用いているのはAcos関数だけだと回転方向が定まらない(どっち回転でもプラス扱いになる)ため //float rElbowYaw = (float)( // Math.Acos(noRollRElbow.Product(elbowZFromShoulder)) * // Math.Sign(noRollRElbow.Cross(elbowZFromShoulder).Product(elbowPosFromShoulder)) // ); //修正:たぶんこっちの方が計算安定する float rElbowYaw = (float)Math.Asin( noRollRElbow.Cross(elbowZFromShoulder).Product(elbowPosFromShoulder) ); //ヒジの曲がり具合を取得: 単に内積とって角度差を見ればOK float cos = wristY.Product(elbowY); float rElbowRoll = (float)Math.Acos(cos); #endregion //HACK: 正負の調整はPepperとKinectで回転方向の取り方が違うことに由来する。 robot.RShoulderPitch = -rShoulderPitch; robot.RShoulderRoll = -rShoulderRoll; robot.RElbowYaw = rElbowYaw; robot.RElbowRoll = rElbowRoll; }