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