private static Point3D computeCoM(Robot robot) { Point3D com = new Point3D(); robot.computeCenterOfMass(com); return com; }
public void packRobotComCopSeries(ArrayList<Point3D> outCom, ArrayList<Point3D> outCop) { outCom.clear(); outCop.clear(); for (int i = 0; i < selectedFrames.length; i++) { dataBuffer.setIndexButDoNotNotifySimulationRewoundListeners(selectedFrames[i]); // model predicted CoM robot.update(); //this pull data from dataBuffer magically through YoVariables Point3D modelCoM = new Point3D(); robot.computeCenterOfMass(modelCoM); outCom.add(modelCoM); // sensedCoP Point3D sensedCoP = new Point3D(dataBuffer.getVariable("sensedCoPX").getValueAsDouble(), dataBuffer.getVariable("sensedCoPY").getValueAsDouble(), dataBuffer.getVariable("sensedCoPZ").getValueAsDouble()); outCop.add(sensedCoP); } }
private void writeInfoToWorkBook(WritableWorkbook workbook) { WritableSheet infoSheet = workbook.createSheet("Run info", workbook.getNumberOfSheets()); int labelColumn = 0; int dataColumn = 1; int row = 0; addStringToSheet(infoSheet, labelColumn, row, "Date: ", headerCellFormat); WritableCell dateCell = new DateTime(dataColumn, row, Date.from(ZonedDateTime.now().toInstant())); addCell(infoSheet, dateCell); row++; addStringToSheet(infoSheet, labelColumn, row, "Robot type: ", headerCellFormat); addStringToSheet(infoSheet, dataColumn, row, robot.getClass().getSimpleName()); row++; addStringToSheet(infoSheet, labelColumn, row, "Robot name: ", headerCellFormat); addStringToSheet(infoSheet, dataColumn, row, robot.getName()); row++; addStringToSheet(infoSheet, labelColumn, row, "Total mass [kg]: ", headerCellFormat); addNumberToSheet(infoSheet, dataColumn, row, robot.computeCenterOfMass(new Point3D())); row++; addStringToSheet(infoSheet, labelColumn, row, "Run time [s]: ", headerCellFormat); addNumberToSheet(infoSheet, dataColumn, row, dataBuffer.getEntry(robot.getYoTime()).getMax()); row++; addStringToSheet(infoSheet, labelColumn, row, "Mechanical cost of transport: ", headerCellFormat); addNumberToSheet(infoSheet, dataColumn, row, computeMechanicalCostOfTransport()); row++; }
private void checkRobotsHaveSameState(Robot robotA, Robot robotB) { Point3D centerOfMassA = new Point3D(); Point3D centerOfMassB = new Point3D(); double totalMassA = robotA.computeCenterOfMass(centerOfMassA); double totalMassB = robotB.computeCenterOfMass(centerOfMassB); Vector3D angularMomentumA = new Vector3D(); Vector3D angularMomentumB = new Vector3D(); robotA.computeAngularMomentum(angularMomentumA); robotB.computeAngularMomentum(angularMomentumB); Vector3D linearMomentumA = new Vector3D(); Vector3D linearMomentumB = new Vector3D(); robotA.computeLinearMomentum(linearMomentumA); robotB.computeLinearMomentum(linearMomentumB); assertEquals(totalMassA, totalMassB, 1e-7); EuclidCoreTestTools.assertTuple3DEquals(centerOfMassA, centerOfMassB, 1e-10); EuclidCoreTestTools.assertTuple3DEquals(linearMomentumA, linearMomentumB, 1e-10); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumA, angularMomentumB, 1e-9); ArrayList<Joint> jointsA = robotA.getRootJoints(); ArrayList<Joint> jointsB = robotB.getRootJoints(); recursivelyTestJointStateIsTheSame(jointsA, jointsB); }
@Test// timeout = 30000 public void testOneRigidJoint() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(1.1, 2.2, 3.3); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); EuclidCoreTestTools.assertTuple3DEquals(centerOfMassOffset, centerOfMass, 1e-10); }
private double computeMechanicalCostOfTransport() double robotMass = robot.computeCenterOfMass(new Point3D()); Vector3D gravity = new Vector3D(); robot.getGravity(gravity);
@Test// timeout = 30000 public void testOneRigidJointWithRotation() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); RotationMatrix rotation = new RotationMatrix(); Vector3D eulerAngles = new Vector3D(0.3, 0.1, 0.2); rotation.setEuler(eulerAngles); rigidJointOne.setRigidRotation(rotation); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(0.3, 0.7, 1.11); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); robot.update(); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); Point3D expectedCenterOfMass = new Point3D(centerOfMassOffset); rotation.transform(expectedCenterOfMass); EuclidCoreTestTools.assertTuple3DEquals(expectedCenterOfMass, centerOfMass, 1e-10); }
@Test// timeout = 30000 public void testOneRigidJointWithTranslation() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); Vector3D translation = new Vector3D(1.1, 2.2, 3.3); rigidJointOne.setRigidTranslation(translation); RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(translation); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(0.99, -0.4, 1.1); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); robot.update(); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); Point3D expectedCenterOfMass = new Point3D(centerOfMassOffset); expectedCenterOfMass.add(translation); EuclidCoreTestTools.assertTuple3DEquals(expectedCenterOfMass, centerOfMass, 1e-10); }
@Test// timeout = 30000 public void testOneRigidJointWithRotationAndTranslation() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); RotationMatrix rotation = new RotationMatrix(); Vector3D eulerAngles = new Vector3D(0.3, 0.1, 0.2); rotation.setEuler(eulerAngles); rigidJointOne.setRigidRotation(rotation); Vector3D translation = new Vector3D(0.3, -0.99, 1.11); rigidJointOne.setRigidTranslation(translation); RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(translation); transform.setRotation(rotation); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(0.3, 0.7, 1.11); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); robot.update(); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); Point3D expectedCenterOfMass = new Point3D(centerOfMassOffset); transform.transform(expectedCenterOfMass); EuclidCoreTestTools.assertTuple3DEquals(expectedCenterOfMass, centerOfMass, 1e-10); }
double totalMass = robot.computeCenterOfMass(comPoint);