public static void OnMotionCursorUpdated(object sender, EventArgs e)
{
Robot r = sender as Robot;
Machina.Vector p = r.GetCurrentPosition();
Machina.Rotation rot = r.GetCurrentRotation();
Joints j = r.GetCurrentAxes();
if (p != null && rot != null)
{
wssv.WebSocketServices.Broadcast($"{{\"msg\":\"pose\",\"data\":[{p.X},{p.Y},{p.Z},{rot.Q.W},{rot.Q.X},{rot.Q.Y},{rot.Q.Z}]}}");
}
else if (j != null)
{
wssv.WebSocketServices.Broadcast($"{{\"msg\":\"joints\",\"data\":[{j.J1},{j.J2},{j.J3},{j.J4},{j.J5},{j.J6}]}}");
}
}
/// <summary>
/// Creates an Orientation object from a Rotation representation.
/// </summary>
/// <param name="r"></param>
internal Orientation(Rotation r)
: this(r.Q)
{
}
public ActionExecutedArgs(Action last, int pendingExecutionOnDevice, int pendingExecutionTotal, Vector pos, Rotation ori, Joints axes, ExternalAxes extax)
{
this.LastAction = last;
this.PendingExecutionOnDevice = pendingExecutionOnDevice;
this.PendingExecutionTotal = pendingExecutionTotal;
this.Position = pos;
this.Rotation = ori;
this.Axes = axes;
this.ExternalAxes = ExternalAxes;
}
/// <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);
}
public bool Rotate(Rotation r)
{
return(this.Rotate(r.Q));
}
public ActionRotation(Rotation rot, bool relRot) : base()
{
this.rotation = new Rotation(rot); // shallow copy
this.relative = relRot;
}
public ActionReleasedArgs(Action last, int pendingReleaseToDevice, Vector pos, Rotation ori, Joints axes, ExternalAxes extax)
{
this.LastAction = last;
this.PendingReleaseToDevice = pendingReleaseToDevice;
this.Position = pos;
this.Rotation = ori;
this.Axes = axes;
this.ExternalAxes = ExternalAxes;
}
