void GetSpidarPose()
{
prevPose = pose;
SpidarVector pos, vel, avel;
SpidarQuaternion rot;
pos = vel = avel = SpidarVector.zero;
rot = SpidarQuaternion.identity;
if (spidar != null)
{
spidar.GetPose(out pos, out rot, out vel, out avel);
}
rawPose.position = Converter.ScaleUp(Converter.Convert(pos), PositionScale);
rawPose.rotation = Converter.ScaleUp(Converter.Convert(rot), RotationScale);
rawPose.velocity = Converter.ScaleUp(Converter.Convert(vel), PositionScale);
rawPose.angularVelocity = Converter.ScaleUp(Converter.Convert(avel), RotationScale);
if (clutchEngaged)
{
pose.position = RotationOffset * (rawPose.position + clutchedPositionOffset) + PositionOffset;
pose.rotation = RotationOffset * rawPose.rotation;
pose.velocity = RotationOffset * rawPose.velocity;
pose.angularVelocity = RotationOffset * rawPose.angularVelocity;
}
else
{
pose.position = RotationOffset * clutchedPosition + PositionOffset;
pose.rotation = RotationOffset * clutchedRotation;
pose.velocity = Vector3.zero;
pose.angularVelocity = Vector3.zero;
}
transform.position = pose.position;
transform.rotation = pose.rotation;
}
/// <summary>
/// グリップの姿勢を取得する．
/// </summary>
/// <param name="position">
/// グリップの位置[m]
/// </param>
/// <param name="rotation">
/// グリップの回転（四元数）
/// </param>
/// <param name="velocity">
/// グリップの速度[m/s]
/// </param>
/// <param name="angularVelocity">
/// グリップの角速度[rad/s]
/// </param>
public void GetPose(out Vector3 position, out Quaternion rotation,
out Vector3 velocity, out Vector3 angularVelocity)
{
Spidar.GetPose(this.SerialNumber, out position, out rotation,
out velocity, out angularVelocity);
}
