public LbcbConversion(String label, Lbcb lbcb, RigidTransform transform) { String newlabel = lbcb.getLabel() + "_" + label; this.lbcb = new Lbcb( newlabel, lbcb.getActuatorPins()); this.transform = transform; this.label = label; }
public void testTransform1() { RigidTransform rig = new RigidTransform(new double[] { 3, 0, 0 }, new double[,] { {1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 1.0} }); log.Debug("Testing small motion center"); testTransform(rig, mcDisp, mcForces); }
private void testTransform(RigidTransform rig, double[][] exDisp, double[][] exForce) { for (int t = 0; t < 5; t++) { CompareDoubleLists cmp = new CompareDoubleLists(); CompareDoubleLists cmpF = new CompareDoubleLists(0.005); double[] actual = rig.transform(platDisp[t], false); log.Debug("Checking mc disp"); cmp.Compare(exDisp[t], actual); actual = rig.transformMoments(platForces[t]); log.Debug("Checking mc force"); cmpF.Compare(exForce[t], actual); actual = rig.transform(exDisp[t], true); log.Debug("Checking plat disp"); cmp.Compare(platDisp[t], actual); actual = rig.transformMoments(exForce[t]); log.Debug("Checking plat force"); cmpF.Compare(platForces[t], actual); } }
public void testTransform4() { RigidTransform rig = new RigidTransform(new double[] { -18.0000, 0, 45.4690 }, new double[,] { {1.016593, -0.000047, -0.006086, 0.122384, -1.617186, 0.775084}, {-0.009181, 1.003122, -0.028188, 1.501314, -0.060338, -0.176663}, {0.006838, 0.025004, 1.001882, -0.282000, -0.167350, 0.043025}, {-0.000022, -0.000022, 0.000109, 1.001187, 0.000965, -0.007494}, {0.000003, -0.000029, 0.000001, 0.001187, 0.999678, -0.025727}, {0.000667, 0.000011, -0.000276, 0.006026, 0.029385, 1.016522}, }); log.Debug("Testing alignment matrix"); testTransform(rig, mcDisp3, mcForces3); }
public void testTransform3() { RigidTransform rig = new RigidTransform(new double[] { -11, 55, 32.5, }, new double[,] { {1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 1.0} }); log.Debug("Testing large motion center"); testTransform(rig, mcDisp2, mcForces2); }
public void testTransform2() { RigidTransform rig = new RigidTransform(new double[] { 32, 0, 65 }, new double[,] { {1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 1.0} }); log.Debug("Testing large motion center"); testTransform(rig, mcDisp1, mcForces1); }
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; }