/**
* Crea una posición (objeto) y la válida. El nombre se obtiene del campo la interfaz gráfica.
* @param fromCommand Si es para el comando "defp".
* @return Transform
*/
public void RecordPosition(bool fromCommand)
{
Transform addedTarget = null;
InputField targetNameInput = this.targetNameInput;
if (fromCommand)
{
targetNameInput = this.DefpNameInput;
}
stateMessageControl.NewBlock();
// No positions recorded
if (targetControl.Count() == 0)
{
CreateDefaultTarget(targetNameInput.text);
}
else // Already at least one point
{
GameObject prevSelectedObject = selectionControl.SearchContainTag("Target");
if (prevSelectedObject != null)
{
prevSelectedObject = prevSelectedObject.transform.parent.gameObject;
// Validate target
Vector3 newPos = prevSelectedObject.transform.position;
if (!ValidTarget(targetNameInput.text, prevSelectedObject.transform, newPos))
{
return;
}
// Add target
addedTarget = targetControl.Add(targetNameInput.text, newPos, robot.GetAnglesFromCopy());
addedTarget.GetComponent <TargetModel>().SetValid(true);
selectionControl.SetActiveAxis(addedTarget, false);
selectionControl.SelectedObject(prevSelectedObject);
stateMessageControl.WriteMessage("Done. Recorded position \"" + targetNameInput.text + "\"", true);
UpdateTargets(targetControl.GetNames());
targetNameInput.text = "";
}
else
{
CreateDefaultTarget(targetNameInput.text);
}
}
stateMessageControl.UpdatePositionLog();
}
/**
* 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);
}