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