Add commutator control based on swing-twist decomposition

.bonsai/Bonsai.config

@@ -2,55 +2,103 @@
 <PackageConfiguration xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
   <Packages>
     <Package id="Bonsai" version="2.8.5" />
+    <Package id="Bonsai.Arduino" version="2.8.1" />
+    <Package id="Bonsai.Audio" version="2.8.0" />
     <Package id="Bonsai.Core" version="2.8.5" />
     <Package id="Bonsai.Design" version="2.8.5" />
+    <Package id="Bonsai.Design.Visualizers" version="2.8.0" />
+    <Package id="Bonsai.Dsp" version="2.8.0" />
+    <Package id="Bonsai.Dsp.Design" version="2.8.0" />
     <Package id="Bonsai.Editor" version="2.8.5" />
+    <Package id="Bonsai.Osc" version="2.7.0" />
+    <Package id="Bonsai.Scripting.Expressions" version="2.8.0" />
+    <Package id="Bonsai.Scripting.Expressions.Design" version="2.8.0" />
+    <Package id="Bonsai.Shaders" version="0.27.0" />
+    <Package id="Bonsai.Shaders.Design" version="0.27.0" />
+    <Package id="Bonsai.StarterPack" version="2.8.1" />
     <Package id="Bonsai.System" version="2.8.1" />
     <Package id="Bonsai.System.Design" version="2.8.0" />
+    <Package id="Bonsai.Vision" version="2.8.0" />
+    <Package id="Bonsai.Vision.Design" version="2.8.1" />
+    <Package id="Bonsai.Windows.Input" version="2.7.0" />
     <Package id="clroni" version="6.1.2" />
     <Package id="jacobslusser.ScintillaNET" version="3.6.3" />
     <Package id="Markdig" version="0.18.1" />
     <Package id="Microsoft.Web.WebView2" version="1.0.1823.32" />
+    <Package id="openal.redist" version="2.0.7" />
     <Package id="OpenCV.Net" version="3.4.2" />
     <Package id="OpenEphys.Onix1" version="0.1.0" />
+    <Package id="OpenTK" version="3.1.0" />
+    <Package id="OpenTK.GLControl" version="3.1.0" />
     <Package id="Rx-Core" version="2.2.5" />
     <Package id="Rx-Interfaces" version="2.2.5" />
     <Package id="Rx-Linq" version="2.2.5" />
     <Package id="Rx-PlatformServices" version="2.2.5" />
     <Package id="SvgNet" version="3.3.3" />
     <Package id="System.Buffers" version="4.5.1" />
+    <Package id="System.Linq.Dynamic" version="1.0.7" />
     <Package id="System.Memory" version="4.5.5" />
     <Package id="System.Numerics.Vectors" version="4.5.0" />
     <Package id="System.Resources.Extensions" version="8.0.0" />
     <Package id="System.Runtime.CompilerServices.Unsafe" version="4.5.3" />
     <Package id="YamlDotNet" version="13.1.1" />
+    <Package id="ZedGraph" version="5.1.7" />
   </Packages>
   <AssemblyReferences>
     <AssemblyReference assemblyName="Bonsai" />
+    <AssemblyReference assemblyName="Bonsai.Arduino" />
+    <AssemblyReference assemblyName="Bonsai.Audio" />
     <AssemblyReference assemblyName="Bonsai.Core" />
     <AssemblyReference assemblyName="Bonsai.Design" />
+    <AssemblyReference assemblyName="Bonsai.Design.Visualizers" />
+    <AssemblyReference assemblyName="Bonsai.Dsp" />
+    <AssemblyReference assemblyName="Bonsai.Dsp.Design" />
     <AssemblyReference assemblyName="Bonsai.Editor" />
+    <AssemblyReference assemblyName="Bonsai.Osc" />
+    <AssemblyReference assemblyName="Bonsai.Scripting.Expressions" />
+    <AssemblyReference assemblyName="Bonsai.Scripting.Expressions.Design" />
+    <AssemblyReference assemblyName="Bonsai.Shaders" />
+    <AssemblyReference assemblyName="Bonsai.Shaders.Design" />
     <AssemblyReference assemblyName="Bonsai.System" />
     <AssemblyReference assemblyName="Bonsai.System.Design" />
+    <AssemblyReference assemblyName="Bonsai.Vision" />
+    <AssemblyReference assemblyName="Bonsai.Vision.Design" />
+    <AssemblyReference assemblyName="Bonsai.Windows.Input" />
     <AssemblyReference assemblyName="OpenEphys.Onix1" />
   </AssemblyReferences>
   <AssemblyLocations>
     <AssemblyLocation assemblyName="Bonsai" processorArchitecture="MSIL" location="Packages/Bonsai.2.8.5/lib/net48/Bonsai.exe" />
+    <AssemblyLocation assemblyName="Bonsai.Arduino" processorArchitecture="MSIL" location="Packages/Bonsai.Arduino.2.8.1/lib/net462/Bonsai.Arduino.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Audio" processorArchitecture="MSIL" location="Packages/Bonsai.Audio.2.8.0/lib/net462/Bonsai.Audio.dll" />
     <AssemblyLocation assemblyName="Bonsai.Core" processorArchitecture="MSIL" location="Packages/Bonsai.Core.2.8.5/lib/net462/Bonsai.Core.dll" />
     <AssemblyLocation assemblyName="Bonsai.Design" processorArchitecture="MSIL" location="Packages/Bonsai.Design.2.8.5/lib/net462/Bonsai.Design.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Design.Visualizers" processorArchitecture="MSIL" location="Packages/Bonsai.Design.Visualizers.2.8.0/lib/net462/Bonsai.Design.Visualizers.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Dsp" processorArchitecture="MSIL" location="Packages/Bonsai.Dsp.2.8.0/lib/net462/Bonsai.Dsp.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Dsp.Design" processorArchitecture="MSIL" location="Packages/Bonsai.Dsp.Design.2.8.0/lib/net462/Bonsai.Dsp.Design.dll" />
     <AssemblyLocation assemblyName="Bonsai.Editor" processorArchitecture="MSIL" location="Packages/Bonsai.Editor.2.8.5/lib/net472/Bonsai.Editor.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Osc" processorArchitecture="MSIL" location="Packages/Bonsai.Osc.2.7.0/lib/net462/Bonsai.Osc.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Scripting.Expressions" processorArchitecture="MSIL" location="Packages/Bonsai.Scripting.Expressions.2.8.0/lib/net462/Bonsai.Scripting.Expressions.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Scripting.Expressions.Design" processorArchitecture="MSIL" location="Packages/Bonsai.Scripting.Expressions.Design.2.8.0/lib/net462/Bonsai.Scripting.Expressions.Design.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Shaders" processorArchitecture="MSIL" location="Packages/Bonsai.Shaders.0.27.0/lib/net462/Bonsai.Shaders.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Shaders.Design" processorArchitecture="MSIL" location="Packages/Bonsai.Shaders.Design.0.27.0/lib/net462/Bonsai.Shaders.Design.dll" />
     <AssemblyLocation assemblyName="Bonsai.System" processorArchitecture="MSIL" location="Packages/Bonsai.System.2.8.1/lib/net462/Bonsai.System.dll" />
     <AssemblyLocation assemblyName="Bonsai.System.Design" processorArchitecture="MSIL" location="Packages/Bonsai.System.Design.2.8.0/lib/net462/Bonsai.System.Design.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Vision" processorArchitecture="MSIL" location="Packages/Bonsai.Vision.2.8.0/lib/net462/Bonsai.Vision.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Vision.Design" processorArchitecture="MSIL" location="Packages/Bonsai.Vision.Design.2.8.1/lib/net462/Bonsai.Vision.Design.dll" />
+    <AssemblyLocation assemblyName="Bonsai.Windows.Input" processorArchitecture="MSIL" location="Packages/Bonsai.Windows.Input.2.7.0/lib/net462/Bonsai.Windows.Input.dll" />
     <AssemblyLocation assemblyName="clroni" processorArchitecture="Amd64" location="Packages/clroni.6.1.2/lib/net472/clroni.dll" />
     <AssemblyLocation assemblyName="Markdig" processorArchitecture="MSIL" location="Packages/Markdig.0.18.1/lib/net40/Markdig.dll" />
     <AssemblyLocation assemblyName="Microsoft.Web.WebView2.Core" processorArchitecture="MSIL" location="Packages/Microsoft.Web.WebView2.1.0.1823.32/lib/net45/Microsoft.Web.WebView2.Core.dll" />
     <AssemblyLocation assemblyName="Microsoft.Web.WebView2.WinForms" processorArchitecture="MSIL" location="Packages/Microsoft.Web.WebView2.1.0.1823.32/lib/net45/Microsoft.Web.WebView2.WinForms.dll" />
     <AssemblyLocation assemblyName="Microsoft.Web.WebView2.Wpf" processorArchitecture="MSIL" location="Packages/Microsoft.Web.WebView2.1.0.1823.32/lib/net45/Microsoft.Web.WebView2.Wpf.dll" />
     <AssemblyLocation assemblyName="OpenCV.Net" processorArchitecture="MSIL" location="Packages/OpenCV.Net.3.4.2/lib/net462/OpenCV.Net.dll" />
     <AssemblyLocation assemblyName="OpenEphys.Onix1" processorArchitecture="Amd64" location="Packages/OpenEphys.Onix1.0.1.0/lib/net472/OpenEphys.Onix1.dll" />
+    <AssemblyLocation assemblyName="OpenTK" processorArchitecture="MSIL" location="Packages/OpenTK.3.1.0/lib/net20/OpenTK.dll" />
+    <AssemblyLocation assemblyName="OpenTK.GLControl" processorArchitecture="MSIL" location="Packages/OpenTK.GLControl.3.1.0/lib/net20/OpenTK.GLControl.dll" />
     <AssemblyLocation assemblyName="ScintillaNET" processorArchitecture="MSIL" location="Packages/jacobslusser.ScintillaNET.3.6.3/lib/net40/ScintillaNET.dll" />
     <AssemblyLocation assemblyName="SVG" processorArchitecture="MSIL" location="Packages/SvgNet.3.3.3/lib/net462/SVG.dll" />
     <AssemblyLocation assemblyName="System.Buffers" processorArchitecture="MSIL" location="Packages/System.Buffers.4.5.1/lib/net461/System.Buffers.dll" />
+    <AssemblyLocation assemblyName="System.Linq.Dynamic" processorArchitecture="MSIL" location="Packages/System.Linq.Dynamic.1.0.7/lib/net40/System.Linq.Dynamic.dll" />
     <AssemblyLocation assemblyName="System.Memory" processorArchitecture="MSIL" location="Packages/System.Memory.4.5.5/lib/net461/System.Memory.dll" />
     <AssemblyLocation assemblyName="System.Numerics.Vectors" processorArchitecture="MSIL" location="Packages/System.Numerics.Vectors.4.5.0/lib/net46/System.Numerics.Vectors.dll" />
     <AssemblyLocation assemblyName="System.Reactive.Core" processorArchitecture="MSIL" location="Packages/Rx-Core.2.2.5/lib/net45/System.Reactive.Core.dll" />
@@ -60,6 +108,7 @@
     <AssemblyLocation assemblyName="System.Resources.Extensions" processorArchitecture="MSIL" location="Packages/System.Resources.Extensions.8.0.0/lib/net462/System.Resources.Extensions.dll" />
     <AssemblyLocation assemblyName="System.Runtime.CompilerServices.Unsafe" processorArchitecture="MSIL" location="Packages/System.Runtime.CompilerServices.Unsafe.4.5.3/lib/net461/System.Runtime.CompilerServices.Unsafe.dll" />
     <AssemblyLocation assemblyName="YamlDotNet" processorArchitecture="MSIL" location="Packages/YamlDotNet.13.1.1/lib/net47/YamlDotNet.dll" />
+    <AssemblyLocation assemblyName="ZedGraph" processorArchitecture="MSIL" location="Packages/ZedGraph.5.1.7/lib/net35-Client/ZedGraph.dll" />
   </AssemblyLocations>
   <LibraryFolders>
     <LibraryFolder path="Packages/clroni.6.1.2/build/native/bin/x64" platform="x64" />
@@ -70,6 +119,8 @@
     <LibraryFolder path="Packages/Microsoft.Web.WebView2.1.0.1823.32/runtimes/win-x64/native_uap" platform="x64" />
     <LibraryFolder path="Packages/Microsoft.Web.WebView2.1.0.1823.32/runtimes/win-x86/native" platform="x86" />
     <LibraryFolder path="Packages/Microsoft.Web.WebView2.1.0.1823.32/runtimes/win-x86/native_uap" platform="x86" />
+    <LibraryFolder path="Packages/openal.redist.2.0.7.0/build/native/bin/x64" platform="x64" />
+    <LibraryFolder path="Packages/openal.redist.2.0.7.0/build/native/bin/x86" platform="x86" />
     <LibraryFolder path="Packages/OpenCV.Net.3.4.2/runtimes/win-x64/native/vc14/bin" platform="x64" />
     <LibraryFolder path="Packages/OpenCV.Net.3.4.2/runtimes/win-x86/native/vc14/bin" platform="x86" />
   </LibraryFolders>

OpenEphys.Commutator/AutoCommutator.bonsai

@@ -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>

OpenEphys.Commutator/OpenEphys.Commutator.csproj

@@ -7,6 +7,10 @@
     <TargetFramework>net472</TargetFramework>
   </PropertyGroup>
 
+  <ItemGroup>
+    <EmbeddedResource Include="**/*.bonsai" />
+  </ItemGroup>
+
   <ItemGroup>
     <PackageReference Include="Bonsai.Core" Version="2.8.5" />
     <PackageReference Include="Bonsai.System" Version="2.8.1" />

OpenEphys.Commutator/QuaternionToTwist.cs

@@ -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);
+                });
+            });
+        }
+    }
+}