public void Test()
{
Log += "\nBEGIN_TEST\n";
current = new Pose();
Log += current.ToString() + "\n";
computeFitness(current);
Log += current.ToString() + "\n";
Log += "END_TEST\n\n";
}
void RpcControlAnimator(int targetHand, Pose newPose, Defs.WeaponType weapon)
{
//Pose
pose = newPose;
//Reset All Bools
foreach (AnimatorControllerParameter parameter in anim.parameters)
{
if (parameter.type == AnimatorControllerParameterType.Bool)
{
anim.SetBool(parameter.name, false);
}
}
//Set Target Bool
string hand;
if (targetHand == 0)
{
hand = "Right";
}
else
{
hand = "Left";
}
anim.SetBool(hand + newPose.ToString() + weapon.ToString(), true);
}
/// <inheritdocs />
public AnchorId AddAlignmentAnchor(string uniqueName, Pose virtualPose, Pose lockedPose)
{
FragmentId fragmentId = CurrentFragmentId;
AnchorId anchorId = ClaimAnchorId();
if (IsGlobal)
{
/// Bake in current snapshot of any application imposed transform (teleport).
virtualPose = manager.PinnedFromFrozen.Multiply(virtualPose);
}
else
{
/// For subtree, applied adjustment transform is LockedFromPinned. Remove existing
/// adjustment here by premultiplying PinnedFromLocked.
virtualPose = PinnedFromLocked.Multiply(virtualPose);
}
#if WLT_EXTRA_LOGGING
string label = "AddAlign1";
Debug.Log($"F{Time.frameCount} {label} {uniqueName} vp={virtualPose.ToString("F3")} lp={lockedPose.ToString("F3")} sp={manager.SpongyFromLocked.Multiply(lockedPose).ToString("F3")}");
#endif // WLT_EXTRA_LOGGING
ReferencePose refPose = new ReferencePose()
{
name = uniqueName,
fragmentId = fragmentId,
anchorId = anchorId,
virtualPose = virtualPose
};
refPose.LockedPose = lockedPose;
referencePoses.Add(refPose);
QueueForSave(refPose);
return(anchorId);
}
public string ToString(string floatingPointformat)
{
return(string.Format("{0} scale:{1} pose:{2} size:{3} environmentTextureData:{4} trackingState:{5} nativePtr:{6}",
m_TrackableId.ToString(), m_Scale.ToString(floatingPointformat),
m_Pose.ToString(floatingPointformat), m_Size.ToString(floatingPointformat),
m_TextureDescriptor.ToString(), m_TrackingState.ToString(), m_NativePtr.ToString()));
}
/// <inheritdoc/>
public async Task <bool> Download()
{
if (!IsReady)
{
return(false);
}
bool allSuccessful = true;
List <SpacePinPegAndProps> readObjects = new List <SpacePinPegAndProps>();
List <CloudAnchorId> cloudAnchorList = new List <CloudAnchorId>();
Dictionary <CloudAnchorId, SpacePinASA> spacePinByCloudId = new Dictionary <CloudAnchorId, SpacePinASA>();
foreach (var spacePin in spacePins)
{
int bindingIdx = FindBindingBySpacePinId(spacePin.SpacePinId);
if (bindingIdx >= 0)
{
string cloudAnchorId = bindings[bindingIdx].cloudAnchorId;
cloudAnchorList.Add(cloudAnchorId);
spacePinByCloudId[cloudAnchorId] = spacePin;
}
}
if (cloudAnchorList.Count > 0)
{
var found = await publisher.Read(cloudAnchorList);
if (found != null)
{
foreach (var keyVal in found)
{
var cloudAnchorId = keyVal.Key;
var spacePin = spacePinByCloudId[cloudAnchorId];
var pegAndProps = keyVal.Value;
Debug.Assert(pegAndProps.localPeg != null);
readObjects.Add(new SpacePinPegAndProps()
{
spacePin = spacePin, pegAndProps = pegAndProps
});
}
}
else
{
SimpleConsole.AddLine(ConsoleHigh, $"publisher Read returned null looking for {cloudAnchorList.Count} ids");
}
}
var wltMgr = WorldLockingManager.GetInstance();
foreach (var readObj in readObjects)
{
Pose lockedPose = wltMgr.LockedFromFrozen.Multiply(readObj.pegAndProps.localPeg.GlobalPose);
SimpleConsole.AddLine(ConsoleLow, $"Dwn: {lockedPose.ToString("F3")}");
readObj.spacePin.SetLockedPose(lockedPose);
readObj.spacePin.SetLocalPeg(readObj.pegAndProps.localPeg);
}
return(allSuccessful);
}
private Vector3 RaycastHitPosition()
{
if (aRRaycastManager.Raycast(new Vector2(Screen.width / 2, Screen.height / 2), hits, TrackableType.PlaneWithinPolygon))
{
hitPose = hits[0].pose;
raycastPositionText.text = hitPose.ToString();
return(hitPose.position);
}
else
{
raycastPositionText.text = hitPose.ToString();
return(hitPose.position);
}
}
public ARAnchor CreateAnchor(Pose pose)
{
IntPtr poseHandle = m_ndkSession.PoseAdapter.Create(pose);
IntPtr anchorHandle = IntPtr.Zero;
ARDebug.LogInfo("native acquire anchor begin with pose:{0}", pose.ToString());
NDKARStatus status = NDKAPI.HwArSession_acquireNewAnchor(m_ndkSession.SessionHandle,
poseHandle, ref anchorHandle);
ARDebug.LogInfo("native acquire anchor end with status={0}", status);
m_ndkSession.PoseAdapter.Destroy(poseHandle);
ARExceptionAdapter.ExtractException(status);
var anchor = m_ndkSession.AnchorManager.ARAnchorFactory(anchorHandle, true);
return(anchor);
}
void RpcDirectAnimator(Pose newPose)
{
//Pose
pose = newPose;
//Reset All Bools
foreach (AnimatorControllerParameter parameter in anim.parameters)
{
if (parameter.type == AnimatorControllerParameterType.Bool)
{
anim.SetBool(parameter.name, false);
}
}
//Set Target Bool
anim.SetBool(newPose.ToString(), true);
}
private void UpdatePoseByeTrackingType(Pose pose)
{
switch (m_TrackingType)
{
case TrackingType.RotationAndPosition:
if (UseRelativeTransform)
{
transform.localRotation = pose.rotation;
transform.localPosition = pose.position;
}
else
{
transform.rotation = pose.rotation;
transform.position = pose.position;
}
NRDebugger.LogError(pose.ToString());
break;
case TrackingType.RotationOnly:
if (UseRelativeTransform)
{
transform.localRotation = pose.rotation;
}
else
{
transform.rotation = pose.rotation;
}
break;
case TrackingType.PositionOnly:
if (UseRelativeTransform)
{
transform.localPosition = pose.position;
}
else
{
transform.position = pose.position;
}
break;
default:
break;
}
}
private static void PrintDisplay()
{
// Clear the current line
Console.Write('\r');
Console.Write("[{0}]", new String('*', roll_w).PadRight(18));
Console.Write("[{0}]", new String('*', pitch_w).PadRight(18));
Console.Write("[{0}]", new String('*', yaw_w).PadRight(18));
if (onArm)
{
Console.Write("[{0}][{1}]",
whichArm.ToString()[0],
currentPose.ToString().PadRight(13));
}
else
{
Console.Write("[?][".PadRight(17) + "]");
}
}
private void UpdatePlacementPose()
{
var screenCenter = arCamera.GetComponent <Camera>().ViewportToScreenPoint(new Vector3(0.5f, 0.5f));
Debug.Log("screenCenter: " + screenCenter);
//var hits = new List<ARRaycastHit>();
arManager.Raycast(screenCenter, hits, TrackableType.Planes);
placementPoseIsValid = hits.Count > 0;
Debug.Log("placementPoseIsValid: " + placementPoseIsValid.ToString());
if (placementPoseIsValid)
{
placementPose = hits[0].pose;
Debug.Log("placementPose: " + placementPose.ToString());
var cameraForward = -1 * arCamera.GetComponent <Camera>().transform.forward;
var cameraBearing = new Vector3(cameraForward.x, 0, cameraForward.z).normalized;
placementPose.rotation = Quaternion.LookRotation(cameraBearing);
}
}
/// <inheritdoc/>
public async Task <bool> Search()
{
if (!IsReady)
{
return(false);
}
Dictionary <CloudAnchorId, LocalPegAndProperties> found = await publisher.Find(searchRadius);
var wltMgr = WorldLockingManager.GetInstance();
bool foundAny = false;
foreach (var keyval in found)
{
string spacePinId = keyval.Value.properties[SpacePinIdKey];
string cloudAnchorId = keyval.Key;
var pegAndProps = keyval.Value;
int idx = FindSpacePinById(spacePinId);
if (idx >= 0)
{
CreateBinding(spacePinId, cloudAnchorId);
foundAny = true;
SpacePinASA spacePin = spacePins[idx];
Pose lockedPose = wltMgr.LockedFromFrozen.Multiply(pegAndProps.localPeg.GlobalPose);
SimpleConsole.AddLine(ConsoleLow, $"Srch: {lockedPose.ToString("F3")}");
spacePin.SetLockedPose(lockedPose);
spacePin.SetLocalPeg(pegAndProps.localPeg);
}
else
{
SimpleConsole.AddLine(ConsoleHigh, $"Found anchor for unknown SpacePin={spacePinId}.");
}
}
return(foundAny);
}
//Gets the current Pose From the user and fires off the Command associated
private async void Myo_PoseChanged(object sender, PoseEventArgs e)
{
Pose curPose = e.Pose;
await Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
BitmapImage bitmapImage = null;
//Path where all the images are stored
string imgPath = "ms-appx://GUI_Ev3_Myo_Robot/Assets/";
//Displays the Current Pose To the Screen
tblUpdates.Text = curPose.ToString();
//Sets the _currentPose to the current pose
_CurrentPose = curPose;
if (_IsRobotRunning)
{
switch (curPose)
{
case Pose.Rest:
RobotStop();
bitmapImage = new BitmapImage(new Uri(imgPath + "Rest.png"));
break;
case Pose.Fist:
RobotFoward();
bitmapImage = new BitmapImage(new Uri(imgPath + "Fist.png"));
break;
case Pose.WaveIn:
RobotLeft();
bitmapImage = new BitmapImage(new Uri(imgPath + "WaveIn.png"));
break;
case Pose.WaveOut:
RobotRight();
bitmapImage = new BitmapImage(new Uri(imgPath + "WaveOut.png"));
break;
case Pose.FingersSpread:
RobotBackward();
bitmapImage = new BitmapImage(new Uri(imgPath + "FingerSpread.png"));
break;
case Pose.DoubleTap:
if (_checkPose[Pose.DoubleTap] == false)
{
RobotLift();
_checkPose[Pose.DoubleTap] = true;
}
else
{
RobotDrop();
_checkPose[Pose.DoubleTap] = false;
}
bitmapImage = new BitmapImage(new Uri(imgPath + "Pinch.png"));
break;
case Pose.Unknown:
bitmapImage = new BitmapImage(new Uri(imgPath + "Unknown.png"));
break;
}
ImgCurPose.Source = bitmapImage;
}
});
}
/// <summary>
/// Generates a string representation of this <see cref="XRHumanBodyJoint"/>.
/// </summary>
/// <param name="format">A format specifier used for the floating point fields.</param>
/// <returns>A string representation of this <see cref="XRHumanBodyJoint"/>.</returns>
public string ToString(string format)
{
return(String.Format("joint [{0}] -> [{1}] localScale:{2} localPose:{3} anchorScale:{4} anchorPose:{5} tracked:{6}",
m_Index, m_ParentIndex, m_LocalScale.ToString(format), m_LocalPose.ToString(format),
m_AnchorScale.ToString(format), m_AnchorPose.ToString(format), tracked.ToString()));
}
public async Task <bool> SendPose(Pose pose)
{
var data = pose.ToString();
return(await WriteData(data));
}
/// <summary>
/// Communicate the data from this point to the alignment manager.
/// </summary>
/// <param name="mgr"></param>
protected void PushAlignmentData(IAlignmentManager mgr)
{
DebugLogExtra($"F{Time.frameCount} Push: {name}: MPG={ModelingPoseGlobal.ToString("F3")} GfP={GlobalFromParent.ToString("F3")} R={restorePoseLocal.ToString("F3")} MPP={WorldLockingManager.GetInstance().PinnedFromFrozen.Multiply(ModelingPoseGlobal)}");
if (PinActive)
{
mgr.RemoveAlignmentAnchor(AnchorId);
}
AnchorId = mgr.AddAlignmentAnchor(AnchorName, ModelingPoseGlobal, lockedPose);
}
//Generate SDF file.
private void GenerateSDF()
{
UnitsOfMeasure oUOM = _invApp.ActiveDocument.UnitsOfMeasure;
AssemblyDocument oAsmDoc = (AssemblyDocument)_invApp.ActiveDocument;
AssemblyComponentDefinition oAsmCompDef = oAsmDoc.ComponentDefinition;
List <ComponentOccurrence> compOccurs = new List <ComponentOccurrence>();
List <AssemblyConstraint> constraints = new List <AssemblyConstraint>();
//Recursively make a list of all component parts.
List <AssemblyComponentDefinition> loopList = new List <AssemblyComponentDefinition>();
//TODO: Store OccuranceName vs Pose for assemblies.
loopList.Add((AssemblyComponentDefinition)oAsmCompDef);
//Recursively make a list of all component parts.
while (loopList.Count > 0)
{
AssemblyComponentDefinition loopAsmCompDef = loopList[0];
loopList.RemoveAt(0);
foreach (ComponentOccurrence occ in loopAsmCompDef.Occurrences)
{
if (occ.DefinitionDocumentType == DocumentTypeEnum.kPartDocumentObject || occ.Definition is WeldmentComponentDefinition)
{
//Store parts and weldments for link creation
compOccurs.Add(occ);
//occ.
WriteLine("Processing part '" + occ.Name + "' with " + occ.Constraints.Count + " constraints.");
}
else if (occ.DefinitionDocumentType == DocumentTypeEnum.kAssemblyDocumentObject)
{
//Get parts from assemblies.
loopList.Add((AssemblyComponentDefinition)occ.Definition);
WriteLine("Processing assembly '" + occ.Name + "' with " + occ.Constraints.Count + " constraints.");
}
}
//Get Assembly Constraints
foreach (AssemblyConstraint cons in loopAsmCompDef.Constraints)
{
constraints.Add(cons);
}
}
WriteLine(compOccurs.Count.ToString() + " parts to convert.");
WriteLine(constraints.Count.ToString() + " constraints to convert.");
//Get all the available links
foreach (ComponentOccurrence oCompOccur in compOccurs)
{
//Define a link
Link link = new Link(RemoveColon(oCompOccur.Name));
//Get global position and COM
double Mass = oCompOccur.MassProperties.Mass;
//Calculate rotation.
Inventor.Matrix R1 = oCompOccur.Transformation;
// Set position and rotation
//TODO: Check globalised scale
//TODO: Set pose relative to parent occurance.
link.Pose = new Pose(R1, precision, scale);
// Get Moments of Inertia.
double[] iXYZ = new double[6];
//Pos of C.O.M., Rot of Link.
// Ixx, Iyy, Izz, Ixy, Iyz, Ixz
// Ixx, Ixy, Ixz, Iyy, Iyz, Izz
oCompOccur.MassProperties.XYZMomentsOfInertia(out iXYZ[0], out iXYZ[3], out iXYZ[5], out iXYZ[1], out iXYZ[4], out iXYZ[2]);
Inventor.Point COMp = oCompOccur.MassProperties.CenterOfMass;
double[] COM = new double[3] {
COMp.X, COMp.Y, COMp.Z
};
// Set Moments of Inertia
//Takes precision from link.Pose.
link.Inertial = new Inertial(Mass, iXYZ, COM, link.Pose, scale);
// Set the URI for the link's model.
String URI = "model://<MODELNAME>/meshes/" + link.Name + ".stl";
link.Visual = new Visual(new Mesh(URI, scale));
link.Collision = new Collision(new Mesh(URI, scale));
// Print out link information.
WriteLine("New Link: --------------------------------------");
WriteLine(" Name: " + link.Name);
WriteLine(" Pose: " + link.Pose.ToString());
//link.Pose = new Pose(link.Pose);
WriteLine(" Pose: " + link.Pose.ToString());
WriteLine(" InertiaPose: " + link.Inertial.Pose.ToString());
Matrix <double> M2 = DenseMatrix.OfArray(link.Inertial.InertiaMatrix);
WriteLine(M2.ToString());
WriteLine(" Mass: " + Mass);
WriteLine(" Rotation Matrix: " + System.Environment.NewLine + link.Pose.PrintMatrix());
// Add link to robot
robo.Links.Add(link);
}
WriteLine(constraints.Count.ToString() + " constraints to convert.");
//Get all the available joints
foreach (AssemblyConstraint constraint in constraints)
{
//Some checks
if (constraint.Suppressed)
{
//Skip suppressed constraints.
WriteLine("Skipped a suppressed constraint.");
continue;
}
//Set some starting variables.
String name = constraint.Name;
Inventor.Point center;
JointType type = JointType.Revolute;
//Calculate which should be child.
ComponentOccurrence childP = constraint.OccurrenceOne;
ComponentOccurrence parentP = constraint.OccurrenceTwo;
if (childP == null || parentP == null)
{
//Skip incomplete constraints
WriteLine("Skipped a constraint without an Occurance.");
continue;
}
//Get degrees of freedom information.
int transDOFCount, rotDOFCount;
Inventor.ObjectsEnumerator transDOF, rotDOF;
Inventor.Point DOFCenter;
childP.GetDegreesOfFreedom(out transDOFCount, out transDOF, out rotDOFCount, out rotDOF, out DOFCenter);
if (rotDOFCount + transDOFCount == 0)
{
parentP.GetDegreesOfFreedom(out transDOFCount, out transDOF, out rotDOFCount, out rotDOF, out DOFCenter);
if (rotDOFCount + transDOFCount == 0)
{
WriteLine("Skipped a constraint with 0 DOF.");
continue;
}
//Parent has DOF but child doesn't. Switch Parent and Child.
ComponentOccurrence temp = childP;
childP = parentP;
parentP = temp;
}
//Get Model Links
Link child = GetLinkByName(RemoveColon(childP.Name));
Link parent = GetLinkByName(RemoveColon(parentP.Name));
if (child == null || parent == null)
{
//Skip incomplete constraints
WriteLine("Skipped a constraint without a Link.");
continue;
}
//Get constraint information
Pose Pose;
Axis Axis;
Inventor.Point Geomcenter;
if (constraint is InsertConstraint && constraint.GeometryOne is Circle)
{
//Handle Insert Constraints (Revolute)
Circle circ = (Circle)constraint.GeometryOne;
Geomcenter = circ.Center;
Axis = new Axis(circ.Normal.X, circ.Normal.Y, circ.Normal.Z);
type = JointType.Revolute;
}
else if (constraint is MateConstraint && constraint.GeometryOne is Line)
{
//Handle Mate Constraints (Prismatic)
Inventor.Line line = (Line)constraint.GeometryOne;
Geomcenter = line.RootPoint;
//TODO: Find out if axis is global, or local to assembly.
Axis = new Axis(line.Direction.X, line.Direction.Y, line.Direction.Z);
type = JointType.Prismatic;
}
else
{
//Skip other constraints
WriteLine("Skipped an uknown constraint");
continue;
}
Pose = new Pose(Geomcenter.X, Geomcenter.Y, Geomcenter.Z, precision, scale);
WriteLine("New joint: --------------------------------------");
WriteLine(" Name: " + name);
WriteLine(" Parent: " + parent.Name);
WriteLine(" Child: " + child.Name);
WriteLine(" Pose: " + Pose.ToString());
Pose.GetPosRelativeTo(child.Pose);
WriteLine(" Child Loc: " + child.Pose.ToString());
WriteLine(" Relative Pose: " + Pose.ToString());
WriteLine(" Axis: " + Axis.ToString());
WriteLine(" rotDOF: " + rotDOFCount + " transDOF: " + transDOFCount);
//Add the joint to the robot
Joint joint = new Joint(name, type);
joint.Axis = Axis;
joint.Parent = parent;
joint.Child = child;
joint.Pose = Pose;
robo.Joints.Add(joint);
}
//Saving turned on?
if (this.checkBox2.Checked)
{
// Save the SDF
FolderBrowserDialog folderDialog1 = new FolderBrowserDialog();
if (folderDialog1.ShowDialog() == DialogResult.OK)
{
robo.WriteSDFToFile(folderDialog1.SelectedPath, precision);
}
//Save STLs?
foreach (ComponentOccurrence oCompOccur in compOccurs)
{
if (oCompOccur.DefinitionDocumentType == DocumentTypeEnum.kPartDocumentObject && this.checkBox1.Checked)
{
PartDocument partDoc = (PartDocument)oCompOccur.Definition.Document;
partDoc.SaveAs(folderDialog1.SelectedPath + "\\meshes\\" + RemoveColon(oCompOccur.Name) + ".stl", true);
WriteLine("Finished saving: " + folderDialog1.SelectedPath + "\\meshes\\" + RemoveColon(oCompOccur.Name) + ".stl");
}
}
}
#region oldcode
//foreach (ComponentOccurrence oCompOccur in oAsmCompDef.Occurrences)
//{
// // Generate links from available subassemblies in main assembly.
// //Debugger.Break();
// //New Link
// robo.Links.Add(new Link(oCompOccur.Name));
// int c = robo.Links.Count - 1;
// WriteLine("Added Link: "+ robo.Links[c].Name +", link count: " + robo.Links.Count.ToString());
// //Find and set parent link
// for (int i = 0; i < robo.Links.Count; i++)
// {
// if (String.Equals(robo.Links[i].Name, ReturnParentName(oCompOccur)))
// {
// robo.Links[c].Parent = robo.Links[i];
// WriteLine("Link's parent: " + robo.Links[i].Name);
// }
// }
// //If link has a parent
// if (robo.Links[c].Parent != null)
// {
// //Define a joint
// robo.Joints.Add(new Joint(FormatJointName(robo.Links[c].Name), JointType.Revolute, robo.Links[c].Parent, robo.Links[c]));
// int j = robo.Joints.Count - 1;
// //Parse joint axis
// switch (robo.Joints[j].Name[robo.Joints[j].Name.Length - 1])
// {
// case 'R':
// robo.Joints[j].Axis = new double[] { 1, 0, 0 };
// break;
// case 'P':
// robo.Joints[j].Axis = new double[] { 0, 1, 0 };
// break;
// case 'Y':
// robo.Joints[j].Axis = new double[] { 0, 0, 1 };
// break;
// default:
// break;
// }
// }
// // Get mass properties for each link.
// double[] iXYZ = new double[6];
// oCompOccur.MassProperties.XYZMomentsOfInertia(out iXYZ[0], out iXYZ[3], out iXYZ[5], out iXYZ[1], out iXYZ[4], out iXYZ[2]); // Ixx, Iyy, Izz, Ixy, Iyz, Ixz -> Ixx, Ixy, Ixz, Iyy, Iyz, Izz
// robo.Links[c].Inertial = new Inertial(oCompOccur.MassProperties.Mass, iXYZ);
// robo.Links[c].Inertial.XYZ = FindCenterOfMassOffset(oCompOccur);
// // Set shape properties for each link.
// robo.Links[c].Visual = new Visual(new Mesh("package://" + robo.Name + "/" + robo.Links[c].Name + ".stl"));
//}
#endregion
}
protected override void SolveInstance(IGH_DataAccess DA)
{
bool activate = false;
bool scan = false;
bool kill = false;
bool clear = false;
string ip = System.String.Empty;
int index = 0;
bool connect = false;
bool resetPP = false;
bool begin = false;
bool stop = false;
bool monitorTCP = false;
bool monitorIO = false;
bool stream = false;
Target targ = null;
if (!DA.GetData(0, ref activate))
{
;
}
if (!DA.GetData(1, ref scan))
{
;
}
if (!DA.GetData(2, ref kill))
{
;
}
if (!DA.GetData(3, ref clear))
{
;
}
if (!DA.GetData(4, ref ip))
{
;
}
if (!DA.GetData(5, ref index))
{
;
}
if (!DA.GetData(6, ref connect))
{
;
}
if (!DA.GetData(7, ref resetPP))
{
;
}
if (!DA.GetData(8, ref begin))
{
;
}
if (!DA.GetData(9, ref stop))
{
;
}
if (!DA.GetData(10, ref monitorTCP))
{
;
}
if (!DA.GetData(11, ref monitorIO))
{
;
}
if (!DA.GetData(12, ref stream))
{
;
}
if (!DA.GetData(13, ref targ))
{
;
}
this.controllerIndex = index;
this.start = begin;
double cRobX = 0;
double cRobY = 0;
double cRobZ = 0;
double cRobQ1 = 0;
double cRobQ2 = 0;
double cRobQ3 = 0;
double cRobQ4 = 0;
Quaternion cRobQuat = new Quaternion();
//Check for valid licence
/*if (bool (Core.AuthTest.loggedIn) != true)
* {
* new GH_RuntimeMessage("Please sign in to confirm your licence", GH_RuntimeMessageLevel.Error);
* return;
* }*/
if (activate)
{
if (scan)
{
// Scan the network for controllers and add them to our controller array
scanner.Scan();
controllers = scanner.GetControllers();
if (controllers.Length > 0)
{
log.Add("Controllers found:");
// List the controller names that were found on the network.
for (int i = 0; i < controllers.Length; i++)
{
log.Add(controllers[i].ControllerName);
}
}
else
{
log.Add("Scan timed out. No controllers were found.");
}
}
if (kill && controller != null)
{
controller.Logoff();
controller.Dispose();
controller = null;
log.Add("Process killed! Abandon ship!");
}
if (clear)
{
log.Clear();
log.Add("Log cleared.");
}
if (connect)
{
if (controller == null && controllers.Length > 0)
{
string controllerID = controllers[index].ControllerName;
log.Add("Selected robot controller: " + controllers[index].ControllerName + ".");
if (controllers[index].Availability == Availability.Available)
{
log.Add("Robot controller " + controllers[index].ControllerName + " is available.");
if (controller != null)
{
controller.Logoff();
controller.Dispose();
controller = null;
}
controller = ControllerFactory.CreateFrom(controllers[index]);
controller.Logon(UserInfo.DefaultUser);
log.Add("Connection to robot controller " + controller.SystemName + " established.");
// Get T_ROB1 queue to send messages to the RAPID task.
if (!controller.Ipc.Exists("RMQ_T_ROB1"))
{
controller.Ipc.CreateQueue("RMQ_T_ROB1", 10, Ipc.MaxMessageSize);
}
tasks = controller.Rapid.GetTasks();
IpcQueue robotQueue = controller.Ipc.GetQueue("RMQ_T_ROB1");
int queueID = robotQueue.QueueId;
string queueName = robotQueue.QueueName;
log.Add("Established RMQ for T_ROB1 on network controller.");
log.Add("Rapid Message Queue ID:" + queueID.ToString() + ".");
log.Add("Rapid Message Queue Name:" + queueName + ".");
RobotQueue = robotQueue;
}
else
{
log.Add("Selected controller not available.");
}
ControllerID = controllerID;
}
else
{
if (controller != null)
{
return;
}
else
{
string exceptionMessage = "No robot controllers found on network.";
ControllerID = "No Controller";
log.Add(exceptionMessage);
}
}
}
if (resetPP && controller != null)
{
using (Mastership m = Mastership.Request(controller.Rapid))
{
// Reset program pointer to main.
tasks[0].ResetProgramPointer();
log.Add("Program pointer set to main on the active task.");
}
}
if (begin)
{
// Execute robot tasks present on controller.
try
{
if (controller.OperatingMode == ControllerOperatingMode.Auto)
{
using (Mastership m = Mastership.Request(controller.Rapid))
{
// Perform operation.
tasks[0].Start();
log.Add("Robot program started on robot " + controller.SystemName + ".");
}
}
else
{
log.Add("Automatic mode is required to start execution from a remote client.");
}
}
catch (System.InvalidOperationException ex)
{
log.Add("Mastership is held by another client." + ex.Message);
}
catch (System.Exception ex)
{
log.Add("Unexpected error occurred: " + ex.Message);
}
}
if (stop)
{
try
{
if (controller.OperatingMode == ControllerOperatingMode.Auto)
{
using (Mastership m = Mastership.Request(controller.Rapid))
{
// Stop operation.
tasks[0].Stop(StopMode.Immediate);
log.Add("Robot program stopped on robot " + controller.SystemName + ".");
}
}
else
{
log.Add("Automatic mode is required to stop execution from a remote client.");
}
}
catch (System.InvalidOperationException ex)
{
log.Add("Mastership is held by another client." + ex.Message);
}
catch (System.Exception ex)
{
log.Add("Unexpected error occurred: " + ex.Message);
}
}
// If active, update the status of the TCP.
if (monitorTCP)
{
if (tcpMonitoringOn == 0)
{
log.Add("TCP monitoring started.");
}
cRobTarg = tasks[0].GetRobTarget();
cRobX = Math.Round(cRobTarg.Trans.X, 3);
cRobY = Math.Round(cRobTarg.Trans.Y, 3);
cRobZ = Math.Round(cRobTarg.Trans.Z, 3);
Point3d cRobPos = new Point3d(cRobX, cRobY, cRobZ);
cRobQ1 = Math.Round(cRobTarg.Rot.Q1, 5);
cRobQ2 = Math.Round(cRobTarg.Rot.Q2, 5);
cRobQ3 = Math.Round(cRobTarg.Rot.Q3, 5);
cRobQ4 = Math.Round(cRobTarg.Rot.Q4, 5);
cRobQuat = new Quaternion(cRobQ1, cRobQ2, cRobQ3, cRobQ4);
tcp = Util.QuaternionToPlane(cRobPos, cRobQuat);
tcpMonitoringOn += 1;
tcpMonitoringOff = 0;
}
else if (tcpMonitoringOn > 0)
{
if (tcpMonitoringOff == 0)
{
log.Add("TCP monitoring stopped.");
}
tcpMonitoringOff += 1;
tcpMonitoringOn = 0;
}
// If active, update the status of the IO system.
if (monitorIO)
{
if (ioMonitoringOn == 0)
{
log.Add("Signal monitoring started.");
}
// Filter only the digital IO system signals.
IOFilterTypes dSignalFilter = IOFilterTypes.Common;
SignalCollection dSignals = controller.IOSystem.GetSignals(dSignalFilter);
IOstatus.Clear();
// Iterate through the found collection and print them to the IO monitoring list.
foreach (Signal signal in dSignals)
{
string sigVal = signal.ToString() + ": " + signal.Value.ToString();
IOstatus.Add(sigVal);
}
ioMonitoringOn += 1;
ioMonitoringOff = 0;
}
else if (ioMonitoringOn > 0)
{
if (ioMonitoringOff == 0)
{
log.Add("Signal monitoring stopped.");
}
ioMonitoringOff += 1;
ioMonitoringOn = 0;
}
if (stream)
{
if (targ != null)
{
IpcMessage message = new IpcMessage();
pos.X = (float)targ.Position.X;
pos.Y = (float)targ.Position.Y;
pos.Z = (float)targ.Position.Z;
ori.Q1 = targ.Quaternion.A;
ori.Q2 = targ.Quaternion.B;
ori.Q3 = targ.Quaternion.C;
ori.Q4 = targ.Quaternion.D;
pose.Trans = pos;
pose.Rot = ori;
string content = "SD;[" +
"\"Linear\"," +
pos.ToString() + "," +
ori.ToString() + "," +
pose.ToString() +
"]";
byte[] data = new UTF8Encoding().GetBytes(content);
message.SetData(data);
RobotQueue.Send(message);
}
}
// Output the status of the connection.
Status = log;
DA.SetDataList(0, log);
DA.SetDataList(1, IOstatus);
DA.SetData(2, new GH_Plane(tcp));
ExpireSolution(true);
}
}
//This Region Contains Everything Myo Related.
#region MyoPoses
/* Nothing new here, methods are explained previously and just used
* within the switch statement for Myo poses.
* The methods are all pretty self-explanatory anyways. */
public async void Myo_PoseChanged(object sender, PoseEventArgs e)
{
Pose curr = e.Pose;
await Dispatcher.RunAsync(Windows.UI.Core.CoreDispatcherPriority.Normal, async() =>
{
tblUpdates.Text = curr.ToString();
switch (curr)
{
case Pose.Rest:
break;
case Pose.Fist:
eraseScribble();
break;
case Pose.WaveIn:
scrollPDFDown();
break;
case Pose.WaveOut:
scrollPDFUp();
break;
case Pose.FingersSpread:
startScribbling();
break;
case Pose.DoubleTap:
saveNote();
break;
case Pose.Unknown:
var dialog = new MessageDialog("I'm sorry, that pose seems to be unknown.");
dialog.Title = "Pose Unknown Help";
dialog.Commands.Add(new UICommand {
Label = "How to Open\\close the App with a Pose.", Id = 0
});
dialog.Commands.Add(new UICommand {
Label = "How to turn the pages with a Pose.", Id = 1
});
var res = await dialog.ShowAsync();
if ((int)res.Id == 0)
{
var dialog0 = new MessageDialog("Try a Fist gesture to close the app." + "\n"
+ "Try a Fingers-Spread gesture to Open the App. :)");
await dialog0.ShowAsync();
}
if ((int)res.Id == 1)
{
var dialog1 = new MessageDialog("Try a Wave-in gesture to turn the page."
+ "\n" + "Try a Wave-Out gesture to go back a page.");
await dialog1.ShowAsync();
}
;
break;
default:
break;
}
});
}
