// ---------------------------------- All controls ------------------------------
void Update()
{
// Menu activation
if (Input.GetKeyDown(KeyCode.Escape))
{
menu.SetActive(true);
}
// Robot values
if (!robot)
{
return;
}
output.text = "Angles: \n";
for (int artIndex = robot.GetArticulations().Length - 1; artIndex >= 0; artIndex--)
{
output.text += robot.GetArticulations()[artIndex].Angle() + "\n";
}
output.text += "Efector pos: \n" + robot.GetE().position + "\n";
//robot.articulations[3].UpdateAngleAsGlobal(new Vector3(0f, 0f, 45f));
/*
* output.text = "Efector pos: \n" + robot.GetE().position + "\n";
*
* output.text += robot.GetArticulations()[3].Angle() + "\n";
* output.text += robot.GetArticulations()[3].transform.rotation.eulerAngles + "\n";
*/
}
public void MoveC(IK robot, Transform finalPoint, Transform middlePoint)
{
List <Transform> list = new List <Transform>();
list.Add(robot.GetE());
list.Add(middlePoint);
list.Add(finalPoint);
Vector3[] positions = spline.PrepareSpline(list);
List <Vector3> points = spline.GetTrayectory(positions);
Transform[] trayectory = new Transform[points.Count - 1]; // skip first one, Efector
for (int i = 0; i < trayectory.Length - 1; i++) // Last one is target, skip
{
Transform transf = new GameObject().transform;
transf.position = points[i + 1];
trayectory[i] = transf;
}
trayectory[trayectory.Length - 1] = finalPoint;
robot.CCDAlg(trayectory, false);
//
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("move" + " " + finalPoint.GetComponent <TargetModel>().GetName() + " " + middlePoint.GetComponent <TargetModel>().GetName());
}
}
public void MoveL(IK robot, Transform target)
{
Vector3 a = robot.GetE().position;
Vector3 b = target.position;
float resolution = 0.1f; // Line quality, small=high
int loops = Mathf.FloorToInt(1f / resolution);
// Linear trayectory
Transform[] trayectory = new Transform[loops];
for (int i = 1; i < loops; i++) // Last one is target, skip
{
Transform transf = new GameObject().transform;
float t = i * resolution;
//transf.position = Vector3.Lerp(robot.E.position, target.position, t);
transf.position = Vector3.Lerp(a, b, t);
trayectory[i - 1] = transf;
}
trayectory[loops - 1] = target;
robot.CCDAlg(trayectory, false);
//
if (gameController.GetOnlineMode())
{
controller.RunCommandOnline("movel" + " " + target.GetComponent <TargetModel>().GetName());
}
}
/**
* Mueve el Scorbot a una posición ya definida. El movimiento es una línea recta.
* @param robot Scorbot
* @param target Posición (objeto)
* @return void
*/
public void MoveL(IK robot, Transform target)
{
// If target with invalid data
if (!target.GetComponent <TargetModel>().GetValid())
{
stateMessageControl.WriteMessage("Error. MOVEL Unreachable position \"" + target.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
// Scorbot end effector
Vector3 a = robot.GetE().position;
// Position coordinates
Vector3 b = target.position;
float resolution = 0.1f; // Line quality, small=high quality
int loops = Mathf.FloorToInt(1f / resolution);
// Linear trajectory
Transform[] trajectory = new Transform[loops];
for (int i = 1; i < loops; i++) // Last one is target, skip
{
// New object
Transform transf = new GameObject().transform;
float t = i * resolution;
// Modify coordinates. Interpolation from a to b
transf.position = Vector3.Lerp(a, b, t);
// Add to trajectory
trajectory[i - 1] = transf;
}
// Add target to trajectory
trajectory[loops - 1] = target;
// Move Scorbot following trajectory. It uses "speedl"
robot.CCDAlg(trajectory, false);
stateMessageControl.WriteMessage("Done. MOVEL \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
// Online mode
if (gameController.GetOnlineMode())
{
bool done = controller.RunCommandUIOnline("movel", target.GetComponent <TargetModel>().GetName());
if (done)
{
string aux = "";
if (!target.GetComponent <TargetModel>().GetSync())
{
aux = ". NO SYNC";
}
stateMessageControl.WriteMessage("Done. Online MOVEL \"" + target.GetComponent <TargetModel>().GetName() + "\"" + aux, done);
}
else
{
stateMessageControl.WriteMessage("Error. Online MOVEL \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
void Update()
{
// After initialization, only once
if (!loadedEffector)
{
// New end effector values
effectorFileControl.Load(scorbots);
loadedEffector = true;
// Only one scorbot should be active
foreach (IK scorbot in scorbots)
{
if (scorbot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERIX.INDEX)
{
scorbot.gameObject.SetActive(true);
}
else
{
scorbot.gameObject.SetActive(false);
}
}
}
// Menu activation
if (menu.activeSelf)
{
if (Input.GetKeyDown(KeyCode.Escape))
{
menu.SetActive(false);
cameraControl.SetIsProcessing(true);
}
}
else
{
if (Input.GetKeyDown(KeyCode.Escape))
{
MainMenu();
}
}
// Scorbot values
if (!robot || !robot.GetE())
{
return;
}
// Scorbot end effector
Vector3 pos = new Vector3(robot.GetE().position.x, robot.GetE().position.z, robot.GetE().position.y);
pos = pos * 10f;
output.text = "Efector pos: \n" + "X: " +
pos.x.ToString(NUMBER_FORMAT) + "\nY: " + pos.y.ToString(NUMBER_FORMAT) + "\nZ: " + pos.z.ToString(NUMBER_FORMAT) + "\n";
DrawTrayectory();
}
public void Here(IK robot, Transform target)
{
if (gameController.GetOnlineMode())
{
// 2 options. use pos from robot (simulation) or real robot
if (isHereFromSimulation)
{
// TEST????
// Copy angles from robot
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
// Get pos and p, r from simulation. Do teach to real Scorbot
List <float> xyzpr = new List <float>()
{
target.position.x, target.position.z, target.position.y,
target.GetComponent <TargetModel>().GetPitch(), -target.GetComponent <TargetModel>().GetRoll()
};
controller.Connection.GetComponent <SerialController>().WriteToControllerTeach(target.GetComponent <TargetModel>().name, xyzpr);
//Teach(robot, target, pos, posPitchRoll[3], posPitchRoll[4]);
stateOutput.text = "Success Here";
}
else // From real Scorbot
{
List <String[]> listString = new List <string[]>();
// This stops main thread
//controller.RunCommandHereOnline(target.GetComponent<TargetModel>().GetName());
//listString = controller.RunCommandListpvOnline(target.GetComponent<TargetModel>().GetName());
string result = "";
Regex rx = new Regex("^.:(.+?)$");
List <int> counts = new List <int>();
List <float> posPitchRoll = new List <float>();
foreach (String[] a in listString)
{
//aux += a.Length.ToString();
foreach (string b in a)
{
//Debug.Log(b);
MatchCollection matches = rx.Matches(b);
foreach (Match match in matches)
{
GroupCollection groups = match.Groups;
result += groups[1].Value + "?";
posPitchRoll.Add(float.Parse(groups[1].Value));
}
}
}
for (int i = 0; i < 5; i++)
{
counts.Add((int)posPitchRoll[0]);
posPitchRoll.RemoveAt(0);
}
result += posPitchRoll.Count + " " + counts.Count + " " + counts[0] + " " + posPitchRoll[0];
stateOutput.text = result;
// Just set robot to angles of real robot, then teach? with POS PITCH ROLL recovered (use target)
//target.GetComponent<TargetModel>().SetAngles(robot.transform.GetComponent<ScorbotModel>().CountsToAngles(counts));
//target.position = robot.GetPosFromCounts(counts);
// 18 19.8 46.9
// Or do teach to get same pos
Teach(robot, target, new Vector3(posPitchRoll[0], posPitchRoll[1], posPitchRoll[2]), posPitchRoll[3], posPitchRoll[4], false);
}
}
else // Offline
{
// Copy angles from robot
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
//List<int> counts = new List<int>() { -13541, -22691, -3489, 56937, 27};
//target.position = robot.GetPosFromCounts(counts);
//stateOutput.text = robot.GetPosFromCounts(counts).ToString();
}
}
/**
* Mueve o rota un objeto dependiento del objeto seleccionado. Si es un eje se produce un movimiento
* con ayuda de los cambios del ratón, si es un eje de rotación se aplica una rotación con ayuda
* de los cambios del ratón.
* @return void
*/
private void MoveOrRotate()
{
// Parent from selected object. An object can contain axis and a selectable object,
// these components are contained in a parent object, so to move everything we just move
// the parent since its children will automatically follow.
Transform parent = selectedObject.transform.parent.transform;
// Selected object is an Axis
if (selectedObject.tag.Contains(AXIS_TAG))
{
Vector3 startPos = parent.position;
// Selected object is an Axis X
if (selectedObject.tag.Contains(X_TAG))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.right, parent.right);
// Move object around axis x, angle determines forward or backward
if (angle < 90f)
{
parent.Translate(new Vector3(axisSensibityReduction * axisSensibity *
Input.GetAxis(MOUSE_X), 0f, 0f));
}
else
{
parent.Translate(new Vector3(axisSensibityReduction * -axisSensibity *
Input.GetAxis(MOUSE_X), 0f, 0f));
}
}
// Selected object is an Axis Y
if (selectedObject.tag.Contains(Y_TAG))
{
// Move object around axis y
parent.Translate(new Vector3(0f, axisSensibityReduction * axisSensibity *
Input.GetAxis(MOUSE_Y), 0f));
}
// Selected object is an Axis Z
if (selectedObject.tag.Contains(Z_TAG))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.right, parent.forward);
// Move object around axis z, angle determines forward or backward
if (angle < 90f)
{
parent.Translate(new Vector3(0f, 0f, axisSensibityReduction * axisSensibity *
Input.GetAxis(MOUSE_X)));
}
else
{
parent.Translate(new Vector3(0f, 0f, axisSensibityReduction * -axisSensibity *
Input.GetAxis(MOUSE_X)));
}
}
// Target (a position) moved. Invalid angles
if (selectedObject.tag.Contains(TARGET_TAG) && (!startPos.Equals(parent.position)))
{
// If it is not a being used by another position (relative), this position not sync anymore
if (parent.GetComponent <TargetModel>().GetRelativeTo() == null)
{
Transform relativePosition = parent.GetComponent <TargetModel>().GetRelativeFrom();
parent.GetComponent <TargetModel>().SetSync(false);
// if this position is being used by another position (relative), that position is not sync anymore
;
if (parent.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);
}
}
// Check if it's an unreachable point
if (robot.TargetInRange(parent))
{
// Reachable. Load data to target
parent.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
parent.GetComponent <TargetModel>().SetValid(true);
}
else // Unreachable
{
parent.GetComponent <TargetModel>().SetValid(false);
}
stateMessageControl.UpdatePositionLog();
}
else // Moved a relative position, revert position
{
parent.GetComponent <TargetModel>().UpdateRelativePosition();
}
}
// Update trayectory in case a position is moved
gameController.DrawTrayectory();
}
// Selected object is a rotation axis
if (selectedObject.tag.Contains(ROTATION_TAG))
{
// Scorbot articulation
Articulation art = parent.GetComponent <Articulation>();
// If selected object parent is an articulation
if (art != null)
{
// If articulation plane is xz, apply rotation
if (art.GetPlane().Equals(PlaneHelper.XZ))
{
parent.GetComponent <Articulation>().Rotate(-rotationSensibity * Input.GetAxis(MOUSE_X));
}
// If articulation plane is xy, apply rotation
if (art.GetPlane().Equals(PlaneHelper.XY))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.forward, parent.forward);
// Rotate object around plane xy, angle determines forward or backward
if (angle < 90f)
{
parent.GetComponent <Articulation>().Rotate(-rotationSensibity * Input.GetAxis(MOUSE_X));
}
else
{
parent.GetComponent <Articulation>().Rotate(rotationSensibity * Input.GetAxis(MOUSE_X));
}
}
// If articulation plane is yz, apply rotation
if (art.GetPlane().Equals(PlaneHelper.YZ))
{
// Angle between camera and object
float angle = Vector3.Angle(cam.forward, parent.right);
// Rotate object around plane yz, angle determines forward or backward
if (angle < 90f)
{
parent.GetComponent <Articulation>().Rotate(-rotationSensibity * Input.GetAxis(MOUSE_X));
}
else
{
parent.GetComponent <Articulation>().Rotate(rotationSensibity * Input.GetAxis(MOUSE_X));
}
}
}
}
// If selected object is Scorbot InnerAxis (InnerTarget contains InnerAxis)
if (selectedObject.tag.Contains(INNERAXIS_TAG))
{
// Move Scorbot to InnerTarget position
robot.CCDAlg(innerTarget.transform);
}
else // Update InnerTarget position to Scorbot end effector position when is not selected
{
innerTarget.transform.parent.transform.position = robot.GetE().position;
}
}
/**
* 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);
}
}
/**
* Mueve el Scorbot a una posición ya definida pasando por otra posición. El movimiento es una curva generada
* por el algoritmo del spline de CatmullRom.
* @param robot Scorbot
* @param finalPoint Posición final(objeto)
* @param middlePoint Posición intermedia (objeto)
* @return void
*/
public void MoveC(IK robot, Transform finalPoint, Transform middlePoint)
{
// Valid final position
if (!finalPoint.GetComponent <TargetModel>().GetValid())
{
stateMessageControl.WriteMessage("Error. MOVEC Unreachable position \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
// Valid intermediate position
if (!middlePoint.GetComponent <TargetModel>().GetValid())
{
stateMessageControl.WriteMessage("Error. MOVEC Unreachable position \"" + middlePoint.GetComponent <TargetModel>().GetName() + "\"", false);
return;
}
List <Transform> list = new List <Transform>();
// Scorbot final effector
list.Add(robot.GetE());
// Intermadiate position
list.Add(middlePoint);
// Final position
list.Add(finalPoint);
// Add 2 control points
Vector3[] positions = spline.PrepareSpline(list);
// Get final trajectory
List <Vector3> points = spline.GetTrayectory(positions);
// Build objects from final trajectory
Transform[] trayectory = new Transform[points.Count - 1]; // skip first one, Effector
for (int i = 0; i < trayectory.Length - 1; i++) // Last one is target, skip
{
Transform transf = new GameObject().transform;
transf.position = points[i + 1];
trayectory[i] = transf;
}
trayectory[trayectory.Length - 1] = finalPoint;
// Move Scorbot following trajectory
robot.CCDAlg(trayectory, false);
stateMessageControl.WriteMessage("Done. MOVEC \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"" +
" \"" + middlePoint.GetComponent <TargetModel>().GetName() + "\"", true);
// Online mode
if (gameController.GetOnlineMode())
{
bool done = controller.RunCommandUIOnline("movec", finalPoint.GetComponent <TargetModel>().GetName(), middlePoint.GetComponent <TargetModel>().GetName());
if (done)
{
string aux = "";
if (!finalPoint.GetComponent <TargetModel>().GetSync() || !middlePoint.GetComponent <TargetModel>().GetSync())
{
aux = ". NO SYNC";
}
stateMessageControl.WriteMessage("Done. Online MOVEC \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"" +
" \"" + middlePoint.GetComponent <TargetModel>().GetName() + aux, done);
}
else
{
stateMessageControl.WriteMessage("Error. Online MOVEC \"" + finalPoint.GetComponent <TargetModel>().GetName() + "\"" +
" \"" + middlePoint.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
