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