/// <summary>
/// Apply Detach Tool action
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionDetach action)
{
if (this.tool == null)
{
Console.WriteLine("Robot had no tool attached");
return(false);
}
// Relative transform
// If user issued a relative action, make sure there are absolute values to work with. (This limitation is due to current lack of FK/IK solvers)
if (this.position == null || this.rotation == null)
{
Console.WriteLine("Sorry, must provide absolute transform values before detaching a tool... " + this);
return(false);
}
// Now undo the tool's transforms
// TODO: at some point in the future, check for translationFirst here
Rotation newRot = Rotation.Combine(this.rotation, Rotation.Inverse(this.tool.TCPOrientation)); // postmultiplication by the inverse rotation
Vector worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
Vector newPos = this.position - worldVector;
this.prevPosition = this.position;
this.position = newPos;
this.prevRotation = this.rotation;
this.rotation = newRot;
this.prevJoints = this.joints;
this.joints = null;
// Detach the tool
this.tool = null;
return(true);
}
/// <summary>
/// Apply Detach Tool action
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionDetachTool action)
{
if (this.tool == null)
{
logger.Verbose("Robot had no tool attached");
return(false);
}
// Shim for lack of IK
// If coming from axes motion, no need to undo the tool's transform on the TCP
if (this.position == null || this.rotation == null)
{
// Really nothing to do here right?
}
// Otherwise undo the tool's transforms
else
{
// TODO: at some point in the future, check for translationFirst here
Rotation newRot = Rotation.Combine(this.rotation, Rotation.Inverse(this.tool.TCPOrientation)); // postmultiplication by the inverse rotation
Vector worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
Vector newPos = this.position - worldVector;
this.prevPosition = this.position;
this.position = newPos;
this.prevRotation = this.rotation;
this.rotation = newRot;
//this.prevAxes = this.axes;
//this.axes = null; // axes were null anyway...?
}
// "Detach" the tool
this.tool = null;
// -> This code was not properly detaching the tool when coming from axis motion
//// Relative transform
//// If user issued a relative action, make sure there are absolute values to work with. (This limitation is due to current lack of FK/IK solvers)
//if (this.position == null || this.rotation == null)
//{
// logger.Warning($"Cannot apply \"{action}\"; please provide absolute transform values before detaching a tool and try again.");
// return false;
//}
//// Now undo the tool's transforms
//// TODO: at some point in the future, check for translationFirst here
//Rotation newRot = Rotation.Combine(this.rotation, Rotation.Inverse(this.tool.TCPOrientation)); // postmultiplication by the inverse rotation
//Vector worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
//Vector newPos = this.position - worldVector;
//this.prevPosition = this.position;
//this.position = newPos;
//this.prevRotation = this.rotation;
//this.rotation = newRot;
//this.prevAxes = this.axes;
//this.axes = null;
//// Detach the tool
//this.tool = null;
return(true);
}
/// <summary>
/// Undo tool-based TCP transformations on a cursor. Useful for Detach operations.
/// </summary>
/// <param name="tool"></param>
internal void UndoToolTransformOnCursor(RobotCursor cursor, Tool tool, RobotLogger logger, bool log)
{
// TODO: at some point in the future, check for translationFirst here
Rotation newRot = Rotation.Combine(cursor.rotation, Rotation.Inverse(tool.TCPOrientation)); // postmultiplication by the inverse rotation
Vector worldVector = Vector.Rotation(tool.TCPPosition, cursor.rotation);
Vector newPos = cursor.position - worldVector;
cursor.prevPosition = cursor.position;
cursor.position = newPos;
cursor.prevRotation = cursor.rotation;
cursor.rotation = newRot;
//this.prevAxes = this.axes;
//this.axes = null; // axes were null anyway...?
if (log)
{
logger.Verbose("Cursor TCP changed to " + cursor.position + " " + new Orientation(cursor.rotation) + " due to tool removal");
}
}