/** * 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); }