// 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;
Example #2
 private SuccessFailurePort UpdateState()
     var resultPort = new SuccessFailurePort();
     //TODO: read state from service
     return resultPort;
Example #3
        // 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(

            return result;