/// <summary>
/// Begins a movement to the specified tool position.
/// </summary>
//
// Here, the BeginMove () command is used to start the linear trajectory:
//
// jointTraj.BeginMove (jointPos, jointCommand, speed, acc);
//
// The two parameters speed and acc are optional. Both are scalar floats. speed is the norm
// of the velocity vector, and acc is the norm of the acceleration vector.
//
// For tool trajectories, the max speed is specified in m/s, and the max acceleration is
// specified in m/s^2. If left unspecified, this command defaults to using a value of 0.5f
// for both speed and acceleration. It is currently recommended to keep the max speed between
// 0.2f and 0.7f.
//
public void MoveToTool()
{
if (toolActive || jointActive)
{
Console.WriteLine("Press i to idle the robot before executing another move.");
}
else if (ParsePositions(ref toolCommand))
{
toolActive = true;
Console.WriteLine("Moving to tool position (" + toolCommand.ToVector3().ToString("f3") + ")");
toolTraj.BeginMove(toolPos, toolCommand);
toolForce.Clear();
toolPid.ResetAll();
}
}
/// <summary>
/// Begins a movement to the specified joint position.
/// </summary>
//
// Here, the BeginMove () command is used to start the linear trajectory:
//
// jointTraj.BeginMove (jointPos, jointCommand, speed, acc);
//
// The two parameters speed and acc are optional. Both are scalar floats. speed is the norm
// of the velocity vector, and acc is the norm of the acceleration vector.
//
// For joint trajectories, no single joint will ever have a speed larger than the speed
// specified and all joints will complete their motion at the same time. The max speed is
// specified in rad/s, and the max acceleration is specified in rad/s^2. If left unspecified,
// this command defaults to using a value of 0.5f for both speed and acceleration.
public void MoveToJoint()
{
if (toolActive || jointActive)
{
Console.WriteLine("Press i to idle the robot before executing another move.\n");
}
else if (ParsePositions(ref jointCommand))
{
jointActive = true;
Console.WriteLine("Moving to joint position (" + jointCommand [0] + ", " +
jointCommand [1] + ", " + jointCommand [2] + ")");
jointTraj.BeginMove(jointPos, jointCommand);
jointTorques.Clear();
jointPid.ResetAll();
}
}
/// <summary>
/// Begins the movement. First the robot will move to the start position, then it will move in a circle
/// until stopped.
/// </summary>
public void StartStop()
{
if (motionActive)
{
Idle();
}
else
{
motionActive = true;
Console.WriteLine("Moving to joint position (" + startPos [0].ToString("f3") + ", " +
startPos [1].ToString("f3") + ", " + startPos [2].ToString("f3") + ")");
jointTorque.Clear();
jointPid.ResetAll();
float speed = 0.2f;
float acceleration = 0.2f;
startTraj.BeginMove(jointPos, startPos, speed, acceleration);
}
}
/// <summary>
/// Begins the movement. First the robot will move to the start position, then it will move in a circle
/// until stopped.
/// </summary>
public void StartStop()
{
if (motionActive)
{
Idle();
}
else
{
motionActive = true;
Console.WriteLine("Moving to tool position (" + startPos [0].ToString("f3") + ", " +
startPos [1].ToString("f3") + ", " + startPos [2].ToString("f3") + ")");
toolForce.Clear();
toolPid.ResetAll();
// Since startPos was not declared as a Vector<float>, one is created here for
// the input to BeginMove().
float speed = 0.2f;
float acceleration = 0.2f;
startTraj.BeginMove(toolPos, Vector <float> .Build.DenseOfArray(startPos), speed, acceleration);
}
}
/// <summary>
/// Holds the current joint position or releases and active hold.
/// </summary>
public void HoldJointPosition()
{
if (toolHolding)
{
Console.WriteLine("Press t to release tool hold before activating joint hold.\n");
return;
}
if (!jointHolding)
{
jointPos.CopyTo(jointHoldPos);
Console.WriteLine("Holding joint position at {0}", jointHoldPos.ToVector3());
jointTorques.Clear();
jointPid.ResetAll();
}
else
{
Console.WriteLine("Releasing joint hold position.");
}
jointHolding = !jointHolding;
}
/// <summary>
/// Holds the current tool position or releases and active hold.
/// </summary>
public void HoldToolPosition()
{
if (jointHolding)
{
Console.WriteLine("Press j to release joint hold before activating tool hold.\n");
return;
}
if (!toolHolding)
{
toolPos.CopyTo(toolHoldPos);
Console.WriteLine("Holding tool position at {0}", toolHoldPos.ToVector3());
toolForce.Clear();
toolPid.ResetAll();
}
else
{
Console.WriteLine("Releasing tool hold position.");
}
toolHolding = !toolHolding;
}
