IEnumerator<ITask> OnApplyJointParametersHandler(OnApplyJointParameters onApply)
{
arm.SetJointTargetPoseRequest req = new arm.SetJointTargetPoseRequest();
req.JointName = onApply.JointName;
AxisAngle aa = new AxisAngle(
new Vector3(1, 0, 0),
(float)(Math.PI * 2 * ((double)onApply.Angle / 360)));
req.TargetOrientation = aa;
_articulatedArmPort.SetJointTargetPose(req);
yield break;
}
/// <summary>
/// Convert a Quaternion to an angle around the y axis from 0 to 2 Pi
/// </summary>
public double ConvertQuaternion(Quaternion q)
{
AxisAngle a = new AxisAngle();
a = Quaternion.ToAxisAngle(q);
if (float.IsNaN(a.Angle))
return 0;
else if (Math.Sign(a.Axis.Y) < 0)
return 2.0 * Math.PI - a.Angle;
return a.Angle;
}
public Axis(string name, AxisAngle angle)
{
this.name = name;
this.angle = angle;
}
