public BurtSharp.CoAP.MsgTypes.RobotCommand CalcForces()
{
Vector3 vel = _currentVelocity;
Vector3 pos = _currentPosition;
Vector3 vel_norm = vel.normalized;
float vel_mag = vel.magnitude;
Vector3 coreTorque = Vector3.zero;
//email felix
//this section is what we want
_teneoTorque0 = 1.0f;
//CalibTenoAngle = -Mathf.DeltaAngle(handangles[2], CalibAngle1) + 180.0f;
//relates torque linearly to difference in angle between player and some arbitrary constant
CalibTenoAngle = -Mathf.DeltaAngle(handangles[3], CalibAngle1) + 180.0f; //apply calibration to pronation/supination portion of handangles
if (CalibTenoAngle > 160 + 180)
{
//Debug.Log ("Teneo Limit high: " );
_teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 160 + 180) * Mathf.PI / 180), 0, 1);
//_teneoTorque = 0f;
}
if (CalibTenoAngle < 20 + 180)
{
//Debug.Log ("Teneo Limit low: " );
//Mathf.Cos((handangles[2]-180+90))
_teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 20 + 180) * Mathf.PI / 180), 0, 1);
//_teneoTorque = 0f;
}
_toolForceQ = _toolForce;
_teneoTorqueQ = _teneoTorque;
//we want to apply different viscosities
//positive viscosity F = -bx
//negative viscosity F = bx
//where b is the PSFactor
//x is the user torque/hand velocity
//need safety checks in place as well
_teneoTorque = _teneoTorque0
return(RobotCommandExtensions.RobotCommandForceTorque(_toolForce * MasterForce, coreTorque, _teneoTorque * MasterForce));
}
public BurtSharp.CoAP.MsgTypes.RobotCommand CalcForces()
{
Vector3 vel = _currentVelocity;
Vector3 pos = _currentPosition;
Vector3 vel_norm = vel.normalized;
float vel_mag = vel.magnitude;
Vector3 coreTorque = Vector3.zero;
//_teneoTorque0 = 1.0f;
CalibTenoAngle = -Mathf.DeltaAngle(handangles [2], CalibAngle1) + 180.0f;
if (CalibTenoAngle > 160 + 180)
{
//Debug.Log ("Teneo Limit high: " );
_teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 160 + 180) * Mathf.PI / 180), 0, 1);
//_teneoTorque = 0f;
}
if (CalibTenoAngle < 20 + 180)
{
//Debug.Log ("Teneo Limit low: " );
//Mathf.Cos((handangles[2]-180+90))
_teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 20 + 180) * Mathf.PI / 180), 0, 1);
//_teneoTorque = 0f;
}
_toolForceQ = _toolForce;
_teneoTorqueQ = _teneoTorque;
return(RobotCommandExtensions.RobotCommandForceTorque(_toolForce * MasterForce, coreTorque, _teneoTorque * MasterForce));
}
/// <summary>
/// This is the control loop that calculates the forces to be applied to the robot. It
/// runs automatically through RobotConnection, as long as it is registered. In this
/// example, this function is registered in the Start () function.
///
/// The RobotCommand type may include a force, a torque, both, or neither. Whichever of
/// these are set get sent to the robot.
/// </summary>
public BurtSharp.CoAP.MsgTypes.RobotCommand CalcForces()
{
/// Dividing the force by kPositionScale allows the gains to be independent of the value
/// of kPositionScale. If you add forces that are not related to haptic objects and are
/// independent of the scaling, you may need to divide only certain components of the
/// force by kPositionScale.
// Vector3 force = _toolForce / kPositionScale;
Vector3 vel = _currentVelocity;
Vector3 pos = _currentPosition;
Vector3 hand_to_target_vec = target_pos - pos;
Vector3 hand_to_target_vec_norm = hand_to_target_vec.normalized;
Vector3 vel_norm = vel.normalized;
float vel_mag = vel.magnitude;
//Vector3 plane_norm = Vector3.Cross(vel_norm, hand_to_target_vec_norm);
//Vector3 force_norm = Vector3.Cross(hand_to_target_vec_norm, plane_norm);
//float proj_angle = Mathf.Acos (Vector3.Dot(vel_norm, hand_to_target_vec_norm));
//float forcemag = 3.0f*vel_mag*Mathf.Sin(proj_angle);
//Vector3 force1 = forcemag*force_norm;
//***CREATE FORCE AND TORQUE USING EA***
//force increases linearly with distance from player game object to target
Vector3 force2 = -EA_gain * hand_to_target_vec;
//* Mathf.PI / 180.0f
//torque increases linearly with difference in angle between player game object and target
_teneoTorque0 = -EA_gain * 0.002f * Mathf.DeltaAngle(target_angles[2], handangles[2]);
//(0.0f*target_angles [2] + 1.0f*transform.eulerAngles[2] );
//Vector3 force = _robot.GetToolVelocity () *0 +2.0f;
//_toolForce =new Vector3 (10,0,0);
_toolForce = force2;
//Debug.Log (target_pos.ToString ());
Vector3 coreTorque = Vector3.zero;
//_teneoTorque0 = 1.0f;
CalibTenoAngle = -Mathf.DeltaAngle(handangles[2], CalibAngle1) + 180.0f;
if (CalibTenoAngle > 160 + 180)
{
//Debug.Log ("Teneo Limit high: " );
//do not apply greater torques when the difference of angle between the target and hand is greater than 160
_teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 160 + 180) * Mathf.PI / 180), 0, 1);
//_teneoTorque = 0f;
}
if (CalibTenoAngle < 20 + 180)
{
//Debug.Log ("Teneo Limit low: " );
//Mathf.Cos((handangles[2]-180+90))
//do not apply lesser torques when the difference of angle between the target and hand is less than 20
_teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 20 + 180) * Mathf.PI / 180), 0, 1);
//_teneoTorque = 0f;
}
//_teneoTorque = _teneoTorque0;
//UnityEngine.Debug.Log (_toolForce.ToString ());
_toolForceQ = _toolForce;
_teneoTorqueQ = _teneoTorque;
//_robot.SetUserGravityCompGain (value, _ugcUnits)
//_robot.SetUserGravityCompGain
return(RobotCommandExtensions.RobotCommandForceTorque(_toolForce * MasterForce, coreTorque, _teneoTorque * MasterForce));
}
