SuccessFailurePort StopCapture() { SuccessFailurePort resultPort = new SuccessFailurePort(); new StopRequest().Send(_client); Activate( Arbiter.Receive <HresultResponse>(false, _pipeDataPort, delegate(HresultResponse result) { if (result.hr == 0) { resultPort.Post(SuccessResult.Instance); } else { resultPort.Post(Marshal.GetExceptionForHR(result.hr)); } }, test => test.type == WebcamResponse.StopCapture ) ); return(resultPort); }
/// <summary> /// Sets the Baud rate used to communicate with the LRF. /// Acceptable values are 38400, 19200 and 9600 /// </summary> public SuccessFailurePort SetDataRate(int rate) { Packet packet; switch (rate) { case 38400: packet = Packet.MonitoringMode(0x40); break; case 19200: packet = Packet.MonitoringMode(0x41); break; case 9600: packet = Packet.MonitoringMode(0x42); break; default: SuccessFailurePort port = new SuccessFailurePort(); port.Post(new ArgumentException("Baud Rate (only 9600, 19200 and 38400 supported)")); return(port); } _rate = rate; return(Send(packet)); }
SuccessFailurePort OpenCamera(CameraInstance camera) { SuccessFailurePort resultPort = new SuccessFailurePort(); new OpenRequest(camera.DevicePath).Send(_client); Activate( Arbiter.Receive <HresultResponse>(false, _pipeDataPort, delegate(HresultResponse result) { if (result.hr == 0) { resultPort.Post(SuccessResult.Instance); } else { resultPort.Post(Marshal.GetExceptionForHR(result.hr)); } }, test => test.type == WebcamResponse.OpenDevices ) ); return(resultPort); }
public SuccessFailurePort SetRate() { Tracer.Trace("CommLink::SetRate()"); SuccessFailurePort port; port = new SuccessFailurePort(); /* * if (_rate != 0) * { * SerialIOManager.SetRate setRate = new SerialIOManager.SetRate(_rate); * * _serial.OperationsPort.Post(setRate); * * port = setRate.ResponsePort; * } * else * { * port = new SuccessFailurePort(); * port.Post(new Exception("Rate not set")); * } */ return(port); }
/// <summary> /// UploadSrgsFile HTTP post handler /// </summary> /// <param name="postData"></param> /// <param name="post"></param> /// <returns></returns> private IEnumerator <ITask> UploadSrgsFileHttpPostHandler(HttpPostRequestData postData, HttpPost post) { string mountPointFileLocation = postData.Parameters["MountPointSaveLocation"]; if (string.IsNullOrEmpty(mountPointFileLocation)) { PostHttpPostParameterError(post, "No mount point save location was specified"); yield break; } HttpPostFile file = postData.Files["SrgsFile"]; if (file == null) { PostHttpPostParameterError(post, "No SRGS file uploaded"); yield break; } // Write file to mount service SuccessFailurePort writerResponsePort = new SuccessFailurePort(); yield return(new IterativeTask(delegate { return WriteFileToMountService( mountPointFileLocation, file.InputStream, writerResponsePort ); })); Exception writeException = (Exception)writerResponsePort; if (writeException != null) { LogWarning(writeException); post.ResponsePort.Post(new HttpResponseType( HttpStatusCode.BadRequest, Fault.FromException(writeException) )); yield break; } // Use newly uploaded file NameValueCollection parameters = new NameValueCollection(); parameters["SrgsFileLocation"] = mountPointFileLocation; yield return(new IterativeTask(delegate { return UseExistingSrgsFileHttpPostHandler( parameters, post ); })); // UseExistingSrgsFileHttpPostHandler already took care of posting // a result on the response port yield break; }
public SuccessFailurePort MoveTo( float FRShoulderFB, float FRShoulderUD, float FRElbow, float MRShoulderFB, float MRShoulderUD, float MRElbow, float RRShoulderFB, float RRShoulderUD, float RRElbow, float FLShoulderFB, float FLShoulderUD, float FLElbow, float time) { SuccessFailurePort responsePort = new SuccessFailurePort(); if (_moveToActive) { responsePort.Post(new Exception("Previous MoveTo still active.")); return(responsePort); } // check bounds. If the target is invalid, post an exception message to the response port with a helpful error. // set the target values on the joint descriptors _joints[0].Target = FRShoulderFB; _joints[1].Target = FRShoulderUD; _joints[2].Target = FRElbow; _joints[3].Target = MRShoulderFB; _joints[4].Target = MRShoulderUD; _joints[5].Target = MRElbow; _joints[6].Target = RRShoulderFB; _joints[7].Target = RRShoulderUD; _joints[8].Target = RRElbow; _joints[9].Target = FLShoulderFB; _joints[10].Target = FLShoulderUD; _joints[11].Target = FLElbow; // calculate a speed value for each joint that will cause it to complete its motion in the specified time for (int i = 0; i < _joints.Length; i++) { _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time; } // set this flag so that the motion is evaluated in the update method _moveToActive = true; // keep a pointer to the response port so we can post a result message to it. _moveToResponsePort = responsePort; return(responsePort); }
public IEnumerator <ITask> DeleteGrammarEntryHandler(DeleteGrammarEntry delete) { // Make sure current grammar type is dictionary-style if (this.state.GrammarType != GrammarType.DictionaryStyle) { Fault fault = Fault.FromCodeSubcodeReason( FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Cannot delete entries from grammar dictionary because grammar" + "currently in use is not DictionaryStyle." ); LogInfo(fault.ToException()); delete.ResponsePort.Post(fault); yield break; } #region Set up load grammar request and load grammar // Set up load grammar request LoadGrammarRequest loadRequest = new LoadGrammarRequest(); loadRequest.GrammarType = GrammarType.DictionaryStyle; loadRequest.DictionaryGrammar = new Dictionary <string, string>(state.DictionaryGrammar); GrammarUtilities.DeleteDictionary(loadRequest.DictionaryGrammar, delete.Body.DictionaryGrammar); // Load new grammar SuccessFailurePort loadGrammarPort = new SuccessFailurePort(); SpawnIterator <LoadGrammarRequest, SuccessFailurePort>( loadRequest, loadGrammarPort, LoadGrammar ); // Check loading outcome yield return((Choice)loadGrammarPort); Exception exception = (Exception)loadGrammarPort; if (exception != null) { LogWarning(exception); delete.ResponsePort.Post(Fault.FromCodeSubcodeReason( FaultCodes.Receiver, DsspFaultCodes.OperationFailed, exception.Message )); yield break; } #endregion SaveState(this.state); // Notify subscribers of state change Replace replace = new Replace(); replace.Body = this.state; SendNotification <Replace>(this.subMgrPort, replace); delete.ResponsePort.Post(DefaultDeleteResponseType.Instance); }
public IEnumerator <ITask> UpsertGrammarEntryHandler(UpsertGrammarEntry upsert) { if (this.state.GrammarType != GrammarType.DictionaryStyle) { // Since we are switching grammar mode make sure there exists a valid, // empty dictionary if (this.state.DictionaryGrammar == null) { this.state.DictionaryGrammar = new Dictionary <string, string>(); } else { this.state.DictionaryGrammar.Clear(); } } #region Set up load grammar request and load grammar // Set up load grammar request LoadGrammarRequest loadRequest = new LoadGrammarRequest(); loadRequest.GrammarType = GrammarType.DictionaryStyle; loadRequest.DictionaryGrammar = new Dictionary <string, string>(state.DictionaryGrammar); GrammarUtilities.UpsertDictionary(loadRequest.DictionaryGrammar, upsert.Body.DictionaryGrammar); // Load new grammar SuccessFailurePort loadGrammarPort = new SuccessFailurePort(); SpawnIterator <LoadGrammarRequest, SuccessFailurePort>( loadRequest, loadGrammarPort, LoadGrammar ); // Check loading outcome yield return((Choice)loadGrammarPort); Exception exception = (Exception)loadGrammarPort; if (exception != null) { LogWarning(exception); upsert.ResponsePort.Post(Fault.FromCodeSubcodeReason( FaultCodes.Receiver, DsspFaultCodes.OperationFailed, exception.Message )); yield break; } #endregion SaveState(this.state); // Notify subscribers of state change Replace replace = new Replace(); replace.Body = this.state; SendNotification <Replace>(this.subMgrPort, replace); upsert.ResponsePort.Post(DefaultUpsertResponseType.Instance); }
/// <summary> /// Helper function to initialize Kinect camera. /// </summary> /// <returns>CCR Iterator</returns> protected IEnumerator <ITask> InitializeKinect() { var finishedInitialization = new SuccessFailurePort(); ThreadPool.QueueUserWorkItem( param => { try { this.InitializeKinectDevice(); finishedInitialization.Post(SuccessResult.Instance); } catch (Exception e) { LogError(e); finishedInitialization.Post(new Exception(Resources.InitializeFailed)); } }); yield return(finishedInitialization.Choice( success => { base.Start(); this.StartPollingBehavior(); // Merge the internal update ports into the main interleave MainPortInterleave.CombineWith( new Interleave( new ExclusiveReceiverGroup( Arbiter.Receive( true, this.alternateContractUpdatePort, this.OnUpdateAlternates)), new ConcurrentReceiverGroup( Arbiter.Receive( true, this.scheduleNextFrameReadPort, this.OnScheduleNextFrameRead)))); LogInfo(Resources.Initialized); }, fault => { LogError(fault); base.StartFailed(); })); }
public IEnumerator <ITask> SubscribeHandler(Subscribe subscribe) { SuccessFailurePort responsePort = SubscribeHelper(this.submgrPort, subscribe.Body, subscribe.ResponsePort); yield return(responsePort.Choice()); var success = (SuccessResult)responsePort; if (success != null) { SendNotificationToTarget <Replace>(subscribe.Body.Subscriber, this.submgrPort, this.state); } yield break; }
/// <summary> /// Try finding the hid. May return failure if not found. /// </summary> public SuccessFailurePort TryFindTheHid() { Tracer.Trace("ProximityBoardCcrServiceCommander::TryFindTheHid()"); SuccessFailurePort port = new SuccessFailurePort(); if (_pmManager.PicPxMod.FindTheHid()) { port.Post(new SuccessResult()); } else { port.Post(new Exception("HID not found")); } return(port); }
/// <summary> /// Shutdown or kill process /// </summary> /// <returns>ITask enumerator</returns> private IEnumerator <ITask> ShutdownSuccessfullyOrKillProcess() { var shutdownOrTimedOut = new SuccessFailurePort(); Activate(TimeoutPort(DefaultProcessTimeout).Receive(dateTime => shutdownOrTimedOut.Post(new Exception()))); Activate(shutdownOrTimedOut.Choice( success => { }, /*cleanly shutdown*/ failure => /*timed out*/ System.Diagnostics.Process.GetCurrentProcess().Kill())); ControlPanelPort.Post(new DsspDefaultDrop()); Activate(Arbiter.ReceiveWithIterator <DsspDefaultDrop>(false, this.mainPort, this.DropHandler)); shutdownOrTimedOut.Post(SuccessResult.Instance); yield break; }
public virtual IEnumerator <ITask> AlertHandler(Alert alert) { SuccessFailurePort result = new SuccessFailurePort(); AlertForm form = null; RunForm runForm = new RunForm( delegate() { form = new AlertForm(result); form.Message = alert.Body.Message; form.Countdown = _defaultTimeout; return(form); } ); WinFormsServicePort.Post(runForm); yield return(Arbiter.Choice( runForm.pResult, delegate(SuccessResult success){}, delegate(Exception e) { result.Post(e); } )); yield return(Arbiter.Choice( result, delegate(SuccessResult success) { alert.ResponsePort.Post(DefaultSubmitResponseType.Instance); if (form.Timeout) { LogWarning("Alert dialog timed out."); } }, delegate(Exception e) { Fault fault = Fault.FromException(e); LogError(null, "Error in Alert Handler", fault); alert.ResponsePort.Post(fault); } )); }
/// <summary> /// Loads grammar on service startup /// </summary> /// <returns></returns> private IEnumerator <ITask> LoadGrammarOnStartup() { // Determine whether we have a valid grammar to load bool loadGrammar = false; switch (this.state.GrammarType) { case GrammarType.DictionaryStyle: loadGrammar = this.state.DictionaryGrammar.Count > 0; break; case GrammarType.Srgs: loadGrammar = !string.IsNullOrEmpty(this.state.SrgsFileLocation); break; } // Load grammar if (loadGrammar) { SuccessFailurePort loadGrammarPort = new SuccessFailurePort(); LoadGrammarRequest loadRequest = new LoadGrammarRequest(); loadRequest.GrammarType = this.state.GrammarType; loadRequest.DictionaryGrammar = this.state.DictionaryGrammar; loadRequest.SrgsFileLocation = this.state.SrgsFileLocation; SpawnIterator <LoadGrammarRequest, SuccessFailurePort>( loadRequest, loadGrammarPort, LoadGrammar ); yield return((Choice)loadGrammarPort); Exception exception = (Exception)loadGrammarPort; if (exception != null) { LogWarning(exception); } } StartService(); yield break; }
public SuccessFailurePort SetRate() { SuccessFailurePort port; if (_rate != 0) { SerialIOManager.SetRate setRate = new SerialIOManager.SetRate(_rate); _serial.OperationsPort.Post(setRate); port = setRate.ResponsePort; } else { port = new SuccessFailurePort(); port.Post(new Exception("Rate not set")); } return(port); }
public IEnumerator <ITask> SetSrgsGrammarFileHandler(SetSrgsGrammarFile setSrgsGrammarFile) { #region Set up load grammar request and load grammar // Set up load grammar request LoadGrammarRequest loadRequest = new LoadGrammarRequest(); loadRequest.GrammarType = GrammarType.Srgs; loadRequest.SrgsFileLocation = setSrgsGrammarFile.Body.FileLocation; // Load new grammar SuccessFailurePort loadGrammarPort = new SuccessFailurePort(); SpawnIterator <LoadGrammarRequest, SuccessFailurePort>( loadRequest, loadGrammarPort, LoadGrammar ); // Check loading outcome yield return((Choice)loadGrammarPort); Exception exception = (Exception)loadGrammarPort; if (exception != null) { LogWarning(exception); setSrgsGrammarFile.ResponsePort.Post(Fault.FromCodeSubcodeReason( FaultCodes.Receiver, DsspFaultCodes.OperationFailed, exception.Message )); yield break; } #endregion SaveState(this.state); // Notify subscribers of state change Replace replace = new Replace(); replace.Body = this.state; SendNotification <Replace>(this.subMgrPort, replace); setSrgsGrammarFile.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public IEnumerator <ITask> SubscribeHandler(Subscribe subscribe) { SubscribeRequestType request = subscribe.Body; SuccessFailurePort subscribePort = SubscribeHelper( _subMgrPort, request, subscribe.ResponsePort ); yield return((Choice)subscribePort); Exception exception = (Exception)subscribePort; if (exception != null) { LogError("Subscribe failed", exception); yield break; } SendNotificationToTarget <Replace>(request.Subscriber, _subMgrPort, _state); yield break; }
SuccessFailurePort SetFormat(Format format) { SuccessFailurePort resultPort = new SuccessFailurePort(); var setFormat = new SetFormatRequest { Width = format.Width, Height = format.Height, MinFrameRate = format.MinFramesPerSecond, MaxFrameRate = format.MaxFramesPerSecond, Compression = format.FourCcCompression }; setFormat.Send(_client); Activate( Arbiter.Receive <HresultResponse>(false, _pipeDataPort, delegate(HresultResponse result) { if (result.hr == 0) { resultPort.Post(SuccessResult.Instance); } else { resultPort.Post(Marshal.GetExceptionForHR(result.hr)); } }, test => test.type == WebcamResponse.SetFormat ) ); return(resultPort); }
/// <summary> /// Loads a grammar into the speech recognition engine; MUST run exclusive /// </summary> /// <param name="request">Request that contains the new grammar to load</param> /// <param name="response">Response port</param> /// <returns></returns> private IEnumerator <ITask> LoadGrammar(LoadGrammarRequest request, SuccessFailurePort response) { #region Build grammar // Build grammar if (request.GrammarType == GrammarType.Srgs) { // Load SRGS grammar file FileReaderPort fileReaderPort = new FileReaderPort(); yield return(new IterativeTask(delegate { return ReadFileFromMountService(request.SrgsFileLocation, fileReaderPort); })); Exception fileReaderException = (Exception)fileReaderPort; if (fileReaderException != null) { LogWarning(fileReaderException); response.Post(fileReaderException); yield break; } try { grammarToLoad = GrammarUtilities.BuildSrgsGrammar((MemoryStream)fileReaderPort); } catch (Exception ex) { LogWarning(ex); response.Post(ex); yield break; } } else { // Build dictionary-style grammar try { grammarToLoad = GrammarUtilities.BuildDictionaryGrammar(request.DictionaryGrammar); } catch (Exception ex) { LogWarning(ex); response.Post(ex); yield break; } } #endregion #region Load grammar and start engine if necessary // Request chance to update recognition engine and cancel current recognition // operation state.Recognizer.RequestRecognizerUpdate(); state.Recognizer.RecognizeAsyncCancel(); yield return((Choice)loadGrammarResponsePort); Exception loadGrammarException = (Exception)loadGrammarResponsePort; if (loadGrammarException != null) { LogWarning(loadGrammarException); response.Post(loadGrammarException); yield break; } // Empty response port SuccessResult loadGrammarSuccess = (SuccessResult)loadGrammarResponsePort; // Start engine again if (state.Recognizer.Grammars.Count > 0 && !state.IgnoreAudioInput) { state.Recognizer.RecognizeAsync(RecognizeMode.Multiple); } #endregion // Store information about the new grammar in the service's state this.state.Grammar = grammarToLoad; this.state.GrammarType = request.GrammarType; if (request.GrammarType == GrammarType.Srgs) { this.state.SrgsFileLocation = request.SrgsFileLocation; this.state.DictionaryGrammar = null; } else { this.state.DictionaryGrammar = request.DictionaryGrammar; this.state.SrgsFileLocation = null; } response.Post(SuccessResult.Instance); }
/// <summary> /// Writes a binary file from a stream to the mount service /// </summary> /// <param name="filename">Filename and path where the file shall be store on the mount service</param> /// <param name="fileStream">Stream containing the file data</param> /// <param name="responsePort">File writer response port</param> /// <returns></returns> private IEnumerator <ITask> WriteFileToMountService( string filename, Stream fileStream, SuccessFailurePort responsePort) { // Stream needs to support seeking, otherwise we will never know when the end // is reached if (!fileStream.CanSeek) { throw new ArgumentException("File stream needs to support seeking"); } // Construct URI to file string fileUri = "http://localhost" + ServicePaths.MountPoint; if (!filename.StartsWith("/")) { fileUri += "/"; } fileUri += filename; // Establish channel with mount service mnt.MountServiceOperations mountPort = ServiceForwarder <mnt.MountServiceOperations>(fileUri); // Set up byte update message mnt.UpdateBytesRequest updateBytesRequest = new mnt.UpdateBytesRequest(); mnt.UpdateBytes updateBytes = new mnt.UpdateBytes(updateBytesRequest); updateBytesRequest.Offset = 0; updateBytesRequest.Truncate = false; // Write file in blocks to mount service updateBytesRequest.Data = new byte[MountServiceWriteBlockSize]; fileStream.Position = 0; int writeOffset = 0; while (fileStream.Position < fileStream.Length) { // Fill buffer int offset = 0; while (offset < MountServiceWriteBlockSize && fileStream.Position < fileStream.Length) { int bytesRead = fileStream.Read( updateBytesRequest.Data, offset, MountServiceWriteBlockSize - offset ); offset += bytesRead; } if (offset < MountServiceWriteBlockSize) { // Last message will most probably not contain a completely filled buffer Array.Resize <byte>(ref updateBytesRequest.Data, offset); } if (fileStream.Position >= fileStream.Length) { // End of stream reached, truncate file on mount service // to current position updateBytesRequest.Truncate = true; } updateBytesRequest.Offset = writeOffset; // Send message to mount service mountPort.Post(updateBytes); yield return((Choice)updateBytes.ResponsePort); Fault fault = (Fault)updateBytes.ResponsePort; if (fault != null) { Exception exception = fault.ToException(); LogWarning(exception); responsePort.Post(exception); yield break; } // Clear response port DefaultUpdateResponseType success = (DefaultUpdateResponseType)updateBytes.ResponsePort; writeOffset += updateBytesRequest.Data.Length; } responsePort.Post(SuccessResult.Instance); }
private SuccessFailurePort UpdateState() { var resultPort = new SuccessFailurePort(); //TODO: read state from service return resultPort; }
public IEnumerator <ITask> ReplaceHandler(Replace replace) { SpeechRecognizerState newState = replace.Body; #region Set up load grammar request, load grammar and on success store new grammar in state // Set up load grammar request LoadGrammarRequest loadRequest = new LoadGrammarRequest(); loadRequest.GrammarType = newState.GrammarType; loadRequest.SrgsFileLocation = newState.SrgsFileLocation; loadRequest.DictionaryGrammar = newState.DictionaryGrammar; // Load new grammar SuccessFailurePort loadGrammarPort = new SuccessFailurePort(); SpawnIterator <LoadGrammarRequest, SuccessFailurePort>( loadRequest, loadGrammarPort, LoadGrammar ); // Check loading outcome yield return((Choice)loadGrammarPort); Exception exception = (Exception)loadGrammarPort; if (exception != null) { LogWarning(exception); replace.ResponsePort.Post(Fault.FromCodeSubcodeReason( W3C.Soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, exception.Message )); yield break; } #endregion #region Check new state's IgnoreAudioInput flag and start recognition engine if necessary if (newState.IgnoreAudioInput != state.IgnoreAudioInput) { state.IgnoreAudioInput = newState.IgnoreAudioInput; if (state.IgnoreAudioInput) { // Stop engine and switch to ignoring audio input state.Recognizer.RecognizeAsyncCancel(); state.Recognizer.SetInputToNull(); } else { SetRecognizerInputToKinectMicArray(); // Because old state ignored audio input the engine is stopped, now that // we switched to listening to audio input the engine needs to be started if (state.Recognizer.Grammars.Count > 0) { state.Recognizer.RecognizeAsync(RecognizeMode.Multiple); } } } #endregion SaveState(this.state); // Notify subscribers of state change replace.Body = this.state; SendNotification <Replace>(this.subMgrPort, replace); replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); }
// This is the basic method used to move the arm. The target position for each joint is specified along // with a time for the movement to be completed. A port is returned which will receive a success message when the // movement is completed or an exception message if an error is encountered. public SuccessFailurePort MoveTo( float baseVal, float shoulderVal, float elbowVal, float wristVal, float rotateVal, float gripperVal, float time) { var responsePort = new SuccessFailurePort(); if (_moveToActive) { responsePort.Post(new Exception("Previous MoveTo still active.")); return responsePort; } var values = new[] { baseVal, shoulderVal, elbowVal, wristVal, rotateVal, gripperVal}; // check bounds. If the target is invalid, post an exception message to the response port with a helpful error. for (int i = 0; i < _joints.Count; i++) { var val = values[i]; if (!_joints[i].ValidTarget(val)) { responsePort.Post(new Exception(_joints[i].Name + "Joint set to invalid value: " + val)); return responsePort; } } /* if ((_joints[5].Target > gripperVal) && (Payload == null)) { _attachPayload = true; } else if ((_joints[5].Target < gripperVal) && (Payload != null)) { _dropPayload = true; } */ // set the target values on the joint descriptors for (int i = 0; i < _joints.Count; i++) _joints[i].Target = values[i]; // calculate a speed value for each joint that will cause it to complete its motion in the specified time for (int i = 0; i < _joints.Count; i++) _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time; // set this flag so that the motion is evaluated in the update method _moveToActive = true; // keep a pointer to the response port so we can post a result message to it. _moveToResponsePort = responsePort; return responsePort; }
/// <summary> /// Constructor /// </summary> /// <param name="result"></param> public AlertForm(SuccessFailurePort result) { _result = result; InitializeComponent(); }
// This method calculates the joint angles necessary to place the arm into the // specified position. The arm position is specified by the X,Y,Z coordinates // of the gripper tip as well as the angle of the grip, the rotation of the grip, // and the open distance of the grip. The motion is completed in the // specified time. public SuccessFailurePort MoveToPosition( float mx, // x position float my, // y position float mz, // z position float p, // angle of the grip float w, // rotation of the grip float grip, // distance the grip is open float time) // time to complete the movement { float baseAngle, shoulder, elbow, wrist; _moveTargetEntity.Position = new xna.Vector3(mx, my, mz); if (!_kinematics.InverseKinematics(mx, my, mz, p, out baseAngle, out shoulder, out elbow, out wrist)) { var s = new SuccessFailurePort(); s.Post(new Exception("Inverse Kinematics failed")); return s; } // Update the form with these parameters WinFormsServicePort.FormInvoke(() => _form.SetPositionText(mx, my, mz, p, w, grip, time)); // Update the form with these parameters WinFormsServicePort.FormInvoke(() => _form.SetJointsText(baseAngle, shoulder, elbow, wrist, w, grip, time)); var result = _robotArm.MoveTo( baseAngle, shoulder, elbow, wrist, w, grip, time); return result; }