/// <summary>
    /// Initializes a new instance of the <see cref="BaseComm"/> class.
    /// </summary>
    public BaseCommExample()
    {
        // Set up communication with the robot and initialize force to zero
        robot = new RobotClient();
        var version = robot.GetVersion();

        Barrett.Logger.Debug(Barrett.Logger.INFO, version.ToString());
        robot.SendCartesianForces(Vector3.zero);

        // Set up keyboard callbacks. PrintUsage needs to be updated when these are changed.
        keyboardManager = new Barrett.KeyboardManager();
        keyboardManager.SetDebug(true);                  // print key pressed
        keyboardManager.AddKeyPressCallback("q", Close); // this will call the Close method when 'q' is pressed
        keyboardManager.AddKeyPressCallback("h", OnHome);
        keyboardManager.AddKeyPressCallback("e", OnEnable);
        keyboardManager.AddKeyPressCallback("d", OnDisable);
        keyboardManager.AddKeyPressCallback("t", SubscribeToUpdate);
        PrintUsage();

        // Loop: send zero force at every timestep.
        bool running = true;

        while (running)
        {
            running = ReadKeyPress();
            Thread.Sleep(50);
            robot.SendCartesianForces(Vector3.zero);
        }
    }
    public SpringExample()
    {
        // Initialize vectors. This is required to set the length of the vectors before they
        // are used. Build.Dense initializes all the elements to 0. Build.DenseOfArray sets
        // the vector length to the length of the array and the values equal to the values
        // in the array.
        jointPos = Vector <float> .Build.Dense(kDof);

        jointSpringPos = Vector <float> .Build.Dense(kDof);

        jointTorques = Vector <float> .Build.Dense(kDof);

        toolPos = Vector <float> .Build.Dense(kNumDim);

        cartSpringPos = Vector <float> .Build.Dense(kNumDim);

        toolForce = Vector <float> .Build.Dense(kNumDim);

        kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);

        // Set up communication with the robot.
        robot = new RobotClient();
        robot.SubscribeToServerUpdate(OnReceiveServerUpdate);

        robot.SubscribeToRobotStatus(status => {
            Barrett.Logger.Debug(Barrett.Logger.INFO, "Handedness: {0}", status.handedness);
            Barrett.Logger.Debug(Barrett.Logger.INFO, "Outerlink: {0}", status.outerlink);
            Barrett.Logger.Debug(Barrett.Logger.INFO, "IsPatientConnected?: {0}", status.patient);
        });

        // Initialize force/torque inputs to zero. Here, both are initialized separately. Alternately,
        // both can be set in a single command with
        //    robot.SendCartesianForcesAndJointTorques (Vector3.zero, Vector3.zero);
        robot.SendCartesianForces(Vector3.zero);
        robot.SendJointTorques(Vector3.zero);

        // Set up keyboard callbacks
        keyboardManager = new Barrett.KeyboardManager();
        keyboardManager.SetDebug(true);           // print key pressed
        keyboardManager.AddKeyPressCallback("h", OnHome);
        keyboardManager.AddKeyPressCallback("e", OnEnable);
        keyboardManager.AddKeyPressCallback("d", OnDisable);
        keyboardManager.AddKeyPressCallback("1", JointSpring);
        keyboardManager.AddKeyPressCallback("2", JointSpring);
        keyboardManager.AddKeyPressCallback("3", JointSpring);
        keyboardManager.AddKeyPressCallback("x", CartesianSpring);
        keyboardManager.AddKeyPressCallback("y", CartesianSpring);
        keyboardManager.AddKeyPressCallback("z", CartesianSpring);
        keyboardManager.AddKeyPressCallback("r", RemoveAllSprings);
        keyboardManager.AddKeyPressCallback("p", PrintInfo);
        keyboardManager.AddKeyPressCallback("q", Close);
        PrintUsage();

        // Loop: calculate forces/torques at every timestep based on current
        // state feedback from the robot.
        bool running = true;

        intervalTimer.Reset();

        while (running)
        {
            running = ReadKeyPress();
            jointTorques.Clear();
            toolForce.Clear();
            jointTorques = CalcJointSpringTorques();
            toolForce    = CalcCartesianSpringForces();

            robot.SendCartesianForcesAndJointTorques(toolForce.ToVector3(), jointTorques.ToVector3())
            .Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
            .Done();

            Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
            intervalTimer.Restart();
        }
    }
Exemplo n.º 3
0
    public SimpleMoveExample()
    {
        // Initialize vectors
        jointPos = Vector <float> .Build.Dense(kDof);

        jointTorques = Vector <float> .Build.Dense(kDof);

        toolPos = Vector <float> .Build.Dense(kNumDim);

        toolForce = Vector <float> .Build.Dense(kNumDim);

        jointCommand = Vector <float> .Build.Dense(kDof);

        toolCommand = Vector <float> .Build.Dense(kNumDim);

        kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);

        kiJoint = Vector <float> .Build.DenseOfArray(kiJointDefault);

        kdJoint = Vector <float> .Build.DenseOfArray(kdJointDefault);

        // Set up communication with the robot and initialize force/torque to zero
        robot = new RobotClient();
        robot.SubscribeToServerUpdate(OnReceiveServerUpdate);
        robot.SubscribeToRobotStatus(status => {
            Barrett.Logger.Debug(Barrett.Logger.INFO, "Handedness: {0}", status.handedness);
            Barrett.Logger.Debug(Barrett.Logger.INFO, "Outerlink: {0}", status.outerlink);
            Barrett.Logger.Debug(Barrett.Logger.INFO, "IsPatientConnected?: {0}", status.patient);
        });

        robot.SendCartesianForces(Vector3.zero);
        robot.SendJointTorques(Vector3.zero);

        // Set up keyboard callbacks
        keyboardManager = new Barrett.KeyboardManager();
        keyboardManager.SetDebug(true);           // print key pressed
        keyboardManager.AddKeyPressCallback("h", OnHome);
        keyboardManager.AddKeyPressCallback("e", OnEnable);
        keyboardManager.AddKeyPressCallback("d", OnDisable);
        keyboardManager.AddKeyPressCallback("j", MoveToJoint);
        keyboardManager.AddKeyPressCallback("t", MoveToTool);
        keyboardManager.AddKeyPressCallback("i", OnIdle);
        keyboardManager.AddKeyPressCallback("p", PrintInfo);
        keyboardManager.AddKeyPressCallback("q", Close);
        PrintUsage();

        // Set up PID controllers
        jointPid = new Barrett.Control.PidVector(kpJoint, kiJoint, kdJoint, kDof, lowpassFilterFreq);
        toolPid  = new Barrett.Control.PidVector(kpTool, kiTool, kdTool, kNumDim, lowpassFilterFreq);

        // Set up trajectory generators
        jointTraj = new Barrett.Control.LinearTrajectoryVector(kDof);
        toolTraj  = new Barrett.Control.LinearTrajectoryVector(kNumDim);

        // Start the dtTimer
        dtTimer.Reset();
        dtTimer.Start();

        // Loop: calculate forces/torques at every timestep based on current
        // state feedback from the robot.
        bool running = true;

        intervalTimer.Reset();
        while (running)
        {
            running = ReadKeyPress();

            float dt = (float)dtTimer.ElapsedTicks / (float)Stopwatch.Frequency;
            dtTimer.Restart();

            if (jointActive)
            {
                jointTraj.Update();
                jointTorques = jointPid.Update(jointTraj.Position, jointPos, dt);
                toolForce.Clear();
            }
            else if (toolActive)
            {
                toolTraj.Update();
                toolForce = toolPid.Update(toolTraj.Position, toolPos, dt);
                jointTorques.Clear();
            }
            else
            {
                jointTorques.Clear();
                toolForce.Clear();
            }

            robot.SendCartesianForcesAndJointTorques(toolForce.ToVector3(), jointTorques.ToVector3())
            .Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
            .Done();

            // Calculate how long to wait until next control cycle
            Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
            intervalTimer.Restart();
        }
    }
    public HoldPositionExample()
    {
        // Initialize vectors. This is required to set the length of the vectors before they
        // are used. Build.Dense initializes all the elements to 0. Build.DenseOfArray sets
        // the vector length to the length of the array and the values equal to the values
        // in the array.
        jointPos = Vector <float> .Build.Dense(kDof);

        jointHoldPos = Vector <float> .Build.Dense(kDof);

        jointTorques = Vector <float> .Build.Dense(kDof);

        toolPos = Vector <float> .Build.Dense(kNumDim);

        toolHoldPos = Vector <float> .Build.Dense(kNumDim);

        toolForce = Vector <float> .Build.Dense(kNumDim);

        kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);

        kiJoint = Vector <float> .Build.DenseOfArray(kiJointDefault);

        kdJoint = Vector <float> .Build.DenseOfArray(kdJointDefault);

        // Set up communication with the robot.
        robot = new RobotClient();
        robot.SubscribeToServerUpdate(OnReceiveServerUpdate);

        robot.SubscribeToRobotStatus(status => {
            Barrett.Logger.Debug(Barrett.Logger.INFO, "Handedness: {0}", status.handedness);
            Barrett.Logger.Debug(Barrett.Logger.INFO, "Outerlink: {0}", status.outerlink);
            Barrett.Logger.Debug(Barrett.Logger.INFO, "IsPatientConnected?: {0}", status.patient);
        });

        // Initialize force/torque inputs to zero. Here, both are initialized separately. Alternately,
        // both can be set in a single command with
        //    robot.SendCartesianForcesAndJointTorques (Vector3.zero, Vector3.zero);
        robot.SendCartesianForces(Vector3.zero);
        robot.SendJointTorques(Vector3.zero);

        // Set up keyboard callbacks
        keyboardManager = new Barrett.KeyboardManager();
        keyboardManager.SetDebug(true);           // print key pressed
        keyboardManager.AddKeyPressCallback("h", OnHome);
        keyboardManager.AddKeyPressCallback("e", OnEnable);
        keyboardManager.AddKeyPressCallback("d", OnDisable);
        keyboardManager.AddKeyPressCallback("j", HoldJointPosition);
        keyboardManager.AddKeyPressCallback("t", HoldToolPosition);
        keyboardManager.AddKeyPressCallback("p", PrintInfo);
        keyboardManager.AddKeyPressCallback("q", Close);
        PrintUsage();

        // Set up PID controllers
        jointPid = new Barrett.Control.PidVector(kpJoint, kiJoint, kdJoint, kDof, lowpassFilterFreq);
        toolPid  = new Barrett.Control.PidVector(kpTool, kiTool, kdTool, kNumDim, lowpassFilterFreq);

        // Start the dtTimer
        dtTimer.Reset();
        dtTimer.Start();

        // Loop: calculate forces/torques at every timestep based on current
        // state feedback from the robot.
        bool running = true;

        intervalTimer.Reset();

        while (running)
        {
            running = ReadKeyPress();

            float dt = (float)dtTimer.ElapsedTicks / (float)Stopwatch.Frequency;
            dtTimer.Restart();
            if (jointHolding)
            {
                jointTorques = jointPid.Update(jointHoldPos, jointPos, dt);
                toolForce.Clear();
            }
            else if (toolHolding)
            {
                toolForce = toolPid.Update(toolHoldPos, toolPos, dt);
                jointTorques.Clear();
            }
            else
            {
                jointTorques.Clear();
                toolForce.Clear();
            }

            robot.SendCartesianForcesAndJointTorques(toolForce.ToVector3(), jointTorques.ToVector3())
            .Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
            .Done();

            Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
            intervalTimer.Restart();
        }
    }
    public CustomJointTrajectory()
    {
        // Initialize vectors
        jointPos = Vector <float> .Build.Dense(kDof);

        jointTorque = Vector <float> .Build.Dense(kDof);

        jointCommand = Vector <float> .Build.Dense(kDof);

        kpJoint = Vector <float> .Build.DenseOfArray(kpJointDefault);

        kiJoint = Vector <float> .Build.DenseOfArray(kiJointDefault);

        kdJoint = Vector <float> .Build.DenseOfArray(kdJointDefault);

        // Set up communication with the robot and initialize force/torque to zero
        robot = new RobotClient();
        robot.SubscribeToServerUpdate(OnReceiveServerUpdate);
        robot.SubscribeToRobotStatus(OnReceiveRobotStatus);
        robot.SendCartesianForces(Vector3.zero);

        // Set up keyboard callbacks
        keyboardManager = new Barrett.KeyboardManager();
        keyboardManager.SetDebug(true);           // print key pressed
        keyboardManager.AddKeyPressCallback("h", OnHome);
        keyboardManager.AddKeyPressCallback("e", OnEnable);
        keyboardManager.AddKeyPressCallback("d", OnDisable);
        keyboardManager.AddKeyPressCallback("s", StartStop);
        keyboardManager.AddKeyPressCallback("q", Close);
        PrintUsage();

        // Add some blank lines for readability, then get the cursor position.
        Console.WriteLine();
        Console.WriteLine();
        int top = Console.CursorTop;

        // Print labels, which do not change. Add an offset on the left side for readability
        // and specify the length of the strings to clear any text that already exists in
        // that space. Make sure that the length is longer than your strings to leave enough
        // space.
        int line   = top;
        int left   = 10;
        int length = 35;

        PrintAtPosition(left, line++, "Commanded joint positions (rad):", length);
        PrintAtPosition(left, line++, "Actual joint positions (rad):", length);
        PrintAtPosition(left, line++, "Joint control torques (N-m):", length);

        // Move the cursor to a nice place to display other text.
        line += 2;
        Console.SetCursorPosition(0, line);

        // Set the start position on each line for the data.
        left  += length;
        length = 30;

        // Set up PID controller
        jointPid = new Barrett.Control.PidVector(kpJoint, kiJoint, kdJoint, kDof, lowpassFilterFreq);

        // Set up trajectory generator
        startTraj = new Barrett.Control.LinearTrajectoryVector(kDof);

        // Start the dtTimer
        dtTimer.Reset();
        dtTimer.Start();

        // Loop: calculate forces/torques at every timestep based on current
        // state feedback from the robot.
        bool running = true;

        intervalTimer.Reset();
        displayTimer.Reset();
        displayTimer.Start();
        while (running)
        {
            running = ReadKeyPress();

            float dt = (float)dtTimer.ElapsedTicks / (float)Stopwatch.Frequency;
            dtTimer.Restart();

            if (motionActive)
            {
                if (!startTraj.DoneMoving)
                {
                    // Follow the linear trajectory to the start position until it is done.
                    startTraj.Update();
                    startTraj.Position.CopyTo(jointCommand);
                }
                else
                {
                    // Start the timer for the circle, if it's not already started.
                    if (!circleTimer.IsRunning)
                    {
                        line += 1;
                        ClearLine(line, (uint)(Console.WindowHeight - line));
                        Console.SetCursorPosition(0, line);
                        Console.WriteLine("Executing circular movement.");
                        circleTimer.Start();
                    }

                    // Calculate the new joint position command. Constant in joint 0 and
                    // cyclic movement in joints 1 and 2.
                    float time = (float)(circleTimer.ElapsedMilliseconds) / 1000f;
                    jointCommand [0] = startPos [0];
                    jointCommand [1] = radius * (Mathf.Cos(2f * Mathf.PI * frequency * time) - 1.0f) + startPos [1];
                    jointCommand [2] = radius * Mathf.Sin(2f * Mathf.PI * frequency * time) + startPos [2];
                }
                jointTorque = jointPid.Update(jointCommand, jointPos, dt);
            }
            else
            {
                jointTorque.Clear();
            }

            robot.SendCartesianForcesAndJointTorques(Vector3.zero, jointTorque.ToVector3())
            .Catch(e => Barrett.Logger.Debug(Barrett.Logger.CRITICAL, "Exception {0}", e))
            .Done();

            // Calculate how long to wait until next control cycle
            Thread.Sleep(Math.Max(0, controlLoopTime - (int)intervalTimer.ElapsedMilliseconds));
            intervalTimer.Restart();

            // Update the display, if applicable.
            if ((int)displayTimer.ElapsedMilliseconds >= displayUpdateTime)
            {
                line = top;
                PrintAtPosition(left, line++, jointCommand.ToVector3().ToString("F4"), length);
                PrintAtPosition(left, line++, jointPos.ToVector3().ToString("F4"), length);
                PrintAtPosition(left, line++, jointTorque.ToVector3().ToString("F4"), length);

                // Move the cursor to a nice place to display user input.
                line += 2;
                Console.SetCursorPosition(0, line);

                displayTimer.Restart();
            }
        }

        Console.Write("Quitting.");
        Environment.Exit(0);
    }
Exemplo n.º 6
0
    /// <summary>
    /// Initializes a new instance of the <see cref="DisplayBasicInfo"/> class.
    /// </summary>
    public DisplayBasicInfo()
    {
        // Set up communication with the robot and initialize force to zero.
        robot = new RobotClient();
        SubscribeToUpdates();
        robot.SendCartesianForces(Vector3.zero);

        // Set up keyboard callbacks.
        keyboardManager = new Barrett.KeyboardManager();
        keyboardManager.SetDebug(true);          // print key pressed
        keyboardManager.AddKeyPressCallback("q", Close);
        keyboardManager.AddKeyPressCallback("h", OnHome);
        keyboardManager.AddKeyPressCallback("e", OnEnable);
        keyboardManager.AddKeyPressCallback("d", OnDisable);
        PrintUsage();

        // Add some blank lines for readability, then get the cursor position.
        Console.WriteLine();
        Console.WriteLine();
        int top = Console.CursorTop;

        // Print labels, which do not change. Add an offset on the left side for readability
        // and specify the length of the strings to clear any text that already exists in
        // that space. Make sure that the length is longer than your strings to leave enough
        // space.
        int line   = top;
        int left   = 10;
        int length = 20;

        PrintAtPosition(left, line++, "Joint positions:", length);
        PrintAtPosition(left, line++, "Tool position:", length);
        PrintAtPosition(left, line++, "Tool Velocity:", length);
        PrintAtPosition(left, line++, "Handedness:", length);
        PrintAtPosition(left, line++, "Outer link status:", length);

        // Set the start position on each line for the data.
        left  += length;
        length = 30;

        // Loop: Display most recent state info, check for new key presses, and send zero force.
        bool running = true;

        while (running)
        {
            line = top;
            PrintAtPosition(left, line++, joint_position.ToString("F4"), length);
            PrintAtPosition(left, line++, tool_position.ToString("F4"), length);
            PrintAtPosition(left, line++, tool_velocity.ToString("F4"), length);
            PrintAtPosition(left, line++, handedness.ToString(), length);
            PrintAtPosition(left, line++, outerlinkStatus.ToString(), length);

            // Move the cursor to a nice place to display user input.
            line += 2;
            Console.SetCursorPosition(0, line);
            running = ReadKeyPress();

            Thread.Sleep(50);
            robot.SendCartesianForces(Vector3.zero);
        }

        Console.Write("Quitting.");
        Environment.Exit(0);
    }