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