/**
* Carga las coordenadas x y z del efector final de un Scorbot. Contexto del Scorbot real.
* La carga se hace mediante fichero o se usa el que hay por defecto en el programa.
* @param file Fichero
* @param scorbot Scorbot
* @return void
*/
private void LoadEffector(string file, IK scorbot)
{
try
{
// Inicial position. HOME
scorbot.GetComponent <ScorbotModel>().GoHome();
List <float[]> values = new List <float[]>();
// Read file
string[] lines = System.IO.File.ReadAllLines(file);
// Effector values. First line
string[] effectorPos = lines[0].Split(' ');
// Expected coordinates x, y, z
if (effectorPos.Length == 3)
{
Vector3 pos = (new Vector3(float.Parse(effectorPos[0]), float.Parse(effectorPos[2]), float.Parse(effectorPos[1])) / 10f);
scorbot.GetComponent <ScorbotModel>().E.position = pos;
// Updating copy end effector
scorbot.UpdateCopyEffector();
stateMessageControl.WriteMessage("Done. Effector values loaded (From file). " + scorbot.name, true);
}
else // Error
{
stateMessageControl.WriteMessage("Error. Effector values not valid (From file). " + scorbot.name, false);
}
}
catch (Exception e) // File not found
{
// Default effector
Vector3 effectorPos = scorbot.GetComponent <ScorbotModel>().E.position * 10f;
// Oneline
string[] lines = new string[] {
effectorPos.x.ToString(NUMBER_FORMAT) + " " +
effectorPos.z.ToString(NUMBER_FORMAT) + " " +
effectorPos.y.ToString(NUMBER_FORMAT)
};
// Overwrite everything
System.IO.File.WriteAllLines(file, lines);
stateMessageControl.WriteMessage("Done. Default effector (Not from file)." + scorbot.name, true);
}
}
void Update()
{
// Activate/deactivate keyboard control
if (!processing)
{
return;
}
if (!robot)
{
return;
}
// Scorbot articulation rotation (See keyCodes at "Start()")
int j = 0;
for (int i = 0; i < robot.GetArticulations().Length; i++)
{
if (Input.GetKey(keyCodes[j]))
{
robot.GetArticulations()[i].Rotate(-ROTATION_SENSIBILITY);
}
j++;
if (Input.GetKey(keyCodes[j]))
{
robot.GetArticulations()[i].Rotate(ROTATION_SENSIBILITY);
}
j++;
}
// Open/close Scorbot end efector
if (Input.GetKey(KeyCode.Y))
{
robot.GetComponent <ScorbotModel>().Open();
}
if (Input.GetKey(KeyCode.Alpha6))
{
robot.GetComponent <ScorbotModel>().Close();
}
}
/**
* Recupera los valores de una posición del Scorbot real en la misma posición en la simulación.
* @param robot Scorbot
* @param target Posición (objeto)
* @return bool Éxito
*/
public bool SyncScorbotToSimulation(IK robot, Transform target)
{
// Only online mode
List <String[]> listString = new List <string[]>();
List <int> counts;
List <float> posPitchRoll;
// This stops main thread
bool listpv = Listpv(target, out counts, out posPitchRoll);
if (listpv)
{
stateMessageControl.WriteMessage("Done. Online SYNC(LISTPV) \"" + target.GetComponent <TargetModel>().GetName() + "\"", listpv);
}
else
{
stateMessageControl.WriteMessage("Error. Online SYNC(LISTPV) \"" + target.GetComponent <TargetModel>().GetName() + "\"", listpv);
return(false);
}
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
for (int i = 0; i < posPitchRoll.Count; i++)
{
posPitchRoll[i] = posPitchRoll[i] / 10f;
}
}
// Do teach only in simulation
bool done = Teach(robot, target, new Vector3(posPitchRoll[0], posPitchRoll[1], posPitchRoll[2]), posPitchRoll[3], posPitchRoll[4], false);
if (done)
{
stateMessageControl.WriteMessage("Done. Online SYNC \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
if (target.GetComponent <TargetModel>().GetRelativeFrom())
{
target.GetComponent <TargetModel>().GetRelativeFrom().GetComponent <TargetModel>().SetSync(true);
}
stateMessageControl.UpdatePositionLog();
return(true);
}
else
{
stateMessageControl.WriteMessage("Error. Online SYNC \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
return(false);
}
}
public void Open()
{
robot.GetComponent <GripScorbotERIX>().Open();
}
/**
* Abre la pinza del Scorbot.
* @return void
*/
public void Open()
{
robot.GetComponent <ScorbotModel>().Open();
}
/**
* Modifica una posición sumando valores los cuales son una posición (x, y,z), inclinación frontal (p) e
* inclinación lateral (r). El valor puede ser solo uno de los anteriores y debe estar en el contexto del
* Scorbot real. byIndex: X, Y, Z, P, R (0..4)
* @param robot Scorbot
* @param target Posición (objeto)
* @param byIndex Índice del tipo de parámetro
* @param value Valor
* @return void
*/
public void Shiftc(IK robot, Transform target, int byIndex, float value)
{
// Offline mode
string[] parameterName = { "x", "y", "z", "p", "r" };
// If is a relative position, error
if (target.GetComponent <TargetModel>().GetRelativeTo() != null)
{
stateMessageControl.WriteMessage("Error. SHIFTC \"" + target.GetComponent <TargetModel>().GetName() + "\" " +
parameterName[byIndex] + " by " + value + ". Relative position", false);
return;
}
// Values from position in real Scorbot context
Vector3 pos = target.GetComponent <TargetModel>().GetPositionInScorbot();
float p = target.GetComponent <TargetModel>().GetPitch();
float r = target.GetComponent <TargetModel>().GetRoll();
// Apply value to corresponding parameter
Vector3 offsetPos = Vector3.zero;
// byIndex: X, Y, Z, P, R (0..4)
switch (byIndex)
{
case 0:
offsetPos = new Vector3(value, 0f, 0f);
break;
case 1:
offsetPos = new Vector3(0f, value, 0f);
break;
case 2:
offsetPos = new Vector3(0f, 0f, value);
break;
case 3:
p += value;
break;
case 4:
r += value;
break;
}
pos += offsetPos;
bool done = Teach(robot, target, pos, p, r, false);
if (done)
{
stateMessageControl.WriteMessage("Done. SHIFTC \"" + target.GetComponent <TargetModel>().GetName() + "\" " +
parameterName[byIndex] + " by " + value, done);
}
else
{
stateMessageControl.WriteMessage("Error. SHIFTC \"" + target.GetComponent <TargetModel>().GetName() + "\" " +
parameterName[byIndex] + " by " + value, done);
}
// Online mode
if (gameController.GetOnlineMode())
{
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
done = controller.RunCommandUIShiftc(target.GetComponent <TargetModel>().GetName(), parameterName[byIndex],
(value * 10f).ToString()); // Number format?
}
else
{
done = controller.RunCommandUIShiftc(target.GetComponent <TargetModel>().GetName(), parameterName[byIndex],
value.ToString()); // Number format?
}
if (done)
{
stateMessageControl.WriteMessage("Done. Online SHIFTC \"" + target.GetComponent <TargetModel>().GetName() + "\" " +
parameterName[byIndex] + " by " + value, done);
target.GetComponent <TargetModel>().SetSync(true);
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online SHIFTC \"" + target.GetComponent <TargetModel>().GetName() + "\" " +
parameterName[byIndex] + " by " + value, done);
}
}
}
/**
* Modifica una posición con los valores actuales del Scorbot. Modifica una posición en el controlador con los
* valores actuales del Scorbot real o el de la simulación. En modo "From simulation" el Scorbot es el de
* la simulación, mientras que en modo "From Scorbot" es el Scorbot real. En modo "From simulation" se ejecuta
* el comando "Teach" (online) con los valores del Scorbot de la simulación. En modo "From Scorbot" se ejecuta
* el comando "Here" (online) en el Scorbot real, seguidamente de "Listpv" (online) para recuperar los datos
* del "Here" y se realiza un "Teach" (Offline) para cargar esos datos.
* @param robot Scorbot
* @param target Posición (objeto)
* @return void
*/
public void Here(IK robot, Transform target)
{
// Online mode
if (gameController.GetOnlineMode())
{
// 2 options. Use pos from simulation or real Scorbot
if (isHereFromSimulation) // Here. Mode "From simulation"
{
// Copy angles from simulation into the position (object)
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
stateMessageControl.WriteMessage("Done. HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.UpdatePositionLog();
// Get pos, p, r from simulation. Do teach to real Scorbot
float multPos = 10f;
float multDegrees = 1f;
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
multPos = 100f;
multDegrees = 10f;
}
List <float> xyzpr = new List <float>()
{
target.position.x *multPos, target.position.z *multPos, target.position.y *multPos,
target.GetComponent <TargetModel>().GetPitch() * multDegrees, target.GetComponent <TargetModel>().GetRoll() * multDegrees
};
bool done = controller.RunCommandUITeach(target.GetComponent <TargetModel>().GetName(), xyzpr);
if (done)
{
stateMessageControl.WriteMessage("Done. Online HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
else // Here. Mode "From real Scorbot"
{
// Real scorbot here
bool here = controller.RunCommandUIOnline("here", target.GetComponent <TargetModel>().GetName());
if (here)
{
stateMessageControl.WriteMessage("Done. Online HERE(HERE) \"" + target.GetComponent <TargetModel>().GetName() + "\"", here);
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online HERE(HERE) \"" + target.GetComponent <TargetModel>().GetName() + "\"", here);
}
// Get data from real Scorbot into the position (object)
Thread.Sleep(200);
bool done = SyncScorbotToSimulation(robot, target);
if (done)
{
stateMessageControl.WriteMessage("Done. Online HERE(SYNC) \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
else
{
stateMessageControl.WriteMessage("Error. Online HERE(SYNC) \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
else // Offline mode
{
// Copy angles from simulation into the position (object)
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.UpdatePositionLog();
stateMessageControl.WriteMessage("Done. HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
}
}
/**
* Define una posición relativa a otra posición con nuevos valores los cuales son una posición (x, y, z),
* inclinación frontal e inclinación lateral. La posición permanecerá relativa. Los valores deben estar
* en el contexto del Scorbot real.
* @param robot Scorbot
* @param target Posición (objeto)
* @param relativeToTarget Posición (objeto) relativa a usar como referencia
* @param pos Coordenadas relativas
* @param p Pitch relativo
* @param r Roll relativo
* @param online Ejecutar modo online
* @param offline Ejecutar modo offline
* @return bool Éxito
*/
public bool TeachR(IK robot, Transform target, Transform relativeToTarget, Vector3 pos, float p, float r, bool online = true, bool offline = true)
{
// Offline mode
if (offline)
{
// New relative position
Vector3 newPos = (new Vector3(relativeToTarget.position.x, relativeToTarget.position.z, relativeToTarget.position.y) * 10f)
+ pos;
// Apply new relative values
bool teachDone = Teach(robot, target, newPos, relativeToTarget.GetComponent <TargetModel>().GetPitch() + p,
relativeToTarget.GetComponent <TargetModel>().GetRoll() + r, false);
if (teachDone)
{
stateMessageControl.WriteMessage("Done. TEACHR \"" + target.GetComponent <TargetModel>().GetName() + "\" \"" +
relativeToTarget.GetComponent <TargetModel>().GetName() + "\"", teachDone);
}
else
{
stateMessageControl.WriteMessage("Error. TEACHR \"" + target.GetComponent <TargetModel>().GetName() + "\" \"" +
relativeToTarget.GetComponent <TargetModel>().GetName() + "\"", teachDone);
return(false);
}
// Activate automatic values update
target.GetComponent <TargetModel>().SetRelativeTo(relativeToTarget, new Vector3(pos.x, pos.z, pos.y) / 10f, p, r);
}
// Online mode
if (gameController.GetOnlineMode() && online)
{
// Build data to send
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
pos = pos * 10f;
p = p * 10f;
r = r * 10f;
}
List <float> xyzpr = new List <float>()
{
pos.x, pos.y, pos.z, p, r
};
bool done = controller.RunCommandUITeachr(target.GetComponent <TargetModel>().GetName(),
relativeToTarget.GetComponent <TargetModel>().GetName(), xyzpr);
if (done)
{
stateMessageControl.WriteMessage("Done. Online TEACHR \"" + target.GetComponent <TargetModel>().GetName() + "\" \"" +
relativeToTarget.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online TEACHR \"" + target.GetComponent <TargetModel>().GetName() + "\" \"" +
relativeToTarget.GetComponent <TargetModel>().GetName() + "\"", done);
return(false);
}
}
return(true);
}
/**
* Modifica una posición con nuevos valores los cuales son una posición (x, y, z), inclinación frontal e
* inclinación lateral. Estos valores deben entar en el contexto del Scorbot real.
* @param robot Scorbot
* @param target Posición (objeto)
* @param pos Coordenadas
* @param p Pitch
* @param r Roll
* @param online Ejecutar modo online
* @param offline Ejecutar modo offline
* @return bool Éxito
*/
public bool Teach(IK robot, Transform target, Vector3 pos, float p, float r, bool online = true, bool offline = true)
{
Vector3 posReal = new Vector3(pos.x, pos.y, pos.z);
// Offline mode
if (offline)
{
// mm to cm. Interchange y and z. Position in simulation
pos = new Vector3(pos.x / 10f, pos.z / 10f, pos.y / 10f);
// Copy initial values
Vector3 startPos = target.position;
Vector3 startPitch = target.GetComponent <TargetModel>().GetAngles()[3];
Vector3 startRoll = target.GetComponent <TargetModel>().GetAngles()[4];
// Apply pitch and roll to target
target.GetComponent <TargetModel>().GetAngles()[3] = robot.GetArticulations()[3].BuiltAngle(p);
float auxR = -r;
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
auxR = r;
}
target.GetComponent <TargetModel>().GetAngles()[4] = robot.GetArticulations()[4].BuiltAngle(auxR);
// Apply new coordinatesC
target.position = pos;
// Check if it's an unreachable point
if (!robot.TargetInRange(target, true))
{
stateMessageControl.WriteMessage("Error. TEACH Unreachable position \"" + target.GetComponent <TargetModel>().GetName() + "\"", false);
// Restore position (object) values
target.position = startPos;
target.GetComponent <TargetModel>().GetAngles()[3] = startPitch;
target.GetComponent <TargetModel>().GetAngles()[4] = startRoll;
return(false);
}
// Recover angles data. Apply to position (object)
target.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.WriteMessage("Done. TEACH \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
stateMessageControl.UpdatePositionLog();
// if this position is being used by another position (relative), that position is not sync anymore
Transform relativePosition = target.GetComponent <TargetModel>().GetRelativeFrom();
if (target.GetComponent <TargetModel>().GetRelativeFrom())
{
relativePosition.GetComponent <TargetModel>().SetSync(false);
// Updating relative position
relativePosition.GetComponent <TargetModel>().UpdateRelativePosition();
// Update angles data
if (robot.TargetInRange(relativePosition))
{
// Reachable. Load data to target
relativePosition.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
relativePosition.GetComponent <TargetModel>().SetValid(true);
}
else // Unreachable
{
relativePosition.GetComponent <TargetModel>().SetValid(false);
}
}
// This position is relative to another, teach destroys relativity
if (target.GetComponent <TargetModel>().GetRelativeTo())
{
target.GetComponent <TargetModel>().SetNoRelativeTo();
}
}
// Online mode
if (gameController.GetOnlineMode() && online)
{
// Build data to send
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
posReal = posReal * 10f;
p = p * 10f;
r = r * 10f;
}
List <float> xyzpr = new List <float>()
{
posReal.x, posReal.y, posReal.z, p, r
};
bool done = controller.RunCommandUITeach(target.GetComponent <TargetModel>().GetName(), xyzpr);
if (done)
{
stateMessageControl.WriteMessage("Done. Online TEACH \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
if (target.GetComponent <TargetModel>().GetRelativeFrom())
{
target.GetComponent <TargetModel>().GetRelativeFrom().GetComponent <TargetModel>().SetSync(true);
}
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online TEACH \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
return(false);
}
}
return(true);
}
