コード例 #1
0
ファイル: RigidTransform.cs プロジェクト: mbletzinger/lbcb-om
 public double[] transform(double[] displacement, bool isreverse)
 {
     DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
     double[] dispcopy = new double[6];
     displacement.CopyTo(dispcopy, 0);
     DenseMatrix2String m2s = new DenseMatrix2String();
     List2String l2s = new List2String();
     DenseVector dispV = new DenseVector(dispcopy);
     log.Debug("original disp: " + l2s.ToString(displacement));
     if (isreverse)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
         log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
     }
     DenseVector newDisp = translate(dispV.Values, isreverse);
     log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
     DenseMatrix rollM = roll.create(dispV.Values[3]);
     DenseMatrix pitchM = pitch.create(dispV.Values[4]);
     DenseMatrix yawM = yaw.create(dispV.Values[5]);
     DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
     log.Debug("rotation: " + m2s.ToString(rotation));
     DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
     log.Debug("unt: " + l2s.ToString(unt.Values));
     DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
     log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
     dispV.SetSubVector(0, 3, newDisp1);
     if (isreverse == false)
     {
         dispV = (DenseVector)xform.Multiply(dispV);
     }
     log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
     return dispV.Values;
 }
コード例 #2
0
        public void setLbcb(bool isLbcb2, double[] flattenedPinArray, double[] motionCenter, double[] flattenedTransformation)
        {
            double[][] pins = new double[6][];
            double[,] xform = new double[6, 6];
            double[,] xpins = new double[6, 6];
            List2String l2s = new List2String();
            DenseMatrix2String m2s = new DenseMatrix2String();

            for (int r = 0; r < 6; r++)
            {
                pins[r] = new double[6];
                for (int c = 0; c < 6; c++)
                {
                    pins[r][c] = flattenedPinArray[r * 6 + c];
                    xpins[r,c] = flattenedPinArray[r * 6 + c];
                    xform[r, c] = flattenedTransformation[r * 6 + c];

                }
            }

            log.Info("Creating " + (isLbcb2 ? "LBCB 2" : "LBCB 1")
                + " with \n]\t Motion Center: " + l2s.ToString(motionCenter)
               + "\n\t Transformation Matrix: " + m2s.ToString(DenseMatrix.OfArray(xform))
               + "\n\t Pins Locations: " + m2s.ToString(DenseMatrix.OfArray(xpins)));
               Lbcb lbcb = new Lbcb((isLbcb2 ? "LBCB 2" : "LBCB 1"), pins);
            lbcbs[(isLbcb2 ? 1 : 0)] = lbcb;
            RigidTransform transform = new RigidTransform(motionCenter,xform);
            transforms[(isLbcb2 ? 1 : 0)] = transform;
        }
コード例 #3
0
ファイル: RigidTransform.cs プロジェクト: mbletzinger/lbcb-om
 public RigidTransform(double[] motionCenter, double[,] transformation)
 {
     this.motionCenter = new DenseVector(motionCenter);
     this.transformation = DenseMatrix.OfArray(transformation);
     List2String l2s = new List2String();
     log.Debug("motionCenter: " + l2s.ToString(this.motionCenter.Values) +
         " platformCenter: " + l2s.ToString(this.platformCenter.Values));
 }
コード例 #4
0
ファイル: RigidTransform.cs プロジェクト: mbletzinger/lbcb-om
 private DenseVector translate(double[] displacement, bool isreverse)
 {
     double[] ddisp = new double[6];
     displacement.CopyTo(ddisp, 0);
     DenseVector translation = DenseVector.Create(3, 0.0); ;
     List2String l2s = new List2String();
     log.Debug("original disp: " + l2s.ToString(displacement));
     DenseVector dispV = new DenseVector(ddisp);
     dispV.CopySubVectorTo(translation, 0, 0, 3);
     log.Debug("given trans:" + l2s.ToString(translation.Values) +
         " from: " + l2s.ToString(dispV.Values) +
         " orig: " + l2s.ToString(displacement));
     DenseVector target;
     DenseVector reference;
     if (isreverse == false)
     {
         log.Debug("Normal Translation");
         directionalVector = (DenseVector)motionCenter.Subtract(platformCenter);
         target = motionCenter;
         reference = platformCenter;
     }
     else
     {
         log.Debug("Reverse Translation");
         directionalVector = (DenseVector)platformCenter.Subtract(motionCenter);
         target = platformCenter;
         reference = motionCenter;
     }
     log.Debug("translation: " + l2s.ToString(translation.Values) +
         " reference: " + l2s.ToString(reference.Values) +
         " target: " + l2s.ToString(target.Values));
     DenseVector newDisp = (DenseVector)translation.Add(reference).Subtract(target);
     log.Debug("calculated trans:" + l2s.ToString(newDisp.Values) +
         " dv: " + l2s.ToString(directionalVector.Values));
     return newDisp;
 }
コード例 #5
0
ファイル: Lbcb.cs プロジェクト: mbletzinger/lbcb-om
        private void solveActuator2CartesianDisp(double[] adisp)
        {
            bool check = false;
            DenseVector cartDisp = new DenseVector(6);
            DenseVector newAct = new DenseVector(adisp);
            DenseVector actError = (DenseVector)newAct.Subtract(actuatorDisp);
            cartesianDisp.CopyTo(cartDisp);
            int iterations = 0;

            while (check == false)
            {
                List2String l2s = new List2String();

                DenseMatrix JacobianMatrix = new DenseMatrix(6, 6);

                for (int i = 0; i < 6; i++)
                {
                    DenseVector DL_Dd = actuators[i].calcNewDiffs(cartDisp.Values);
                    JacobianMatrix.SetRow(i, DL_Dd);
                }
                DenseVector diffCart = (DenseVector)JacobianMatrix.LU().Solve(actError);
                log.Debug("Cartesian differences " + l2s.ToString(diffCart.Values));
                cartDisp = (DenseVector)cartDisp.Add(diffCart);
                setCartesianDisp(cartDisp.Values);
                log.Debug("New cartesian estimate " + this);
                actError = (DenseVector)newAct.Subtract(actuatorDisp);
                log.Debug("Actuator error " + l2s.ToString(actError.Values));

                check = withinErrorWindow(actError);
                if (iterations > 20)
                {
                    check = true;
                    log.Error("Calculations for " + label + " won't converge with " + this);
                }
                iterations++;
            }
        }
コード例 #6
0
ファイル: Lbcb.cs プロジェクト: mbletzinger/lbcb-om
 public override string ToString()
 {
     List2String l2s = new List2String();
     string result = label + ":";
     result += "\nCartD: " + l2s.ToString(cartesianDisp.Values);
     result += "\nCartF: " + l2s.ToString(cartesianForce.Values);
     result += "\nActD: " + l2s.ToString(actuatorDisp.Values);
     result += "\nActF: " + l2s.ToString(actuatorForce.Values);
     //foreach (LbcbActuator act in actuators)
     //{
     //    result += "\n" + act;
     //}
     return result;
 }
コード例 #7
0
        public void ConvertCartesian2Actuator()
        {
            List2String l2s = new List2String();
            foreach (ExpectedLbcbValues ev in expected1)
            {
                LbcbConversion control1 = lbcbFactory.get("Command 1");
                LbcbConversion daq1 = lbcbFactory.get("Readings 1");
                CompareDoubleLists cmp = new CompareDoubleLists();
                log.Debug("Setting Cart Disp:" + l2s.ToString(ev.getDofs(DofType.CartCommands)));
                log.Debug("Before " + control1.getLbcb());
                double[] acts = control1.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartCommands));
                log.Debug("After " + control1.getLbcb());
                cmp.Compare(ev.getDofs(DofType.ActCommands), acts);
                acts = control1.getActuatorDisplacements();
                cmp.Compare(ev.getDofs(DofType.ActCommands), acts);

                acts = daq1.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartDisplacements));
                cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
                acts = daq1.getActuatorDisplacements();
                cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
            }
            foreach (ExpectedLbcbValues ev in expected2)
            {
                LbcbConversion control2 = lbcbFactory.get("Command 2");
                LbcbConversion daq2 = lbcbFactory.get("Readings 2");
                CompareDoubleLists cmp = new CompareDoubleLists();
                log.Debug("Setting Cart Disp:" + l2s.ToString(ev.getDofs(DofType.CartCommands)));
                double[] acts = control2.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartCommands));
                cmp.Compare(ev.getDofs(DofType.ActCommands), acts);
                acts = control2.getActuatorDisplacements();
                cmp.Compare(ev.getDofs(DofType.ActCommands), acts);

                acts = daq2.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartDisplacements));
                cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
                acts = daq2.getActuatorDisplacements();
                cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
            }
        }
コード例 #8
0
ファイル: LbcbActuator.cs プロジェクト: mbletzinger/lbcb-om
        public double[] calcNewDiffs(double[] cartesian)
        {
            DenseMatrix rotTrig = DenseMatrix.Create(3,2,0);
            double theta_x = cartesian[3];
            double theta_y = cartesian[4];
            double theta_z = cartesian[5];
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            rotTrig.At(0, 0, Math.Sin(theta_x));
            rotTrig.At(0, 1, Math.Cos(theta_x));
            rotTrig.At(1, 0, Math.Sin(theta_y));
            rotTrig.At(1, 1, Math.Cos(theta_y));
            rotTrig.At(2, 0, Math.Sin(theta_z));
            rotTrig.At(2, 1, Math.Cos(theta_z));
            log.Debug("Rotation Angles " + rotTrig.ToString("#000.00000\t", formatProvider));
            DenseVector basepin = fixedPin;
            DenseVector nominalplatformpin = initialPlatformPin;
            double Ppx = nominalplatformpin[0];
            double Ppy = nominalplatformpin[1];
            double Ppz = nominalplatformpin[2];
            log.Debug("Ppx= " + Ppx + "Ppy= " + Ppy + "Ppz= " + Ppz);
            DenseVector currentplatformpin = platformPin;
            List2String l2s = new List2String();

            DenseVector phai = (DenseVector) currentplatformpin.Subtract(basepin);
            log.Debug("phai " + l2s.ToString(phai.Values));

            if (length == 0.0)
            {
                log.Error("Actuator length is zero.  Caused a divide-by-zero error");
                throw new Exception("Actuator length is zero causing a divide by zero error");
            }

            phai = (DenseVector) phai.Divide(length);
            log.Debug("phai " + l2s.ToString(phai.Values));

            DenseMatrix J = DenseMatrix.Create(3,3,0);
            // Jacobian Element
            J[0, 0] = 0;
            J[0, 1] =  -rotTrig[1, 0] * rotTrig[2, 1] * Ppx + rotTrig[1, 0] * rotTrig[2, 0] * Ppy + rotTrig[1, 1] * Ppz;
            J[0, 2] =  -rotTrig[1, 1] * rotTrig[2, 0] * Ppx - rotTrig[1, 1] * rotTrig[2, 1] * Ppy;
            J[1, 0] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppx +
                    (-rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] - rotTrig[0, 0] * rotTrig[0, 1]) * Ppy
                     - rotTrig[0, 1] * rotTrig[1, 1] * Ppz;
            J[1, 1] =  rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx - rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
                     - rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
            J[1, 2] = (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] * rotTrig[0, 1] * rotTrig[2, 1]) * Ppx +
                    (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 1] * rotTrig[2, 0]) * Ppy;
            J[2, 0] = (rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] + rotTrig[0, 1] * rotTrig[2, 0]) * Ppx +
                    (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 1] * rotTrig[2, 1]) * Ppy
                     - rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
            J[2, 1] =  -rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx + rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
                     - rotTrig[0, 1] * rotTrig[1, 0] * Ppz;
            J[2, 2] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 0] * rotTrig[2, 1]) * Ppx
                    + (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppy;

            log.Debug("Jacobian " + J.ToString("#000.00000\t", formatProvider));
            DenseVector diffs = new DenseVector(6);
            diffs.SetSubVector(0, 3, phai);
            DenseVector rphai = new DenseVector(new[] { (J[0, 0] * phai[0] + J[1, 0] * phai[1] + J[2, 0] * phai[2]),
                    (J[0, 1] * phai[0] + J[1, 1] * phai[1] + J[2, 1] * phai[2]),
                        (J[0, 2] * phai[0] + J[1, 2] * phai[2] + J[2, 2] * phai[2]) });
            log.Debug("rphai " + l2s.ToString(rphai.Values));
            diffs.SetSubVector(3, 3, rphai);
            return diffs.Values;
        }
コード例 #9
0
ファイル: LbcbActuator.cs プロジェクト: mbletzinger/lbcb-om
        public override string ToString()
        {
            List2String l2s = new List2String();
            string result = label + " ";
            result += "\tCurrent: " + l2s.ToString(platformPin.Values);
            result += "\tFixed: " + l2s.ToString(fixedPin.Values);
            result += "\tInitial: " + l2s.ToString(initialPlatformPin.Values);
            result += "\tDisplacement: " + getCurrentDisplacement();
            result += "\tLength: " + length;
            result += "\tInitial Length: " + initialLength;

            return result;
        }