-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #2 from open-ephys/modular-commutator
Add commutator control based on swing-twist decomposition
- Loading branch information
Showing
4 changed files
with
233 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,113 @@ | ||
| <?xml version="1.0" encoding="utf-8"?> | ||
| <WorkflowBuilder Version="2.8.5" | ||
| xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" | ||
| xmlns:rx="clr-namespace:Bonsai.Reactive;assembly=Bonsai.Core" | ||
| xmlns:commutator="clr-namespace:OpenEphys.Commutator;assembly=OpenEphys.Commutator" | ||
| xmlns:port="clr-namespace:Bonsai.IO.Ports;assembly=Bonsai.System" | ||
| xmlns="https://bonsai-rx.org/2018/workflow"> | ||
| <Description>Control an Open Ephys commutator by using rotation angle measurements.</Description> | ||
| <Workflow> | ||
| <Nodes> | ||
| <Expression xsi:type="WorkflowInput"> | ||
| <Name>Source1</Name> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="rx:SampleInterval"> | ||
| <rx:Interval>PT0.1S</rx:Interval> | ||
| </Combinator> | ||
| </Expression> | ||
| <Expression xsi:type="ExternalizedMapping"> | ||
| <Property Name="RotationAxis" /> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="commutator:QuaternionToTwist"> | ||
| <commutator:RotationAxis> | ||
| <commutator:X>0</commutator:X> | ||
| <commutator:Y>0</commutator:Y> | ||
| <commutator:Z>1</commutator:Z> | ||
| </commutator:RotationAxis> | ||
| </Combinator> | ||
| </Expression> | ||
| <Expression xsi:type="Format"> | ||
| <Format>{{turn : {0}}}</Format> | ||
| <Selector>it</Selector> | ||
| </Expression> | ||
| <Expression xsi:type="ExternalizedMapping"> | ||
| <Property Name="Value" DisplayName="LedEnable" Description="If true, the commutator's LED will show status information. Otherwise, it will turn off." /> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="BooleanProperty"> | ||
| <Value>false</Value> | ||
| </Combinator> | ||
| </Expression> | ||
| <Expression xsi:type="rx:Condition"> | ||
| <Workflow> | ||
| <Nodes> | ||
| <Expression xsi:type="WorkflowInput"> | ||
| <Name>Source1</Name> | ||
| </Expression> | ||
| <Expression xsi:type="WorkflowOutput" /> | ||
| </Nodes> | ||
| <Edges> | ||
| <Edge From="0" To="1" Label="Source1" /> | ||
| </Edges> | ||
| </Workflow> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="StringProperty"> | ||
| <Value>{led: true}</Value> | ||
| </Combinator> | ||
| </Expression> | ||
| <Expression xsi:type="rx:Condition"> | ||
| <Workflow> | ||
| <Nodes> | ||
| <Expression xsi:type="WorkflowInput"> | ||
| <Name>Source1</Name> | ||
| </Expression> | ||
| <Expression xsi:type="BitwiseNot" /> | ||
| <Expression xsi:type="WorkflowOutput" /> | ||
| </Nodes> | ||
| <Edges> | ||
| <Edge From="0" To="1" Label="Source1" /> | ||
| <Edge From="1" To="2" Label="Source1" /> | ||
| </Edges> | ||
| </Workflow> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="StringProperty"> | ||
| <Value>{led: false}</Value> | ||
| </Combinator> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="rx:Merge" /> | ||
| </Expression> | ||
| <Expression xsi:type="ExternalizedMapping"> | ||
| <Property Name="PortName" /> | ||
| </Expression> | ||
| <Expression xsi:type="Combinator"> | ||
| <Combinator xsi:type="port:SerialWriteLine"> | ||
| <port:PortName /> | ||
| <port:NewLine>\r\n</port:NewLine> | ||
| </Combinator> | ||
| </Expression> | ||
| <Expression xsi:type="WorkflowOutput" /> | ||
| </Nodes> | ||
| <Edges> | ||
| <Edge From="0" To="1" Label="Source1" /> | ||
| <Edge From="1" To="3" Label="Source1" /> | ||
| <Edge From="2" To="3" Label="Source2" /> | ||
| <Edge From="3" To="4" Label="Source1" /> | ||
| <Edge From="4" To="11" Label="Source1" /> | ||
| <Edge From="5" To="6" Label="Source1" /> | ||
| <Edge From="6" To="7" Label="Source1" /> | ||
| <Edge From="6" To="9" Label="Source1" /> | ||
| <Edge From="7" To="8" Label="Source1" /> | ||
| <Edge From="8" To="11" Label="Source2" /> | ||
| <Edge From="9" To="10" Label="Source1" /> | ||
| <Edge From="10" To="11" Label="Source3" /> | ||
| <Edge From="11" To="13" Label="Source1" /> | ||
| <Edge From="12" To="13" Label="Source2" /> | ||
| <Edge From="13" To="14" Label="Source1" /> | ||
| </Edges> | ||
| </Workflow> | ||
| </WorkflowBuilder> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,65 @@ | ||
| using System; | ||
| using System.ComponentModel; | ||
| using System.Linq; | ||
| using System.Numerics; | ||
| using System.Reactive.Linq; | ||
| using Bonsai; | ||
|
|
||
| namespace OpenEphys.Commutator | ||
| { | ||
| /// <summary> | ||
| /// Calculates a the rotation about a specified axis (the "twist") that has occurred between successive 3D | ||
| /// rotation measurements. | ||
| /// </summary> | ||
| [Description("Calculates a feedback control signal to compensate for twisting about the specified axis, in units of turns.")] | ||
| public class QuaternionToTwist : Combinator<Quaternion, double> | ||
| { | ||
| /// <summary> | ||
| /// Gets or sets the direction vector specifying the axis around which to calculate the twist. | ||
| /// </summary> | ||
| /// <remarks> | ||
| /// This vector should point, using the reference frame of the device producing rotation measurements, | ||
| /// in the direction that the tether exits the headstage. Note that negating this vector will result in | ||
| /// negating the direction of twisting. | ||
| /// </remarks> | ||
| [TypeConverter(typeof(NumericRecordConverter))] | ||
| [Description("The direction vector specifying the axis around which to calculate the twist.")] | ||
| public Vector3 RotationAxis { get; set; } = Vector3.UnitZ; | ||
|
|
||
| /// <summary> | ||
| /// Calculates a twist about <see cref="RotationAxis"/> that has occurred between successive rotation | ||
| /// measurements provided by the input sequence. | ||
| /// </summary> | ||
| /// <param name="source">The sequence of rotation measurements.</param> | ||
| /// <returns>The sequence of twist values, in units of turns.</returns> | ||
| public override IObservable<double> Process(IObservable<Quaternion> source) | ||
| { | ||
| return Observable.Defer(() => | ||
| { | ||
| double? previousTwist = 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); | ||
| if (dotProduct < 0) // account for angle-axis flipping | ||
| { | ||
| twist = -twist; | ||
| } | ||
|
|
||
| // 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 | ||
| : 0; | ||
| previousTwist = twistAngle; | ||
| return -feedback / (2 * Math.PI); | ||
| }); | ||
| }); | ||
| } | ||
| } | ||
| } |