Ejemplo n.º 1
0
 /// <summary>
 /// Begins a movement to the specified tool position.
 /// </summary>
 //
 // Here, the BeginMove () command is used to start the linear trajectory:
 //
 //    jointTraj.BeginMove (jointPos, jointCommand, speed, acc);
 //
 // The two parameters speed and acc are optional. Both are scalar floats. speed is the norm
 // of the velocity vector, and acc is the norm of the acceleration vector.
 //
 // For tool trajectories, the max speed is specified in m/s, and the max acceleration is
 // specified in m/s^2. If left unspecified, this command defaults to using a value of 0.5f
 // for both speed and acceleration. It is currently recommended to keep the max speed between
 // 0.2f and 0.7f.
 //
 public void MoveToTool()
 {
     if (toolActive || jointActive)
     {
         Console.WriteLine("Press i to idle the robot before executing another move.");
     }
     else if (ParsePositions(ref toolCommand))
     {
         toolActive = true;
         Console.WriteLine("Moving to tool position (" + toolCommand.ToVector3().ToString("f3") + ")");
         toolTraj.BeginMove(toolPos, toolCommand);
         toolForce.Clear();
         toolPid.ResetAll();
     }
 }
Ejemplo n.º 2
0
 /// <summary>
 /// Begins a movement to the specified joint position.
 /// </summary>
 //
 // Here, the BeginMove () command is used to start the linear trajectory:
 //
 //    jointTraj.BeginMove (jointPos, jointCommand, speed, acc);
 //
 // The two parameters speed and acc are optional. Both are scalar floats. speed is the norm
 // of the velocity vector, and acc is the norm of the acceleration vector.
 //
 // For joint trajectories, no single joint will ever have a speed larger than the speed
 // specified and all joints will complete their motion at the same time. The max speed is
 // specified in rad/s, and the max acceleration is specified in rad/s^2. If left unspecified,
 // this command defaults to using a value of 0.5f for both speed and acceleration.
 public void MoveToJoint()
 {
     if (toolActive || jointActive)
     {
         Console.WriteLine("Press i to idle the robot before executing another move.\n");
     }
     else if (ParsePositions(ref jointCommand))
     {
         jointActive = true;
         Console.WriteLine("Moving to joint position (" + jointCommand [0] + ", " +
                           jointCommand [1] + ", " + jointCommand [2] + ")");
         jointTraj.BeginMove(jointPos, jointCommand);
         jointTorques.Clear();
         jointPid.ResetAll();
     }
 }
    /// <summary>
    /// Begins the movement. First the robot will move to the start position, then it will move in a circle
    /// until stopped.
    /// </summary>
    public void StartStop()
    {
        if (motionActive)
        {
            Idle();
        }
        else
        {
            motionActive = true;
            Console.WriteLine("Moving to joint position (" + startPos [0].ToString("f3") + ", " +
                              startPos [1].ToString("f3") + ", " + startPos [2].ToString("f3") + ")");
            jointTorque.Clear();
            jointPid.ResetAll();

            float speed        = 0.2f;
            float acceleration = 0.2f;
            startTraj.BeginMove(jointPos, startPos, speed, acceleration);
        }
    }
    /// <summary>
    /// Begins the movement. First the robot will move to the start position, then it will move in a circle
    /// until stopped.
    /// </summary>
    public void StartStop()
    {
        if (motionActive)
        {
            Idle();
        }
        else
        {
            motionActive = true;
            Console.WriteLine("Moving to tool position (" + startPos [0].ToString("f3") + ", " +
                              startPos [1].ToString("f3") + ", " + startPos [2].ToString("f3") + ")");
            toolForce.Clear();
            toolPid.ResetAll();

            // Since startPos was not declared as a Vector<float>, one is created here for
            // the input to BeginMove().
            float speed        = 0.2f;
            float acceleration = 0.2f;
            startTraj.BeginMove(toolPos, Vector <float> .Build.DenseOfArray(startPos), speed, acceleration);
        }
    }
 /// <summary>
 /// Holds the current joint position or releases and active hold.
 /// </summary>
 public void HoldJointPosition()
 {
     if (toolHolding)
     {
         Console.WriteLine("Press t to release tool hold before activating joint hold.\n");
         return;
     }
     if (!jointHolding)
     {
         jointPos.CopyTo(jointHoldPos);
         Console.WriteLine("Holding joint position at {0}", jointHoldPos.ToVector3());
         jointTorques.Clear();
         jointPid.ResetAll();
     }
     else
     {
         Console.WriteLine("Releasing joint hold position.");
     }
     jointHolding = !jointHolding;
 }
    /// <summary>
    /// Holds the current tool position or releases and active hold.
    /// </summary>
    public void HoldToolPosition()
    {
        if (jointHolding)
        {
            Console.WriteLine("Press j to release joint hold before activating tool hold.\n");
            return;
        }

        if (!toolHolding)
        {
            toolPos.CopyTo(toolHoldPos);
            Console.WriteLine("Holding tool position at {0}", toolHoldPos.ToVector3());
            toolForce.Clear();
            toolPid.ResetAll();
        }
        else
        {
            Console.WriteLine("Releasing tool hold position.");
        }
        toolHolding = !toolHolding;
    }