Skip to content

Commit e5f7724

Browse files
kjy5kjy5
authored
625 change probe type otf for pathfinder (#626)
* Rebuild logic * Can recreate probes, breaks panels * Early shot for pathfinder
1 parent 9db92fa commit e5f7724

File tree

2 files changed

+145
-91
lines changed

2 files changed

+145
-91
lines changed

‎Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController.cs‎

Lines changed: 125 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,10 @@ public bool IsRightHanded
8686
}
8787
}
8888

89+
// Helper functions to create and destroy a probe
90+
public Action<ProbeProperties.ProbeType> CreatePathfinderProbe { private get; set; }
91+
public Action DestroyThisProbe { private get; set; }
92+
8993
#region Private internal fields
9094

9195
private Vector4 _lastManipulatorPosition = Vector4.zero;
@@ -341,98 +345,139 @@ private void EchoPosition(Vector4 pos)
341345
{
342346
if (!enabled && _probeController == null) return;
343347

344-
// Check for special Pathfinder mode (directly set probe position, no calculations needed)
348+
// Check for special Pathfinder mode
345349
if (NumAxes == -1)
346350
{
347-
CommunicationManager.Instance.GetAngles(ManipulatorID, angles =>
351+
// Check if probe type changed
352+
CommunicationManager.Instance.GetShankCount(ManipulatorID, shankCount =>
348353
{
349-
_probeController.SetProbeAngles(new Vector3(angles.x, 90 - angles.y, angles.z));
350-
351-
// If only the DV axis moved, then we drop on DV. Otherwise, we drop on depth.
352-
if (Math.Abs(pos.z - _lastManipulatorPosition.z) > 0.0001)
353-
IsSetToDropToSurfaceWithDepth = Math.Abs(pos.x - _lastManipulatorPosition.x) > 0.0001 ||
354-
Math.Abs(pos.y - _lastManipulatorPosition.y) > 0.0001;
355-
356-
// Copy in new 3-axis position into saved 4-axis position
357-
_lastManipulatorPosition = new Vector4(pos.x, pos.y, pos.z, _lastManipulatorPosition.w);
358-
359-
// Apply brain surface offset on correct axis
360-
var brainSurfaceAdjustment = float.IsNaN(BrainSurfaceOffset) ? 0 : BrainSurfaceOffset;
361-
if (IsSetToDropToSurfaceWithDepth)
362-
_lastManipulatorPosition.w -= brainSurfaceAdjustment;
363-
else
364-
_lastManipulatorPosition.z -= brainSurfaceAdjustment;
365-
366-
// Convert Pathfinder space coordinates into active atlas space
367-
var convertedPos =
368-
_probeController.Insertion.World2T_Vector(
369-
CoordinateSpace.Space2World_Vector(_lastManipulatorPosition));
370-
371-
// Copy and add 4th axis back in
372-
_probeController.SetProbePosition(new Vector4(convertedPos.x, convertedPos.y, convertedPos.z,
373-
_lastManipulatorPosition.w));
374-
});
354+
// Use 2.4 if 4 shank, otherwise default to 1
355+
var probeType = shankCount == 4
356+
? ProbeProperties.ProbeType.Neuropixels24
357+
: ProbeProperties.ProbeType.Neuropixels1;
358+
359+
// print("Read type: " + probeType + "; Current type: " + _probeManager.ProbeType);
360+
// Check if change is needed
361+
if (probeType != _probeManager.ProbeType)
362+
{
363+
// Unregister manipulator
364+
_probeManager.SetIsEphysLinkControlled(false, ManipulatorID, true, () =>
365+
{
366+
// Create new probe
367+
CreatePathfinderProbe.Invoke(probeType);
368+
369+
// Destroy current probe
370+
DestroyThisProbe.Invoke();
371+
}, Debug.LogError);
372+
373+
// Exit early as this probe no longer exists
374+
return;
375+
}
376+
377+
// Otherwise, update probe angles
378+
CommunicationManager.Instance.GetAngles(ManipulatorID, angles =>
379+
{
380+
_probeController.SetProbeAngles(new Vector3(angles.x, 90 - angles.y, angles.z));
381+
382+
// If only the DV axis moved, then we drop on DV. Otherwise, we drop on depth.
383+
if (Math.Abs(pos.z - _lastManipulatorPosition.z) > 0.0001)
384+
IsSetToDropToSurfaceWithDepth = Math.Abs(pos.x - _lastManipulatorPosition.x) > 0.0001 ||
385+
Math.Abs(pos.y - _lastManipulatorPosition.y) > 0.0001;
386+
387+
// Copy in new 3-axis position into saved 4-axis position
388+
_lastManipulatorPosition = new Vector4(pos.x, pos.y, pos.z, _lastManipulatorPosition.w);
389+
390+
// Apply brain surface offset on correct axis
391+
var brainSurfaceAdjustment = float.IsNaN(BrainSurfaceOffset) ? 0 : BrainSurfaceOffset;
392+
if (IsSetToDropToSurfaceWithDepth)
393+
_lastManipulatorPosition.w -= brainSurfaceAdjustment;
394+
else
395+
_lastManipulatorPosition.z -= brainSurfaceAdjustment;
396+
397+
// Convert Pathfinder space coordinates into active atlas space
398+
var convertedPos =
399+
_probeController.Insertion.World2T_Vector(
400+
CoordinateSpace.Space2World_Vector(_lastManipulatorPosition));
401+
402+
// Copy and add 4th axis back in
403+
_probeController.SetProbePosition(new Vector4(convertedPos.x, convertedPos.y,
404+
convertedPos.z,
405+
_lastManipulatorPosition.w));
406+
407+
// Log and continue echoing
408+
LogAndContinue();
409+
}, Debug.LogError);
410+
}, Debug.LogError);
411+
412+
// Exit early as we've handled Pathfinder
413+
return;
375414
}
415+
416+
// Calculate last used direction for dropping to brain surface (between depth and DV)
417+
var dvDelta = Math.Abs(pos.z - _lastManipulatorPosition.z);
418+
var depthDelta = Math.Abs(pos.w - _lastManipulatorPosition.w);
419+
if (dvDelta > 0.0001 || depthDelta > 0.0001) IsSetToDropToSurfaceWithDepth = depthDelta > dvDelta;
420+
_lastManipulatorPosition = pos;
421+
422+
// Apply zero coordinate offset
423+
var zeroCoordinateAdjustedManipulatorPosition = pos - ZeroCoordinateOffset;
424+
425+
// Convert to coordinate space
426+
var manipulatorSpacePosition = CoordinateTransform.T2U(zeroCoordinateAdjustedManipulatorPosition);
427+
428+
// Brain surface adjustment
429+
var brainSurfaceAdjustment = float.IsNaN(BrainSurfaceOffset) ? 0 : BrainSurfaceOffset;
430+
if (IsSetToDropToSurfaceWithDepth)
431+
zeroCoordinateAdjustedManipulatorPosition.w += brainSurfaceAdjustment;
376432
else
377-
{
378-
// Calculate last used direction for dropping to brain surface (between depth and DV)
379-
var dvDelta = Math.Abs(pos.z - _lastManipulatorPosition.z);
380-
var depthDelta = Math.Abs(pos.w - _lastManipulatorPosition.w);
381-
if (dvDelta > 0.0001 || depthDelta > 0.0001) IsSetToDropToSurfaceWithDepth = depthDelta > dvDelta;
382-
_lastManipulatorPosition = pos;
383-
384-
// Apply zero coordinate offset
385-
var zeroCoordinateAdjustedManipulatorPosition = pos - ZeroCoordinateOffset;
386-
387-
// Convert to coordinate space
388-
var manipulatorSpacePosition = CoordinateTransform.T2U(zeroCoordinateAdjustedManipulatorPosition);
389-
390-
// Brain surface adjustment
391-
var brainSurfaceAdjustment = float.IsNaN(BrainSurfaceOffset) ? 0 : BrainSurfaceOffset;
392-
if (IsSetToDropToSurfaceWithDepth)
393-
zeroCoordinateAdjustedManipulatorPosition.w += brainSurfaceAdjustment;
394-
else
395-
manipulatorSpacePosition.y -= brainSurfaceAdjustment;
433+
manipulatorSpacePosition.y -= brainSurfaceAdjustment;
396434

397-
// Convert to world space
398-
var zeroCoordinateAdjustedWorldPosition =
399-
CoordinateSpace.Space2World(manipulatorSpacePosition);
435+
// Convert to world space
436+
var zeroCoordinateAdjustedWorldPosition =
437+
CoordinateSpace.Space2World(manipulatorSpacePosition);
400438

401-
// Set probe position (change axes to match probe)
402-
var transformedApmldv =
403-
_probeController.Insertion.World2T_Vector(zeroCoordinateAdjustedWorldPosition);
439+
// Set probe position (change axes to match probe)
440+
var transformedApmldv =
441+
_probeController.Insertion.World2T_Vector(zeroCoordinateAdjustedWorldPosition);
404442

405-
// Split between 3 and 4 axis assignments
406-
if (CoordinateTransform.Prefix == "3lhm")
407-
_probeController.SetProbePosition(transformedApmldv);
408-
else
409-
_probeController.SetProbePosition(new Vector4(transformedApmldv.x, transformedApmldv.y,
410-
transformedApmldv.z, zeroCoordinateAdjustedManipulatorPosition.w));
411-
}
443+
// Split between 3 and 4 axis assignments
444+
if (CoordinateTransform.Prefix == "3lhm")
445+
_probeController.SetProbePosition(transformedApmldv);
446+
else
447+
_probeController.SetProbePosition(new Vector4(transformedApmldv.x, transformedApmldv.y,
448+
transformedApmldv.z, zeroCoordinateAdjustedManipulatorPosition.w));
412449

413-
// Log every 5 hz
414-
if (Time.time - _lastLoggedTime >= 0.2)
415-
{
416-
_lastLoggedTime = Time.time;
417-
var tipPos = _probeController.ProbeTipT.position;
450+
// Log and continue echoing
451+
LogAndContinue();
452+
return;
418453

419-
// ["ephys_link", Real time stamp, Manipulator ID, X, Y, Z, W, Phi, Theta, Spin, TipX, TipY, TipZ]
420-
string[] data =
454+
void LogAndContinue()
455+
{
456+
// Log every 5 hz
457+
if (Time.time - _lastLoggedTime >= 0.2)
421458
{
422-
"ephys_link", Time.realtimeSinceStartup.ToString(CultureInfo.InvariantCulture), ManipulatorID,
423-
pos.x.ToString(CultureInfo.InvariantCulture), pos.y.ToString(CultureInfo.InvariantCulture),
424-
pos.z.ToString(CultureInfo.InvariantCulture), pos.w.ToString(CultureInfo.InvariantCulture),
425-
_probeController.Insertion.Yaw.ToString(CultureInfo.InvariantCulture),
426-
_probeController.Insertion.Pitch.ToString(CultureInfo.InvariantCulture),
427-
_probeController.Insertion.Roll.ToString(CultureInfo.InvariantCulture),
428-
tipPos.x.ToString(CultureInfo.InvariantCulture), tipPos.y.ToString(CultureInfo.InvariantCulture),
429-
tipPos.z.ToString(CultureInfo.InvariantCulture)
430-
};
431-
OutputLog.Log(data);
432-
}
459+
_lastLoggedTime = Time.time;
460+
var tipPos = _probeController.ProbeTipT.position;
461+
462+
// ["ephys_link", Real time stamp, Manipulator ID, X, Y, Z, W, Phi, Theta, Spin, TipX, TipY, TipZ]
463+
string[] data =
464+
{
465+
"ephys_link", Time.realtimeSinceStartup.ToString(CultureInfo.InvariantCulture), ManipulatorID,
466+
pos.x.ToString(CultureInfo.InvariantCulture), pos.y.ToString(CultureInfo.InvariantCulture),
467+
pos.z.ToString(CultureInfo.InvariantCulture), pos.w.ToString(CultureInfo.InvariantCulture),
468+
_probeController.Insertion.Yaw.ToString(CultureInfo.InvariantCulture),
469+
_probeController.Insertion.Pitch.ToString(CultureInfo.InvariantCulture),
470+
_probeController.Insertion.Roll.ToString(CultureInfo.InvariantCulture),
471+
tipPos.x.ToString(CultureInfo.InvariantCulture),
472+
tipPos.y.ToString(CultureInfo.InvariantCulture),
473+
tipPos.z.ToString(CultureInfo.InvariantCulture)
474+
};
475+
OutputLog.Log(data);
476+
}
433477

434-
// Continue echoing position
435-
CommunicationManager.Instance.GetPos(ManipulatorID, EchoPosition);
478+
// Continue echoing position
479+
CommunicationManager.Instance.GetPos(ManipulatorID, EchoPosition);
480+
}
436481
}
437482

438483
#endregion

‎Assets/Scripts/Pinpoint/UI/EphysLinkSettings/ManipulatorConnectionPanel.cs‎

Lines changed: 20 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -78,17 +78,26 @@ public void Initialize(EphysLinkSettings settingsMenu, string manipulatorID,
7878
var probeType = shankCount == 4
7979
? ProbeProperties.ProbeType.Neuropixels24
8080
: ProbeProperties.ProbeType.Neuropixels1;
81-
82-
// Create new probe
83-
var trajectoryPlannerManager = FindObjectOfType<TrajectoryPlannerManager>();
84-
var newProbe = trajectoryPlannerManager.AddNewProbe(probeType);
85-
86-
// Configure probe and link to Ephys Link
87-
newProbe.ManipulatorBehaviorController.NumAxes = numAxes;
88-
newProbe.Color = Color.magenta;
89-
newProbe.name = "nsp_" + manipulatorID;
90-
newProbe.Saved = false;
91-
newProbe.SetIsEphysLinkControlled(true, manipulatorID);
81+
82+
CreatePathfinderProbe(probeType);
83+
return;
84+
85+
void CreatePathfinderProbe(ProbeProperties.ProbeType newProbeType)
86+
{
87+
// Create new probe
88+
var trajectoryPlannerManager = FindObjectOfType<TrajectoryPlannerManager>();
89+
var newProbe = trajectoryPlannerManager.AddNewProbe(newProbeType);
90+
91+
// Configure probe and link to Ephys Link
92+
newProbe.ManipulatorBehaviorController.NumAxes = numAxes;
93+
newProbe.ManipulatorBehaviorController.CreatePathfinderProbe = CreatePathfinderProbe;
94+
newProbe.ManipulatorBehaviorController.DestroyThisProbe = () =>
95+
trajectoryPlannerManager.DestroyProbe(newProbe);
96+
newProbe.Color = Color.magenta;
97+
newProbe.name = "nsp_" + manipulatorID;
98+
newProbe.Saved = false;
99+
newProbe.SetIsEphysLinkControlled(true, manipulatorID);
100+
}
92101
}, Debug.LogError);
93102

94103
// Exit (don't need to do anything else for Pathfinder)

0 commit comments

Comments
 (0)