/// <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 Attach Tool Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionAttach action)
{
// The cursor has now a tool attached to it
this.tool = action.tool;
// 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 attaching a tool... " + this);
}
else
{
// Now transform the cursor position to the tool's transformation params:
Vector worldVector = Vector.Rotation(action.tool.TCPPosition, this.rotation);
Vector newPos = this.position + worldVector;
Rotation newRot = Rotation.Combine(this.rotation, action.tool.TCPOrientation); // postmultiplication
this.prevPosition = this.position;
this.position = newPos;
this.prevRotation = this.rotation;
this.rotation = newRot;
this.prevJoints = this.joints;
// this.joints = null; // flag joints as null to avoid Joint instructions using obsolete data --> no need to do this, joints remain the same anyway?
}
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>
/// Modify a cursor's TCP transform according to a tool. Useful for Attach operations.
/// </summary>
/// <param name="tool"></param>
internal void ApplyToolTransformToCursor(RobotCursor cursor, Tool tool, RobotLogger logger, bool log)
{
// Now transform the cursor position to the tool's transformation params:
Vector worldVector = Vector.Rotation(tool.TCPPosition, cursor.rotation);
Vector newPos = cursor.position + worldVector;
Rotation newRot = Rotation.Combine(cursor.rotation, tool.TCPOrientation); // postmultiplication
cursor.prevPosition = cursor.position;
cursor.position = newPos;
cursor.prevRotation = cursor.rotation;
cursor.rotation = newRot;
//cursor.prevAxes = cursor.axes; // why was this here? joints don't change on tool attachment...
if (log)
{
logger.Verbose("Cursor TCP changed to " + cursor.position + " " + new Orientation(cursor.rotation) + " due to tool attachment");
}
}
/// <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");
}
}
/// <summary>
/// Apply Attach Tool Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionAttachTool action)
{
// Sanity
if (!availableTools.ContainsKey(action.toolName))
{
logger.Warning($"No tool named \"{action.toolName}\" defined in this robot; please use \"DefineTool\" first.");
return(false);
}
// Shim for lack of IK
// If coming from axes motion, no need to transform the TCP
if (this.position == null || this.rotation == null)
{
logger.Warning($"Attaching tool without TCP values, inconsistent results may follow...?");
}
// Otherwise transform the TCP
else
{
// Now transform the cursor position to the tool's transformation params:
Vector worldVector = Vector.Rotation(this.tool.TCPPosition, this.rotation);
Vector newPos = this.position + worldVector;
Rotation newRot = Rotation.Combine(this.rotation, this.tool.TCPOrientation); // postmultiplication
this.prevPosition = this.position;
this.position = newPos;
this.prevRotation = this.rotation;
this.rotation = newRot;
//this.prevAxes = this.axes; // why was this here? joints don't change on tool attachment...
}
// The cursor has now a tool attached to it
this.tool = availableTools[action.toolName];
return(true);
}
/// <summary>
/// Apply Transformation Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionTransformation action)
{
Vector newPos;
Rotation newRot;
// Relative transform
if (action.relative)
{
// 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 (position == null || rotation == null)
{
logger.Warning($"Cannot apply \"{action}\", must provide absolute transform values first before applying relative ones...");
return(false);
}
// This is Translate + Rotate
if (action.translationFirst)
{
if (referenceCS == ReferenceCS.World)
{
newPos = position + action.translation;
newRot = Rotation.Combine(action.rotation, rotation); // premultiplication
}
else
{
//Vector worldVector = Vector.Rotation(action.translation, Rotation.Conjugate(this.rotation));
Vector worldVector = Vector.Rotation(action.translation, this.rotation);
newPos = position + worldVector;
newRot = Rotation.Combine(rotation, action.rotation); // postmultiplication
}
}
// or Rotate + Translate
else
{
if (referenceCS == ReferenceCS.World)
{
newPos = position + action.translation;
newRot = Rotation.Combine(action.rotation, rotation); // premultiplication
}
else
{
// @TOCHECK: is this correct?
newRot = Rotation.Combine(rotation, action.rotation); // postmultiplication
//Vector worldVector = Vector.Rotation(action.translation, Rotation.Conjugate(newRot));
Vector worldVector = Vector.Rotation(action.translation, newRot);
newPos = position + worldVector;
}
}
}
// Absolute transform
else
{
newPos = new Vector(action.translation);
newRot = new Rotation(action.rotation);
}
//// @TODO: this must be more programmatically implemented
//if (Control.SAFETY_CHECK_TABLE_COLLISION)
//{
// if (Control.IsBelowTable(newPos.Z))
// {
// if (Control.SAFETY_STOP_ON_TABLE_COLLISION)
// {
// Console.WriteLine("Cannot perform action: too close to base XY plane --> TCP.z = {0}", newPos.Z);
// return false;
// }
// else
// {
// Console.WriteLine("WARNING: too close to base XY plane, USE CAUTION! --> TCP.z = {0}", newPos.Z);
// }
// }
//}
prevPosition = position;
position = newPos;
prevRotation = rotation;
rotation = newRot;
prevAxes = axes;
axes = null; // flag joints as null to avoid Joint instructions using obsolete data
if (isExtruding)
{
this.ComputeExtrudedLength();
}
if (_logRelativeActions && action.relative)
{
logger.Verbose("TCP transform at " + this.position + " " + new Orientation(this.rotation));
}
return(true);
}