}//function - Start
#endregion //Start
#region Update
//-----------------------------------------------------------------------------
// UPDATED BY CHRISTOPHER HUNT <*****@*****.**>
void Update()
{
// Is called periodically for handling events
MovementState movementState = GetMovementState();
if (movementState == MovementState.Rest)
{
f_TargetArmAngles = GetRightUpperArmAngles();
f_TargetFingerAngles = GetRightFingerAngles();
}
else
{
switch (movementState)
{
case MovementState.ShoulderFlexion:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_ShoulderFlexionArmAngles;
break;
case MovementState.ShoulderExtension:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_ShoulderExtensionArmAngles;
break;
case MovementState.ShoulderAdduction:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_ShoulderAdductionArmAngles;
break;
case MovementState.ShoulderAbduction:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_ShoulderAbductionArmAngles;
break;
case MovementState.HumeralMedial:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_HumeralMedialArmAngles;
break;
case MovementState.HumeralLateral:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_HumeralLateralArmAngles;
break;
case MovementState.ElbowFlexion:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_ElbowFlexionArmAngles;
break;
case MovementState.ElbowExtension:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_ElbowExtensionArmAngles;
break;
case MovementState.WristPronate:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_WristPronateArmAngles;
break;
case MovementState.WristSupinate:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_WristSupinateArmAngles;
break;
case MovementState.WristUlnar:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_WristUlnarArmAngles;
break;
case MovementState.WristRadial:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_WristRadialArmAngles;
break;
case MovementState.WristFlexion:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_WristFlexionArmAngles;
break;
case MovementState.WristExtension:
f_StartArmAngles = GetRightUpperArmAngles();
f_TargetArmAngles = f_WristExtensionArmAngles;
break;
case MovementState.HandOpen:
f_StartFingerAngles = GetRightFingerAngles();
f_TargetFingerAngles = f_HandOpenFingerAngles;
break;
case MovementState.TripodGrasp:
f_StartFingerAngles = GetRightFingerAngles();
f_TargetFingerAngles = f_TripodFingerAngles;
break;
case MovementState.KeyGrasp:
f_StartFingerAngles = GetRightFingerAngles();
f_TargetFingerAngles = f_KeyFingerAngles;
break;
case MovementState.PinchGrasp:
f_StartFingerAngles = GetRightFingerAngles();
f_TargetFingerAngles = f_PinchOpenFingerAngles;
break;
case MovementState.PowerGrasp:
f_StartFingerAngles = GetRightFingerAngles();
f_TargetFingerAngles = f_PowerFingerAngles;
break;
case MovementState.IndexGrasp:
f_StartFingerAngles = GetRightFingerAngles();
f_TargetFingerAngles = f_ActiveIndexFingerAngles;
break;
}
}
// if we are not resting
if (movementState != MovementState.Rest)
{
if (VulcanXHandle.m_haveRightMPL)
{
// move finger angles for right MPL
float [] f_CurrentFingerAngles = GetRightFingerAngles();
float [] f_CurrentFingerDistances = new float[MPL_NUM_FINGER_JOINTS];
float maxFingerDistance = 0.0f;
for (int i = 0; i < MPL_NUM_FINGER_JOINTS; i++)
{
f_CurrentFingerDistances[i] = Mathf.Abs(f_CurrentFingerAngles[i] - f_TargetFingerAngles[i]);
maxFingerDistance = Mathf.Max(maxFingerDistance, f_CurrentFingerDistances[i]);
}
for (int i = 0; i < MPL_NUM_FINGER_JOINTS; i++)
{
if (f_CurrentFingerDistances[i] > ANGLE_EPSILON)
{
if (f_CurrentFingerAngles[i] < f_TargetFingerAngles[i])
{
f_CurrentFingerAngles[i] += (MaxFingerSpeed * (f_CurrentFingerDistances[i] / maxFingerDistance) * Time.deltaTime);
}
else if (f_CurrentFingerAngles[i] > f_TargetFingerAngles[i])
{
f_CurrentFingerAngles[i] -= (MaxFingerSpeed * (f_CurrentFingerDistances[i] / maxFingerDistance) * Time.deltaTime);
}
}
}
SetRightFingerAngles(f_CurrentFingerAngles);
// move upper arm angles for right MPL
float [] f_CurrentArmAngles = GetRightUpperArmAngles();
float [] f_CurrentArmDistances = new float[MPL_NUM_ARM_JOINTS];
float maxArmDistance = 0.0f;
for (int i = 0; i < MPL_NUM_ARM_JOINTS; i++)
{
if (!float.IsNaN(f_TargetArmAngles[i]))
{
f_CurrentArmDistances[i] = Mathf.Abs(f_CurrentArmAngles[i] - f_TargetArmAngles[i]);
maxArmDistance = Mathf.Max(maxArmDistance, f_CurrentArmDistances[i]);
}
}
for (int i = 0; i < MPL_NUM_ARM_JOINTS; i++)
{
if (!float.IsNaN(f_TargetArmAngles[i]))
{
if (f_CurrentArmDistances[i] > ANGLE_EPSILON)
{
if (f_CurrentArmAngles[i] < f_TargetArmAngles[i])
{
f_CurrentArmAngles[i] += (MaxArmSpeed * Time.deltaTime);
}
else if (f_CurrentArmAngles[i] > f_TargetArmAngles[i])
{
f_CurrentArmAngles[i] -= (MaxArmSpeed * Time.deltaTime);
}
}
}
}
SetRightUpperArmAngles(f_CurrentArmAngles);
}
else if (VulcanXHandle.m_haveLeftMPL)
{
// pass
}
}
// move MPL limb
if (VulcanXHandle.m_haveRightMPL)
{
VulcanXHandle.SetRightUpperArmAngles(GetRightUpperArmAngles());
VulcanXHandle.SetRightFingerAngles(GetRightFingerAngles());
}
else if (VulcanXHandle.m_haveLeftMPL)
{
VulcanXHandle.SetLeftUpperArmAngles(GetLeftUpperArmAngles());
VulcanXHandle.SetLeftFingerAngles(GetLeftFingerAngles());
}
}//function - Update
}//function - Start
#endregion //Start
#region Update
//-----------------------------------------------------------------------------
// UPDATED BY CHRISTOPHER HUNT <*****@*****.**>
void Update()
{
// Is called periodically for handling events
if (GetMovementState() == MOVEMENT_STATE_STOP)
{
f_TargetFingerAngles = GetRightFingerAngles();
}
else if (GetMovementState() == MOVEMENT_STATE_HAND_OPEN)
{
f_TargetFingerAngles = f_HandOpenFingerAngles;
}
else if (GetMovementState() == MOVEMENT_STATE_CYLINDER_GRASP)
{
f_TargetFingerAngles = f_CylinderFingerAngles;
}
else if (GetMovementState() == MOVEMENT_STATE_TRIPOD_GRASP)
{
f_TargetFingerAngles = f_TripodFingerAngles;
}
else if (GetMovementState() == MOVEMENT_STATE_KEY_GRASP)
{
f_TargetFingerAngles = f_KeyFingerAngles;
}
else if (GetMovementState() == MOVEMENT_STATE_PINCH_GRASP)
{
f_TargetFingerAngles = f_PinchOpenFingerAngles;
}
else if (GetMovementState() == MOVEMENT_STATE_POWER_GRASP)
{
f_TargetFingerAngles = f_PowerFingerAngles;
}
// if (GetMovementState() == MOVEMENT_STATE_STOP)
// {
// }
// else if (GetMovementState() == MOVEMENT_STATE_CLENCH)
// {
// float f_durationClench = 2f;
// float f_OscillatingValueClench = Mathf.PingPong(Time.time, f_durationClench) / f_durationClench;
// float f_FingerExtent = 60f;
// float[] f_RightFingerAnglesClench = new float[20] { 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent };
// for (ii = 0; ii < f_HomeFingerAngles.Length; ii++)
// {
// f_RightFingerAnglesClench[ii] = f_RightFingerAnglesClench[ii] * f_OscillatingValueClench;
// }//for - traverse the finger joints
// //Send values to adjust commanded joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesClench);
// }
// else if (GetMovementState() == MOVEMENT_STATE_FINGER_ROLL)
// {
// float f_durationFingerRoll = 0.5f;
// float f_OscillatingValueIndex = Mathf.PingPong(Time.time, f_durationFingerRoll) / f_durationFingerRoll;
// float f_OscillatingValueMiddle = Mathf.PingPong(Time.time - 1 * f_durationFingerRoll / 3, f_durationFingerRoll) / f_durationFingerRoll;
// float f_OscillatingValueRing = Mathf.PingPong(Time.time - 2 * f_durationFingerRoll / 3, f_durationFingerRoll) / f_durationFingerRoll;
// float f_OscillatingValueLittle = Mathf.PingPong(Time.time - 3 * f_durationFingerRoll / 3, f_durationFingerRoll) / f_durationFingerRoll;
// float f_FingerExtent = 60f;
// float[] f_RightFingerAnglesRoll = new float[20] { 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent };
// for (ii = 0; ii < 4; ii++)
// {
// f_RightFingerAnglesRoll[ii] = f_RightFingerAnglesRoll[ii] * f_OscillatingValueIndex;
// }//for - traverse the finger joints
// for (ii = 4; ii < 8; ii++)
// {
// f_RightFingerAnglesRoll[ii] = f_RightFingerAnglesRoll[ii] * f_OscillatingValueMiddle;
// }//for - traverse the finger joints
// for (ii = 8; ii < 12; ii++)
// {
// f_RightFingerAnglesRoll[ii] = f_RightFingerAnglesRoll[ii] * f_OscillatingValueRing;
// }//for - traverse the finger joints
// for (ii = 12; ii < 16; ii++)
// {
// f_RightFingerAnglesRoll[ii] = f_RightFingerAnglesRoll[ii] * f_OscillatingValueLittle;
// }//for - traverse the finger joints
// for (ii = 16; ii < 20; ii++)
// {
// f_RightFingerAnglesRoll[ii] = f_RightFingerAnglesRoll[ii] * 0.30f;
// }//for - traverse the finger joints
// //Send values to adjust commanded joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesRoll);
// }//if - check movement type
// else if (GetMovementState() == MOVEMENT_STATE_WAVE)
// {
// float[] f_RightFingerAnglesWave = new float[20] { 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f, 0f };
// float[] f_RightArmJointAnglesWave = new float[7] { -20f, -40f, -90f, 115f, 0f, 0f, 0f };
// float f_durationWave = 1f;
// float f_OscillatingValueWave = (Mathf.PingPong(Time.time, f_durationWave) / f_durationWave) - 0.5f;
// f_RightArmJointAnglesWave[3] = f_RightArmJointAnglesWave[3] + f_OscillatingValueWave * 20f;
// // VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAnglesWave); //Passes an array of the upper 7 joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesWave);
// }//if - Wave
// else if (GetMovementState() == MOVEMENT_STATE_HAND_SHAKE)
// {
// float f_FingerExtent = 20f;
// float[] f_RightFingerAnglesHandShake = new float[20] { 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent * 1.2f, f_FingerExtent * 1.2f, f_FingerExtent*1.2f, f_FingerExtent*3, f_FingerExtent, f_FingerExtent, f_FingerExtent };
// float[] f_RightArmJointAnglesHandShake = new float[7] { 0f, 0f, 00f, 70f, 0f, 0f, 0f };
// float f_durationHandShake = 1f;
// float f_OscillatingValueHandShake = (Mathf.PingPong(Time.time, f_durationHandShake) / f_durationHandShake) - 0.5f;
// f_RightArmJointAnglesHandShake[3] = f_RightArmJointAnglesHandShake[3] + f_OscillatingValueHandShake * 20f;
// // VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAnglesHandShake); //Passes an array of the upper 7 joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesHandShake);
// }//if - Hand Shake
// else if (GetMovementState() == MOVEMENT_STATE_WAG_FINGER)
// {
// float f_FingerExtent = 60f;
// float[] f_RightFingerAnglesWagFinger = new float[20] { 0f, 0f, 0f, 0f, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent };
// float[] f_RightArmJointAnglesWagFinger = new float[7] { -20f, -40f, -90f, 115f, 0f, 0f, 0f };
// float f_durationWave = 0.5f;
// float f_OscillatingValueFingerWag = (Mathf.PingPong(Time.time, f_durationWave) / f_durationWave) - 0.5f;
// //Wagging Finger directly
// f_RightFingerAnglesWagFinger[0] = f_OscillatingValueFingerWag * 30f;
// f_RightArmJointAnglesWagFinger[5] = f_RightArmJointAnglesWagFinger[5] + f_OscillatingValueFingerWag * 20f;
// //Wagging the Wrist Deviator
// //f_RightArmJointAnglesWave[5] = f_OscillatingValueFingerWag * 30f;
// //Send values to adjust commanded joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesWagFinger);
// // VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAnglesWagFinger); //Passes an array of the upper 7 joint angles
// }//if - Wag Finger
// else if (GetMovementState() == MOVEMENT_STATE_FIST_BUMP)
// {
// float f_FingerExtent = 65f;
// float[] f_RightFingerAnglesFistBump = new float[20] { 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent };
// float[] f_RightArmJointAnglesFistBump = new float[7] { 0f, 0f, 0f, 100f, 90f, 0f, 20f };
// float f_durationFistBump = 1f;
// float f_OscillatingValueFistBump = (Mathf.PingPong(Time.time, f_durationFistBump) / f_durationFistBump) - 0.75f;
// f_RightArmJointAnglesFistBump[0] = f_RightArmJointAnglesFistBump[0] + f_OscillatingValueFistBump * 20f;
// // VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAnglesFistBump); //Passes an array of the upper 7 joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesFistBump);
// }//if - Fist Bump
// else if (GetMovementState() == MOVEMENT_STATE_SPHERE_GRASP)
// {
// float f_durationClench = 2f;
// stateTimer += Time.deltaTime;
// // float f_OscillatingValueClench = Mathf.Min(stateTimer / f_durationClench,1);
// float[] f_RightArmJointAnglesSphereGrasp = new float[7] { 0f, 0f, 00f, 100f, 130f, 0f, 0f };
// float f_OscillatingValueClench = Mathf.PingPong(Time.time, f_durationClench) / f_durationClench;
// float f_FingerExtent = 60f;
// float[] f_RightFingerAnglesClench = new float[20] { 50f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 50f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 50f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 50f, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent+30, f_FingerExtent- 30, f_FingerExtent- 30, f_FingerExtent-30};
// for (ii = 0; ii < f_HomeFingerAngles.Length; ii++)
// {
// f_RightFingerAnglesClench[ii] = f_RightFingerAnglesClench[ii] * f_OscillatingValueClench;
// }//for - traverse the finger joints
// //Send values to adjust commanded joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesClench);
// // VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAnglesSphereGrasp);
// } //if - Sphere Grasp
// else if (GetMovementState() == MOVEMENT_STATE_CYLINDER_GRASP)
// {
// float f_durationClench = 2f;
// stateTimer += Time.deltaTime;
// //float f_OscillatingValueClench = Mathf.Min(stateTimer / f_durationClench, 1);
// float[] f_RightArmJointAnglesCylinderGrasp = new float[7] { 0f, 0f, 00f, 100f, 25f, 0f, 0f };
// float f_OscillatingValueClench = Mathf.PingPong(Time.time, f_durationClench) / f_durationClench;
// float f_FingerExtent = 50f;
// float[] f_RightFingerAnglesClench = new float[20] { 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, 0f, f_FingerExtent, f_FingerExtent, f_FingerExtent, f_FingerExtent + 40, f_FingerExtent - 20, f_FingerExtent - 20, f_FingerExtent - 20 };
// /*
// for (ii = 0; ii < f_HomeFingerAngles.Length; ii++)
// {
// f_RightFingerAnglesClench[ii] = f_RightFingerAnglesClench[ii] * f_OscillatingValueClench;
// }//for - traverse the finger joints
// */
// //Send values to adjust commanded joint angles
// VulcanXHandle.SetRightFingerAngles(f_RightFingerAnglesClench);
// // VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAnglesCylinderGrasp);
// } //if - Cylinder Grasp
if (VulcanXHandle.m_haveRightMPL)
{
// set target angles
SetRightFingerAngles(f_TargetFingerAngles);
// move upper arm angles for right MPL
float[] f_RightArmJointAngles = new float[7] {
ms_rightShoulderFE, ms_rightShoulderAA, ms_rightHumeralRot,
ms_rightElbowFE, ms_rightWristRot, ms_rightWristDev, ms_rightWristFE
};
VulcanXHandle.SetRightUpperArmAngles(f_RightArmJointAngles);
// move finger angles for right MPL
float[] f_RightFingerJointAngles = new float[20] {
ms_rightIndexAA, ms_rightIndexMCP, ms_rightIndexPIP, ms_rightIndexDIP,
ms_rightMiddleAA, ms_rightMiddleMCP, ms_rightMiddlePIP, ms_rightMiddleDIP,
ms_rightRingAA, ms_rightRingMCP, ms_rightRingPIP, ms_rightRingDIP,
ms_rightLittleAA, ms_rightLittleMCP, ms_rightLittlePIP, ms_rightLittleDIP,
ms_rightThumbAA, ms_rightThumbFE, ms_rightThumbMCP, ms_rightThumbDIP
};
VulcanXHandle.SetRightFingerAngles(f_RightFingerJointAngles);
}
else if (VulcanXHandle.m_haveLeftMPL)
{
// pass
}
}//function - Update