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