public static RsMoveInstruction CreateNewInstructionInActivePath(Matrix4 frame)
{
var path = Station.ActiveStation?.ActiveTask?.ActivePathProcedure;
if (path == null)
{
throw new InvalidOperationException();
}
var task = path.Parent as RsTask;
if (task == null)
{
throw new InvalidOperationException();
}
var wobj = task.ActiveWorkObject;
var robTarget = CreateRobTargetAndTarget(frame, task, wobj);
var procDef = task.ActiveProcessDefinition;
var procTemplate = procDef.ActiveProcessTemplate;
var instruction = new RsMoveInstruction(task, procDef.Name, procTemplate.Name, MotionType.Linear, wobj.Name, robTarget.Name, task.ActiveTool.Name);
path.Instructions.Add(instruction);
return(instruction);
}
void HandleBeginDrag(VrEventArgs args)
{
_frameGfx.Visible = false;
var hitInstruction = args.HitResult as HitResult <RsMoveInstruction>;
var hitSegment = args.HitResult as HitResult <PathSegment>;
if (hitInstruction != null)
{
_dragOffset = args.Frame.InverseRigid() * hitInstruction.HitObject.GetToTarget().Transform.GlobalMatrix;
WithUndo("VR Drag", () => PathEditingHelper.MoveInstructionToFrame(hitInstruction.HitObject, args.Frame * _dragOffset));
_newHighlightTarget = hitInstruction?.HitObject?.GetToTarget();
}
else if (hitSegment != null)
{
WithUndo("VR Drag", () => _newInstruction = PathEditingHelper.InsertMoveInstruction(hitSegment.HitObject, args.Frame));
}
else if (args.HitResult != null)
{
throw new InvalidOperationException("unrecognized HitResult");
}
else
{
WithUndo("VR Create Instruction", () => _newInstruction = PathEditingHelper.CreateNewInstructionInActivePath(args.Frame));
_dragOffset = Matrix4.Identity;
args.CreatedObject = () => new HitResult <RsMoveInstruction>(_newInstruction, 0, args.Frame.Translation);
}
}
public static void DeleteInstruction(RsMoveInstruction instruction)
{
var path = (RsPathProcedure)instruction.Parent;
var target = instruction.GetToTarget();
var robTarget = instruction.GetToRobTarget();
var task = (RsTask)target.Parent;
path.Instructions.Remove(instruction);
task.Targets.Remove(target);
task.DataDeclarations.Remove(robTarget);
}
public static void MoveInstructionToFrame(RsMoveInstruction instruction, Matrix4 frame)
{
if (instruction == null)
{
return;
}
var target = instruction.GetToTarget();
target.Transform.GlobalMatrix = frame;
}
RsTarget GetTarget(RsMoveInstruction instruction)
{
RsTarget target;
if (!_targets.TryGetValue(instruction, out target))
{
target = instruction.GetToTarget();
_targets[instruction] = target;
}
return(target);
}
public static string CreatePath(string procedureName)
{
Project.UndoContext.BeginUndoStep("RsPathProcedure Create");
try
{
//Get the active Station
Station station = Project.ActiveProject as Station;
// Create a PathProcedure.
RsPathProcedure myPath = new RsPathProcedure(procedureName);
// Add the path to the ActiveTask.
station.ActiveTask.PathProcedures.Add(myPath);
myPath.ModuleName = "module1";
myPath.ShowName = true;
myPath.Synchronize = true;
myPath.Visible = true;
//Make the path procedure as active path procedure
station.ActiveTask.ActivePathProcedure = myPath;
//Create Path
foreach (RsTarget target in targets)
{
RsMoveInstruction moveInstruction =
new RsMoveInstruction(station.ActiveTask, "Move", "Default",
MotionType.Linear, target.WorkObject.Name,
target.Name, station.ActiveTask.ActiveTool.Name);
myPath.Instructions.Add(moveInstruction);
}
ArrayList messages = new ArrayList();
station.ActiveTask.SyncPathProcedure(myPath.ModuleName + "/" + myPath.Name,
SyncDirection.ToController,
messages);
targets.Clear();
}
catch (Exception ex)
{
Project.UndoContext.CancelUndoStep(CancelUndoStepType.Rollback);
Logger.AddMessage(new LogMessage(ex.Message.ToString()));
}
finally
{
Project.UndoContext.EndUndoStep();
}
return("Path " + procedureName + " created");
}
public static RsActionInstruction CreateSetBrush(RsMoveInstruction insertBefore, Axis axis, int value, string brushNum)
{
//:TODO: Check that SetBrush template is available?
var path = (RsPathProcedure)insertBefore.Parent;
var task = (RsTask)path.Parent;
int index = path.Instructions.IndexOf(insertBefore);
var ai = new RsActionInstruction(task, "SetBrush", axis.ToString());
ai.InstructionArguments[axis.ToString()].Value = value.ToString();
ai.InstructionArguments["BrushNumber"].Value = brushNum;
path.Instructions.Insert(index, ai);
return(ai);
}
public static RsMoveInstruction InsertMoveInstruction(PathSegment hitObject, Matrix4 frame)
{
var nextInstruction = hitObject.MoveInstruction;
var path = (RsPathProcedure)nextInstruction.Parent;
var task = (RsTask)path.Parent;
var wobj = nextInstruction.GetToTarget().WorkObject;
var robTarget = CreateRobTargetAndTarget(frame, task, wobj);
// Take from nextInstruction instead?
var procDef = task.ActiveProcessDefinition;
var procTemplate = procDef.ActiveProcessTemplate;
var instruction = new RsMoveInstruction(task, procDef.Name, procTemplate.Name, MotionType.Linear, wobj.Name, robTarget.Name, task.ActiveTool.Name);
path.Instructions.Insert(path.Instructions.IndexOf(nextInstruction), instruction);
return(instruction);
}
public override void Update(VrSession session)
{
var ctrl = session.RightController;
if (_dragMoveInstr != null)
{
if (ctrl.InputState.TriggerPressed)
{
//double newZoneVal =
}
else
{
_dragMoveInstr = null;
}
}
var ctrlPos = ctrl.PointerTransform.Translation;
var hitResult = _detector.DetectMoveInstruction(ctrlPos) as HitResult <RsMoveInstruction>;
if (hitResult != null)
{
//:TODO: Update some preview
if (ctrl.InputState.TriggerPressed && !ctrl.PreviousInputState.TriggerPressed)
{
// Improve?
_zoneArg = hitResult.HitObject.InstructionArguments["Z"];
if (_zoneArg == null)
{
return;
}
int zoneVal = 0;
if (_zoneArg.Name.StartsWith("z") && _zoneArg.Enabled)
{
int.TryParse(_zoneArg.Value, out zoneVal);
}
_distOffset = ctrlPos.Distance(hitResult.HitPoint) - zoneVal;
_dragMoveInstr = hitResult.HitObject;
}
}
}
public IHitResult DetectMoveInstruction(Vector3 testPoint)
{
var instructions = AllPaths
.Where(p => BoxCull(p, testPoint))
.SelectMany(p => p.Instructions)
.OfType <RsMoveInstruction>();
double minSquareDistance = double.PositiveInfinity;
RsMoveInstruction closestInstruction = null;
foreach (var instruction in instructions)
{
var toRobTarget = GetTarget(instruction);
// If it is a joint target.
if (toRobTarget == null)
{
continue;
}
var p = toRobTarget.GetGfx().GlobalMatrix.Translation;
double distance = p.SquareDistance(testPoint);
if (distance < minSquareDistance)
{
minSquareDistance = distance;
closestInstruction = instruction;
}
}
if (minSquareDistance < MaximumDetectionDistance * MaximumDetectionDistance)
{
return(new HitResult <RsMoveInstruction>(
closestInstruction,
minSquareDistance,
GetTarget(closestInstruction).GetGfx().GlobalMatrix.Translation));
}
else
{
return(null);
}
}
private static void CreatePath(RsToolData SelectedToolData, RsWorkObject SelectedWorkObject, string speed, string zone)
{
Project.UndoContext.BeginUndoStep("RsPathProcedure Create");
try
{
//Get the active Station
Station station = Project.ActiveProject as Station;
// Create a PathProcedure.
RsPathProcedure myPath = new RsPathProcedure("AutoPath" + pathnumber);
// Add the path to the ActiveTask.
station.ActiveTask.PathProcedures.Add(myPath);
myPath.ModuleName = "module1";
myPath.ShowName = true;
myPath.Synchronize = true;
myPath.Visible = true;
//Make the path procedure as active path procedure
station.ActiveTask.ActivePathProcedure = myPath;
int i = 0;
//Create Path
foreach (RsTarget target in station.ActiveTask.Targets)
{
if (i == 0)
{
// new RsMoveInstruction(station.ActiveTask, "Move", "Default",
//MotionType.Joint, station.ActiveTask.ActiveWorkObject.Name, target.Name, station.ActiveTask.ActiveTool.Name);
RsMoveInstruction moveInstruction =
new RsMoveInstruction(station.ActiveTask, "Move", "Default",
MotionType.Joint, SelectedWorkObject.Name, target.Name, SelectedToolData.Name);
moveInstruction.InstructionArguments["Speed"].Value = speed;
moveInstruction.InstructionArguments["Zone"].Value = zone;
myPath.Instructions.Add(moveInstruction);
}
else
{
RsMoveInstruction moveInstruction =
new RsMoveInstruction(station.ActiveTask, "Move", "Default", MotionType.Linear, SelectedWorkObject.Name,
target.Name, SelectedToolData.Name);
moveInstruction.InstructionArguments["Speed"].Value = speed;
moveInstruction.InstructionArguments["Zone"].Value = zone;
myPath.Instructions.Add(moveInstruction);
}
i++;
pathnumber++;
}
}
catch (Exception ex)
{
Project.UndoContext.CancelUndoStep(CancelUndoStepType.Rollback);
Logger.AddMessage(new LogMessage(ex.Message.ToString()));
}
finally
{
Project.UndoContext.EndUndoStep();
}
}
public PathSegment(RsTarget previousTarget, RsMoveInstruction moveInstruction)
{
PreviousTarget = previousTarget;
MoveInstruction = moveInstruction;
}
public override void Update(VrUpdateArgs args)
{
var session = args.Session;
DeleteBrushTempGfx();
var input = session.SemanticInput();
if (_trackedBrushInstruction != null)
{
if (input.IsSelectPressed)
{
// Drag brush
var relatedMI = FindRelatedMoveInstruction(_trackedBrushInstruction);
if (relatedMI != null)
{
Matrix4 wobjMat = relatedMI.GetWorkObject().ObjectFrame.GlobalMatrix;
Vector3 point = wobjMat.InverseRigid().MultiplyPoint(session.RightController.PointerTransform.Translation);
var pos = PathEditingHelper.GetBrushEventPosition(_trackedBrushInstruction);
string val = ((int)(point[(int)pos.Item1 - 1] * 1000)).ToString();
if (val != pos.Item2.Value)
{
WithUndoAppend("VR Move SetBrush", () => { pos.Item2.Value = val; });
}
_brushSelector.Caption = _trackedBrushInstruction.DisplayName;
}
return;
}
}
var hitObj = GetHitObject(session.RightController.PointerTransform.Translation);
var trackedMoveInstruction = hitObj?.Item1 as RsMoveInstruction;
if (trackedMoveInstruction != null)
{
//:TODO: Supported for PaintC??
if (trackedMoveInstruction.Name != "PaintL" && trackedMoveInstruction.Name != "PaintC" ||
trackedMoveInstruction.GetToTarget() == null)
{
trackedMoveInstruction = null;
}
}
var trackedBrushInstr = hitObj?.Item1 as RsActionInstruction;
if (trackedBrushInstr != null && !string.Equals(trackedBrushInstr.Name, "SetBrush", StringComparison.OrdinalIgnoreCase))
{
trackedBrushInstr = null;
}
if (trackedMoveInstruction != _trackedMoveInstruction)
{
_trackedMoveInstruction = trackedMoveInstruction;
if (_brushSelector == null)
{
_brushSelector = new VrScrollSelector(session.RightController, "", _brushValues, _lastBrushNum);
}
_brushSelector.Caption = (_trackedMoveInstruction != null) ? "Add SetBrush" : "Brush Number";
}
else if (trackedBrushInstr != _trackedBrushInstruction || _brushSelector == null)
{
if (_brushSelector != null)
{
_brushSelector.Dispose();
}
_trackedBrushInstruction = trackedBrushInstr;
if (_trackedBrushInstruction != null)
{
var numarg = _trackedBrushInstruction.InstructionArguments["BrushNumber"];
_brushSelector = new VrScrollSelector(session.RightController, _trackedBrushInstruction.DisplayName, _brushValues, numarg.Value);
}
else
{
_brushSelector = new VrScrollSelector(session.RightController, (_trackedMoveInstruction != null) ? "Add SetBrush" : "Brush Number", _brushValues, _lastBrushNum);
}
}
if (_trackedBrushInstruction != null && input.DeleteClick)
{
var path = (RsPathProcedure)_trackedBrushInstruction.Parent;
WithUndo("VR Delete SetBrush", () => { path.Instructions.Remove(_trackedBrushInstruction); });
_trackedBrushInstruction = null;
return;
}
bool brushNumUpdated = _brushSelector.Update();
if (brushNumUpdated)
{
_lastBrushNum = _brushSelector.SelectedValue;
}
if (_trackedMoveInstruction != null && _trackedBrushInstruction == null)
{
Matrix4 wobjMat = _trackedMoveInstruction.GetWorkObject().ObjectFrame.GlobalMatrix;
CreateBrushPlanePreview(hitObj.Item2, wobjMat);
if (input.SelectClick)
{
Vector3 point = wobjMat.InverseRigid().MultiplyPoint(hitObj.Item2);
var axis = ComputeBrushAxis();
int val = (int)(point[(int)axis - 1] * 1000);
WithUndo("VR Create SetBrush", () =>
{
PathEditingHelper.CreateSetBrush(
_trackedMoveInstruction,
axis,
val,
_brushSelector.SelectedValue);
}
);
DeleteBrushTempGfx();
}
}
else if (_trackedBrushInstruction != null && brushNumUpdated)
{
var numarg = _trackedBrushInstruction.InstructionArguments["BrushNumber"];
WithUndo("VR Edit SetBrush", () => { numarg.Value = _brushSelector.SelectedValue; });
_brushSelector.Caption = _trackedBrushInstruction.DisplayName;
}
}
public override void Update(VrUpdateArgs args)
{
var session = args.Session;
// KNARK - Rift
var wand = session.RightController as VrViveWandController;
if (wand == null)
{
return;
}
RsMoveInstruction mi = null;
if (_currentMoveInstruction != null)
{
// "Sticky" selection - don't remove the UI until 1) user releases the touchpad and 2) moves controller a bit away from path
//:NOTE: Sticky distance can be annoying when editing nearby segments - review. Use shorter dist if another MI is closer/picked?
if (wand.InputState.IsTouchPadTouched || _pickedPos.Distance(wand.PointerTransform.Translation) < _stickyDist && _hasScrolled)
{
mi = _currentMoveInstruction;
}
}
if (mi == null && !wand.InputState.IsGripPressed && !wand.InputState.IsTouchPadPressed && !wand.InputState.IsTriggerPressed)
{
mi = GetPickedMoveInstruction(session);
}
if (mi != _currentMoveInstruction)
{
// Selected MI has changed, delete any old UI and create a new if needed
if (_selector != null)
{
_selector.Dispose(); _selector = null; _hasScrolled = false;
}
_currentMoveInstruction = mi;
if (mi != null)
{
_pickedPos = session.RightController.PointerTransform.Translation;
var task = mi.GetInternalParentOfType <RsTask>();
//:TODO: Some paint stations have too many speeddatas for this type of UI to make sense. How to handle?
string taskName = task.Name;
if (string.IsNullOrEmpty(taskName))
{
taskName = "T_ROB1"; // station/dummy tasks
}
var speeds = task.GetTaskContext().SymbolTable.GetVisibleDataDeclarations("/RAPID/" + taskName, "speeddata")
.Where(s => !s.Name.StartsWith("vrot")) //???
.Take(60) //TEST
.Select(s => s.Name)
.ToList();
speeds.Sort(ABB.Robotics.RobotStudio.UI.UIServices.NaturalOrderSort);
string speed = GetSpeedArg(mi).Value;
int tmp = speeds.FindIndex(s => s.Equals(speed, StringComparison.OrdinalIgnoreCase));
if (tmp == -1)
{
speeds.Insert(0, "Unknown");
speed = speeds[0];
}
else
{
speed = speeds[tmp]; // fix capitalization
}
string title = "->" + mi.GetToTarget().Name; // TEST - what do we want here?
_selector = new VrScrollSelector(session.RightController, title, speeds, speed);
_newSelection = true;
}
}
else if (_currentMoveInstruction != null && _selector != null)
{
if (wand.InputState.IsTouchPadTouched)
{
_hasScrolled = true;
}
// Same selection - update _selector and apply results
bool ok = _selector.Update();
if (ok && _selector.SelectedValue != "Unknown")
{
Action foo = () => { GetSpeedArg(_currentMoveInstruction).Value = _selector.SelectedValue; };
if (_newSelection)
{
_newSelection = false;
WithUndo("VR Edit Speed", foo);
}
else
{
WithUndoAppend("VR Edit Speed", foo);
}
}
}
}
RsInstructionArgument GetSpeedArg(RsMoveInstruction mi)
{
return(mi.GetArgumentsByDataType("speeddata").FirstOrDefault());
}
