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