public void LoadTHAvatar()
{
SaveSystem.LoadUserData("MD1942");
AvatarSystem.LoadPlayer(UserType.Ablebodied, AvatarType.Transhumeral);
AvatarSystem.LoadAvatar(SaveSystem.ActiveUser, AvatarType.Transhumeral);
// Initialize prosthesis
GameObject prosthesisManagerGO = GameObject.FindGameObjectWithTag("ProsthesisManager");
elbowManager = prosthesisManagerGO.AddComponent <ConfigurableElbowManager>();
elbowManager.InitializeProsthesis(SaveSystem.ActiveUser.upperArmLength, (SaveSystem.ActiveUser.forearmLength + SaveSystem.ActiveUser.handLength / 2.0f), 1.5f);
// Set the reference generator to jacobian-based.
elbowManager.ChangeReferenceGenerator("VAL_REFGEN_LINKINSYN");
//elbowManager.ChangeReferenceGenerator("VAL_REFGEN_JACOBIANSYN");
// Initialize UI.
//InitializeUI();
// Configure the grasp manager
GameObject graspManagerGO = GameObject.FindGameObjectWithTag("GraspManager");
if (graspManagerGO == null)
{
throw new System.Exception("Grasp Manager not found.");
}
GraspManager graspManager = graspManagerGO.GetComponent <GraspManager>();
graspManager.managerType = GraspManager.GraspManagerType.Assisted;
graspManager.managerMode = GraspManager.GraspManagerMode.Restriced;
// set syn
elbowManager.SetSynergy(theta);
}
private void FixedUpdate()
{
if (buttonAction.GetStateDown(SteamVR_Input_Sources.Any))
{
theta += 0.05f;
elbowManager.SetSynergy(theta);
}
}
/// <summary>
/// Handles the procedures performed when analysing results.
/// </summary>
public override void HandleResultAnalysis()
{
if (!debug)
{
// Performance evaluation
if (evaluatorType == EvaluatorType.Compensation)
{
evaluator.AddData <Vector3>(shDataBuffer, UpperBodyCompensationMotionPM.SHOULDER);
evaluator.AddData <Vector3>(c7DataBuffer, UpperBodyCompensationMotionPM.TRUNK);
}
else if (evaluatorType == EvaluatorType.KinematicEnergy)
{
throw new System.NotImplementedException("KE method not yet implemented.");
}
float J = evaluator.Update();
//Debug.Log("J = " + J);
string iterationResults = iterationNumber + "," +
J;
// If it's able-bodied, no need to do anything
// If it's KE-Adaptive-Synergy then perform synergy update
if (experimentType == ExperimentType.TypeTwo && adaptiveSynergy)
{
// Perform update
theta = personaliser.UpdateParameter(J, iterationNumber);
elbowManager.SetSynergy(theta);
// Add algorithm states data to log
foreach (float value in personaliser.GetStates())
{
iterationResults += "," + value;
}
iterationResults += "," + theta;
}
Vector3 ballPos = gridManager.GetSelectedBallPosition();
iterationResults += "," + targetOrder[iterationNumber - 1] + "," + ballPos.x + "," + ballPos.y + "," + ballPos.z;
//Debug.Log("Theta: " + theta);
// Log results
performanceDataLogger.AppendData(iterationResults);
performanceDataLogger.SaveLog();
}
}
/// <summary>
/// Initializes the ExperimentSystem and its components.
/// Verifies that all components needed for the experiment are available.
/// This must be done in Start.
/// Extend this method by doing your own implementation, with base.InitExperimentSystem() being called at the start.
/// </summary>
public override void InitialiseExperimentSystems()
{
//
// Set the experiment type configuration
//
// Type one if able-bodied subject
if (AvatarSystem.AvatarType == AvatarType.AbleBodied)
{
experimentType = ExperimentType.TypeOne; // Able-bodied experiment type
taskDataFormat = ablebodiedDataFormat;
performanceDataFormat = ablebodiedPerformanceDataFormat;
}
// Type two if prosthetic (Adaptive Synergy)
else if (AvatarSystem.AvatarType == AvatarType.Transhumeral)
{
experimentType = ExperimentType.TypeTwo; // Able-bodied experiment type
taskDataFormat = prostheticDataFormat;
performanceDataFormat = prostheticPerformanceDataFormat;
}
// Then run the base initialisation which is needed, with a small modification
//
// Set the experiment name only when debugging. Take the name from the gameobject + Debug
//
if (debug)
{
ExperimentSystem.SetActiveExperimentID(this.gameObject.name + "_Debug");
}
// Make sure flow control is initialised
sessionNumber = 1;
iterationNumber = 1;
//
// Create the default data loggers
//
taskDataLogger = new DataStreamLogger("TaskData/" + AvatarSystem.AvatarType.ToString());
ExperimentSystem.AddExperimentLogger(taskDataLogger);
taskDataLogger.AddNewLogFile(sessionNumber, iterationNumber, taskDataFormat); // Add file
// Restart UDP threads
foreach (ISensor sensor in AvatarSystem.GetActiveSensors())
{
if (sensor is UDPSensorManager udpSensor)
{
//Debug.Log(wifiSensor.RunThread);
udpSensor.StartSensorReading();
//Debug.Log(wifiSensor.RunThread);
}
}
// Send the player to the experiment centre position
TeleportToStartPosition();
startPosPhoto.SetActive(false);
//
// Create the performance data loggers
//
performanceDataLogger = new DataStreamLogger("PerformanceData");
ExperimentSystem.AddExperimentLogger(performanceDataLogger);
performanceDataLogger.AddNewLogFile(AvatarSystem.AvatarType.ToString(), sessionNumber, performanceDataFormat); // Add file
if (SaveSystem.ActiveUser.lefty)
{
leftySign = -1.0f;
}
//
// Iterations configuration
//
// Set iterations variables for flow control.
targetNumber = gridRows * gridColumns;
for (int i = 0; i < iterationsPerSession.Count; i++)
{
iterationsPerSession[i] = targetNumber * iterationsPerTarget;
}
// Create the list of target indexes and shuffle it.
for (int i = 0; i < targetNumber; i++)
{
for (int j = 0; j < iterationsPerTarget; j++)
{
targetOrder.Add(i);
}
}
targetOrder.Shuffle();
//
// Initialize world positioning
//
// Get user physiological data.
float subjectHeight = SaveSystem.ActiveUser.height;
float subjectArmLength = SaveSystem.ActiveUser.upperArmLength + SaveSystem.ActiveUser.forearmLength + (SaveSystem.ActiveUser.handLength / 2);
// Set the grid distance from subject
gridManager.transform.position = new Vector3((-gridReachMultiplier * subjectArmLength) - 0.1f, gridHeightMultiplier * subjectHeight, 0.0f);
//
// Add arm motion trackers for able-bodied case.
//
if (experimentType == ExperimentType.TypeOne)
{
// Lower limb motion tracker
GameObject llMotionTrackerGO = GameObject.FindGameObjectWithTag("ForearmTracker");
lowerArmTracker = new VIVETrackerManager(llMotionTrackerGO.transform);
ExperimentSystem.AddSensor(lowerArmTracker);
// Upper limb motion tracker
GameObject ulMotionTrackerGO = AvatarSystem.AddMotionTracker();
upperArmTracker = new VIVETrackerManager(ulMotionTrackerGO.transform);
ExperimentSystem.AddSensor(upperArmTracker);
}
else if (experimentType == ExperimentType.TypeTwo)
{
// Get active sensors from avatar system and get the vive tracker being used for the UA
foreach (ISensor sensor in AvatarSystem.GetActiveSensors())
{
if (sensor is VIVETrackerManager)
{
upperArmTracker = (VIVETrackerManager)sensor;
}
}
if (upperArmTracker == null)
{
throw new System.NullReferenceException("The residual limb tracker was not found.");
}
// Set VIVE tracker and Linear synergy as active.
// Get prosthesis
prosthesisManagerGO = GameObject.FindGameObjectWithTag("ProsthesisManager");
elbowManager = prosthesisManagerGO.GetComponent <ConfigurableElbowManager>();
// Set the reference generator to linear synergy.
elbowManager.ChangeSensor("VAL_SENSOR_VIVETRACKER");
elbowManager.ChangeReferenceGenerator("VAL_REFGEN_LINKINSYN");
// Create the personalisation algorithm object
elbowManager.SetSynergy(theta);
float[] ditherFrequency = { Mathf.PI / 4, 2 * Mathf.PI / 4 };
float[] observerGain = { 0.3840f, 0.6067f, -0.2273f, -0.8977f, -1.0302f };
float[][] A = new float[2][];
A[0] = new float[2] {
1.3130f, -0.8546f
};
A[1] = new float[2] {
1.0f, 0.0f
};
float[] B = { 1.0f, 0.0f };
float[] C = { 0.0131f, -0.0131f };
float D = 0.0f;
personaliser = new TransCyberHPIPersonalisation(ditherAmplitude, 0, ditherFrequency, observerGain, 1, optimiserGain, 0.1f, A, B, C, D, theta, 0.1f, 3.0f);
}
if (evaluatorType == EvaluatorType.Compensation)
{
// Performance evaluation objects
evaluator = new UpperBodyCompensationMotionPM(0.5f, 0.5f);
shDataBuffer = new List <Vector3>();
c7DataBuffer = new List <Vector3>();
}
else if (evaluatorType == EvaluatorType.KinematicEnergy)
{
throw new System.NotImplementedException("KE method not yet implemented");
}
// Debug?
if (!debug)
{
// Shoulder acromium head tracker
GameObject motionTrackerGO1 = AvatarSystem.AddMotionTracker();
shoulderTracker = new VIVETrackerManager(motionTrackerGO1.transform);
ExperimentSystem.AddSensor(shoulderTracker);
// C7 tracker
GameObject motionTrackerGO2 = AvatarSystem.AddMotionTracker();
c7Tracker = new VIVETrackerManager(motionTrackerGO2.transform);
ExperimentSystem.AddSensor(c7Tracker);
}
//
// Hand tracking sensor
//
GameObject handGO = GameObject.FindGameObjectWithTag("Hand");
handTracker = new VirtualPositionTracker(handGO.transform);
ExperimentSystem.AddSensor(handTracker);
// Spawn grid
gridManager.SpawnGrid(gridRows, gridColumns, gridSpacing);
}