/// <summary>
/// 肘座標優先モードで腕のIKを解き、順次サーボ角度を設定していく
/// </summary>
private void ApplyIK_ElbowFirst()
{
// これ以下に肩に近づきすぎた肘目標点は無視する閾値
const float sqrMinDistance = 0.0001f; // [m^2]
float sign = (isRightSide ? -1f : 1f); // 左右の腕による方向入れ替え用
Vector3 x0 = shoulderPitch.transform.position; // UpperArmJointの座標
Quaternion invBaseRotation = Quaternion.Inverse(baseTransform.rotation);
Vector3 x0e = invBaseRotation * (elbowTarget.position - x0); // x0から肘目標点までのベクトル
// 閾値より肘目標点が肩に近すぎる場合、手首優先モードのIKにする
if (x0e.sqrMagnitude < sqrMinDistance)
{
ApplyIK_HandFirst();
return;
}
float a0 = Mathf.Atan2(sign * x0e.z, -x0e.y) * Mathf.Rad2Deg; // 肩のX軸周り回転[deg]
shoulderPitch.SetServoValue(a0);
Vector3 x1 = upperArmRoll.transform.position; // 上腕始点の座標
Quaternion invShoulderRotation = Quaternion.Inverse(shoulderPitch.normalizedRotation);
Vector3 x1e = invShoulderRotation * (elbowTarget.position - x1);
float a1 = Mathf.Atan2(sign * x1e.y, sign * -x1e.x) * Mathf.Rad2Deg; // 上腕のZ軸周り回転[deg]
upperArmRoll.SetServoValue(a1);
Vector3 x3 = lowerArmRoll.transform.position; // 肘座標
Quaternion invUpperArmRotation = Quaternion.Inverse(upperArmRoll.normalizedRotation);
Vector3 x3h = invUpperArmRotation * (handTarget.position - x3); // 肘から手首目標点へ向かうベクトル
// 閾値より手首目標点が肘に近すぎる場合は、肘から先は処理しない
if (x3h.sqrMagnitude < sqrMinDistance)
{
return;
}
float a2 = Mathf.Atan2(-x3h.x, -x3h.z) * Mathf.Rad2Deg; // 上腕のX軸周り回転[deg]
upperArmPitch.SetServoValue(a2);
Vector3 x4 = handPitch.transform.position;
float l3h = Mathf.Sqrt(x3h.z * x3h.z + x3h.x * x3h.x);
float a3 = Mathf.Atan2(sign * -l3h, x3h.y) * Mathf.Rad2Deg; // 前腕のZ軸周り回転[deg]
lowerArmRoll.SetServoValue(a3);
}
/// <summary>
/// Quaternion で渡された姿勢のうち、X, Y, Z 軸いずれか周り成分を抽出してサーボ角に反映します
/// </summary>
/// <param name="rot">目標姿勢</param>
/// <param name="joint">指定サーボ</param>
/// <returns>回転させた軸成分を除いた残りの回転 Quaternion</returns>
internal Quaternion ApplyPartialRotation(Quaternion rot, ModelJoint joint)
{
Quaternion q = rot;
Vector3 axis = Vector3.right;
float direction = (joint.isInverse ? -1f : 1f); // 逆転なら-1
switch (joint.targetAxis)
{
case ModelJoint.Axis.X:
q.y = q.z = 0;
if (q.x < 0)
{
direction = -direction;
}
axis = Vector3.right;
break;
case ModelJoint.Axis.Y:
q.x = q.z = 0;
if (q.y < 0)
{
direction = -direction;
}
axis = Vector3.up;
break;
case ModelJoint.Axis.Z:
q.x = q.y = 0;
if (q.z < 0)
{
direction = -direction;
}
axis = Vector3.forward;
break;
}
if (q.w == 0 && q.x == 0 && q.y == 0 && q.z == 0)
{
Debug.Log("Joint: " + joint.name + " rotation N/A");
q = Quaternion.identity;
}
q.Normalize();
float angle = Mathf.Acos(q.w) * 2.0f * Mathf.Rad2Deg * direction;
var actualAngle = joint.SetServoValue(angle);
return(rot * Quaternion.Inverse(q));
}
public void ApplyIK()
{
if (!footTarget)
{
return;
}
Quaternion invBaseRotation = Quaternion.Inverse(baseTransform.rotation);
Quaternion targetRotation = invBaseRotation * footTarget.rotation;
Vector3 rt = targetRotation.eulerAngles;
float a0 = rt.y;
upperLegYaw.SetServoValue(a0);
}
public void ApplyIK()
{
if (!headTarget)
{
return;
}
Quaternion invBaseRotation = Quaternion.Inverse(baseTransform.rotation);
Vector3 gazeVec = invBaseRotation * (headTarget.position - headRoll.transform.position);
Quaternion lookAtRot = Quaternion.LookRotation(gazeVec);
Vector3 eular = lookAtRot.eulerAngles;
float yaw = eular.y - (eular.y > 180f ? 360f : 0f);
float pitch = eular.x - (eular.x > 180f ? 360f : 0f);
neckYaw.SetServoValue(yaw);
headPitch.SetServoValue(pitch);
}
/// <summary>
/// 視線を指定座標に向ける（LookAt）パターンでIKを適用
/// </summary>
private void ApplyIK_Gaze()
{
Quaternion invBaseRotation = Quaternion.Inverse(baseTransform.rotation);
Vector3 gazeVec = invBaseRotation * (headBaseTarget.position - headRoll.transform.position);
Quaternion lookAtRot = Quaternion.LookRotation(gazeVec);
Vector3 eular = lookAtRot.eulerAngles;
float yaw = eular.y - (eular.y > 180f ? 360f : 0f);
float pitch = eular.x - (eular.x > 180f ? 360f : 0f);
yaw = neckYaw.SetServoValue(yaw); // 戻り値は制限後の角度
pitch = headPitch.SetServoValue(pitch); // 戻り値は制限後の角度
// ロール（萌え軸）の向きを2点の目標から求める
Quaternion rot = Quaternion.AngleAxis(-yaw, Vector3.up) * Quaternion.AngleAxis(-pitch, Vector3.right);
Vector3 upVec = rot * invBaseRotation * (headOrientationTarget.position - headBaseTarget.position);
float roll = 0f;
if (!Mathf.Approximately(upVec.x, 0f) || !Mathf.Approximately(upVec.y, 0f))
{
roll = -Mathf.Atan2(upVec.x, upVec.y) * Mathf.Rad2Deg;
}
headRoll.SetServoValue(roll);
}
public void ApplyIK()
{
if (!footTarget)
{
return;
}
// これ以下に元位置に近づきすぎた目標は無視する閾値
const float sqrMinDistance = 0.000025f; // [m^2]
const float a1Limit = 0.1f; // [deg]
float sign = (isRightSide ? -1f : 1f); // 左右の腕による方向入れ替え用
Quaternion invBaseRotation = Quaternion.Inverse(baseTransform.rotation);
Quaternion targetRotation = invBaseRotation * footTarget.rotation;
// Unityでは Z,Y,X の順番なので、ロボットとは一致しないはず
Vector3 rt = targetRotation.eulerAngles;
float yaw = -rt.y;
float pitch = -rt.x;
float roll = -rt.z;
float a0, a1, a2, a3, a4, a5;
a0 = yaw;
Vector3 dx = invBaseRotation * (upperLegYaw.transform.position + (baseTransform.rotation * xo06) - footTarget.position); // 初期姿勢時足元に対する足目標の変位
dx = Quaternion.AngleAxis(-a0, Vector3.up) * dx; // ヨーがある場合は目標変位も座標変換
dx.z = 0f; // ひとまず Z は無視してのIKを実装
if (dx.sqrMagnitude < sqrMinDistance)
{
// 目標が直立姿勢に近ければ、指令値はゼロとする
a1 = a2 = a3 = a4 = a5 = 0f;
}
else
{
a1 = Mathf.Atan2(dx.x, -xo15.y + dx.y) * Mathf.Rad2Deg;
a5 = -a1;
//float len15 = dx.x / Mathf.Sin(a1 * Mathf.Deg2Rad); // 屈伸した状態での x1-x5 間長さ // sinだと a1==0 のとき失敗する
float len15 = (-xo15.y + dx.y) / Mathf.Cos(a1 * Mathf.Deg2Rad); // 屈伸した状態での x1-x5 間長さ
float cosa2 = (len15 + xo12.y + xo45.y) / (-xo23.y - xo34.y);
if (cosa2 >= 1f)
{
// 可動範囲以上に伸ばされそうな場合
a2 = 0f;
}
else if (cosa2 <= 0f)
{
// 完全に屈曲よりもさらに下げられそうな場合
a2 = sign * 90f;
}
else
{
// 屈伸の形に収まりそうな場合
a2 = sign * Mathf.Acos(cosa2) * Mathf.Rad2Deg;
}
a4 = a2; // ひとまず、 a4 は a2 と同じとなる動作のみ可
a3 = a2 + a4;
}
upperLegYaw.SetServoValue(a0);
upperLegRoll.SetServoValue(a1);
upperLegPitch.SetServoValue(a2);
kneePitch.SetServoValue(a3);
anklePitch.SetServoValue(a4);
footRoll.SetServoValue(a5);
}