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