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