Exemplo n.º 1
0
    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());
        }
    }
Exemplo n.º 2
0
    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());
        }
    }
Exemplo n.º 3
0
    /**
     * 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);
            }
        }
    }
Exemplo n.º 4
0
    public void Move(IK robot, Transform target)
    {
        if (target.GetComponent <TargetModel>().GetAngles() != null)
        {
            robot.Move(target);
            //Debug.Log("Using angles");
        }
        else
        {
            robot.CCDAlg(target);
            target.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
            //Debug.Log("Using IK");
        }

        //
        if (gameController.GetOnlineMode())
        {
            controller.RunCommandOnline("move" + " " + target.GetComponent <TargetModel>().GetName());
        }
    }
    /**
     * 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;
        }
    }
Exemplo n.º 6
0
    /**
     * 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);
            }
        }
    }