Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make QuaternionToTwist.RotationAxis configuration property #12

Merged
merged 2 commits into from
Nov 1, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions OpenEphys.Commutator/Defintions.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
namespace OpenEphys.Commutator
{
static class Definitions
{
internal const string ConfigurationCategory = "Configuration";
internal const string AcquisitionCategory = "Acquisition";
}
}
28 changes: 15 additions & 13 deletions OpenEphys.Commutator/QuaternionToTwist.cs
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ public class QuaternionToTwist : Combinator<Quaternion, double>
/// in the direction that the tether exits the headstage. Note that negating this vector will result in
/// negating the direction of twisting.
/// </remarks>
[Category(Definitions.ConfigurationCategory)]
[TypeConverter(typeof(NumericRecordConverter))]
[Description("The direction vector specifying the axis around which to calculate the twist.")]
public Vector3 RotationAxis { get; set; } = Vector3.UnitZ;
Expand All @@ -35,30 +36,31 @@ public class QuaternionToTwist : Combinator<Quaternion, double>
/// <returns>The sequence of twist values, in units of turns.</returns>
public override IObservable<double> Process(IObservable<Quaternion> source)
{
var rotationAxis = RotationAxis;

return Observable.Defer(() =>
{
double? previousTwist = default;
double? previousAngleAboutAxis = default;
return source.Select(rotation =>
{
// project rotation axis onto the direction axis
var direction = RotationAxis;
var rotationAxis = new Vector3(rotation.X, rotation.Y, rotation.Z);
var dotProduct = Vector3.Dot(rotationAxis, direction);
var projection = dotProduct / Vector3.Dot(direction, direction) * direction;
var twist = new Quaternion(projection, rotation.W);
twist = Quaternion.Normalize(twist);
var vectorPart = new Vector3(rotation.X, rotation.Y, rotation.Z);
var dotProduct = Vector3.Dot(vectorPart, rotationAxis);
var projection = dotProduct / Vector3.Dot(rotationAxis, rotationAxis) * rotationAxis;
var rotationAboutAxis = new Quaternion(projection, rotation.W);
rotationAboutAxis = Quaternion.Normalize(rotationAboutAxis);
if (dotProduct < 0) // account for angle-axis flipping
{
twist = -twist;
rotationAboutAxis = -rotationAboutAxis;
}

// normalize twist feedback in units of turns
var twistAngle = 2 * Math.Acos(twist.W);
var feedback = previousTwist.HasValue
? (twistAngle - previousTwist.GetValueOrDefault() + 3 * Math.PI) % (2 * Math.PI) - Math.PI
var angleAboutAxis = 2 * Math.Acos(rotationAboutAxis.W);
var twist = previousAngleAboutAxis.HasValue
? (angleAboutAxis - previousAngleAboutAxis.GetValueOrDefault() + 3 * Math.PI) % (2 * Math.PI) - Math.PI
: 0;
previousTwist = twistAngle;
return -feedback / (2 * Math.PI);
previousAngleAboutAxis = angleAboutAxis;
return -twist / (2 * Math.PI);
});
});
}
Expand Down
8 changes: 3 additions & 5 deletions OpenEphys.Commutator/SerialCommutator.cs
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,14 @@ namespace OpenEphys.Commutator
[Description("Controls an Open Ephys commutator using a serial port.")]
public class SerialCommutator : Combinator<double, string>
{
const string ConfigurationCategory = "Configuration";
const string AcquisitionCategory = "Acquisition";

readonly BehaviorSubject<bool> enabled = new(true);
readonly BehaviorSubject<bool> led = new(true);

/// <summary>
/// Gets or sets the name of the serial port.
/// </summary>
[Category(ConfigurationCategory)]
[Category(Definitions.ConfigurationCategory)]
[TypeConverter(typeof(PortNameConverter))]
[Description("The name of the serial port.")]
public string PortName { get; set; }
Expand All @@ -35,7 +33,7 @@ public class SerialCommutator : Combinator<double, string>
/// If true, the commutator will activate the motor and respond to turn commands. If false,
/// the motor driver will be deactivated and motion commands will be ignored.
/// </remarks>
[Category(AcquisitionCategory)]
[Category(Definitions.AcquisitionCategory)]
[Description("If true, the commutator will be enabled. If false, it will disable the motor and ignore motion commands.")]
public bool Enable
{
Expand All @@ -50,7 +48,7 @@ public bool Enable
/// If true, the commutator indication LED turn on. If false, the indication LED will turn
/// off.
/// </remarks>
[Category(AcquisitionCategory)]
[Category(Definitions.AcquisitionCategory)]
[Description("If true, the commutator indication LED turn on. If false, the indication LED will turn off.")]
public bool EnableLed
{
Expand Down