Kinematics-Forward_Class

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Leadshine.SMC.IDE.Motion;
using System.Threading;
using System.Drawing.Drawing2D;
using MathNet.Numerics.LinearAlgebra;
using System.IO;
using robot;

namespace robot
{
    class forward_instantaneous
    {
        public forward_instantaneous()
        {

        }
        double con;
        double[] a;                
        double[] b;                 
       

   
        public double instantaneous_psi, instantaneous_theta, instantaneous_z;      
        public double instantaneous_Gx, instantaneous_Gy, instantaneous_Gz;          
        public double e_tool=0;/
        public voi