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