/**
* Comprueba si una posición (objeto) tiene un nombre válido y está en el alcance del Scorbot.
* @param targetName Nombre de la posición
* @param newTarget Posición (objeto)
* @param newPos Coordenadas
* @return bool Válido
*/
private bool ValidTarget(string targetName, Transform newTarget, Vector3 newPos)
{
// Check if it's a valid name
if (targetName.Equals("") || targetName == null)
{
stateMessageControl.WriteMessage("Error. Name required", false);
return(false);
}
if (!targetControl.ValidNameLength(targetName))
{
stateMessageControl.WriteMessage("Error. Name too long \"" + targetName + "\"", false);
return(false);
}
if (!targetControl.ValidName(targetName))
{
stateMessageControl.WriteMessage("Error. Name already in use \"" + targetName + "\"", false);
return(false);
}
newTarget.position = newPos;
// Check if it's an unreachable position
if (!robot.TargetInRange(newTarget.transform))
{
stateMessageControl.WriteMessage("Error. Unreachable position \"" + targetName + "\"", false);
return(false);
}
return(true);
}
public void RecordPosition()
{
Transform newTarget = Instantiate(targetPrefab).transform;
// No points recorded
if (targetControl.Count() == 0)
{
//ValidTarget(newTarget);
if (targetNameInput.text.Equals("") || targetNameInput.text == null)
{
stateOutput.text = "Name required";
Destroy(newTarget.gameObject);
return;
}
Vector3 recordedPos = defaultPosition;
newTarget.position = recordedPos;
// Check if it's an unreachable point
if (!robot.TargetInRange(newTarget.transform))
{
Destroy(newTarget.gameObject);
stateOutput.text = "Unreachable point";
return;
}
stateOutput.text = "OK";
// Add target
Transform addedTarget = targetControl.Add(targetNameInput.text, recordedPos, robot.GetAnglesFromCopy());
selectionControl.SetActiveAxis(addedTarget, true);
selectionControl.SelectedObject(addedTarget.gameObject);
SetTarget(addedTarget);
UpdateTargets(targetControl.GetNames());
targetNameInput.text = "";
//DrawTrayectory();
}
else // Already at least one point
{
GameObject prevSelectedObject = selectionControl.SearchContainTag("Target");
if (prevSelectedObject != null)
{
prevSelectedObject = prevSelectedObject.transform.parent.gameObject;
if (targetNameInput.text.Equals("") || targetNameInput.text == null)
{
stateOutput.text = "Name required";
Destroy(newTarget.gameObject);
return;
}
Vector3 recordedPos = prevSelectedObject.transform.position;
newTarget.position = recordedPos;
// Check if it's an unreachable point
if (!robot.TargetInRange(prevSelectedObject.transform))
{
Destroy(newTarget.gameObject);
stateOutput.text = "Unreachable point";
return;
}
stateOutput.text = "OK";
// Add target
Transform addedTarget = targetControl.Add(targetNameInput.text, recordedPos, robot.GetAnglesFromCopy());
selectionControl.SetActiveAxis(addedTarget, false);
selectionControl.SelectedObject(prevSelectedObject);
UpdateTargets(targetControl.GetNames());
targetNameInput.text = "";
//DrawTrayectory();
}
}
Destroy(newTarget.gameObject);
}
// mm. pos in real Scorbot
public void Teach(IK robot, Transform target, Vector3 pos, float p, float r, bool online = true)
{
Vector3 posReal = new Vector3(pos.x, pos.y, pos.z);
// mm to cm. pos in simulation
pos = new Vector3(pos.x / 10f, pos.z / 10f, pos.y / 10f);
Vector3 startPos = target.position;
Vector3 startPitch = target.GetComponent <TargetModel>().GetAngles()[3];
Vector3 startRoll = target.GetComponent <TargetModel>().GetAngles()[4];
if (target.GetComponent <TargetModel>().GetAngles() == null)
{
stateOutput.text = "No angles data";
//Debug.Log("No angles data");
//return;
// Just fill something
List <Vector3> angles = new List <Vector3>(5);
for (int i = 0; i < 5; i++)
{
angles.Add(Vector3.zero);
}
target.GetComponent <TargetModel>().SetAngles(angles);
//Debug.Log(angles.Count);
}
// DO HERE and recover angles?
// Apply pitch and roll to target
target.GetComponent <TargetModel>().GetAngles()[3] = robot.GetArticulations()[3].BuiltAngle(p);
target.GetComponent <TargetModel>().GetAngles()[4] = robot.GetArticulations()[4].BuiltAngle(-r);
// Check if it's an unreachable point
target.position = pos;
if (!robot.TargetInRange(target, true))
{
//stateOutput.text = "Unreachable point";
//Debug.Log("Unreachable Teach");
stateOutput.text = "Unreachable Teach";
// Restore target
target.position = startPos;
target.GetComponent <TargetModel>().GetAngles()[3] = startPitch;
target.GetComponent <TargetModel>().GetAngles()[4] = startRoll;
return;
}
target.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
//Debug.Log("Success Teach");
stateOutput.text = "Success Teach";
// Calculate new config with new restrictions
if (gameController.GetOnlineMode() && online)
{
List <float> xyzpr = new List <float>()
{
posReal.x, posReal.y, posReal.z, p, r
};
controller.Connection.GetComponent <SerialController>().WriteToControllerTeach(target.GetComponent <TargetModel>().GetName(), xyzpr);
stateOutput.text = "Success Online Teach";
}
}
/**
* 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 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);
}
