public double getElbowTorque() { return elbowJoint.getTauYoVariable().getDoubleValue(); }
PinJointRobotSensor(PinJoint joint) { q = joint.getQYoVariable(); qd = joint.getQDYoVariable(); qdd = joint.getQDDYoVariable(); tau_actual = joint.getTauYoVariable(); }
@Test// timeout=300000 public void testFloatingJointAndPinJointWithMassiveBody() throws UnreasonableAccelerationException { Random random = new Random(1659L); Robot robot = new Robot("r1"); robot.setGravity(0.0); FloatingJoint root1 = new FloatingJoint("root1", new Vector3D(), robot); robot.addRootJoint(root1); Link floatingBody = new Link("floatingBody"); floatingBody.setMass(random.nextDouble()); floatingBody.setComOffset(random.nextDouble(), random.nextDouble(), random.nextDouble()); floatingBody.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(floatingBody.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); root1.setLink(floatingBody); Vector3D offset = EuclidCoreRandomTools.nextVector3D(random); PinJoint pin1 = new PinJoint("pin1", offset, robot, EuclidCoreRandomTools.nextVector3D(random)); pin1.setLink(massiveLink()); root1.addJoint(pin1); pin1.setTau(random.nextDouble()); robot.doDynamicsButDoNotIntegrate(); double scalarInertiaAboutJointAxis = computeScalarInertiaAroundJointAxis(floatingBody, pin1); double torqueFromDynamics = scalarInertiaAboutJointAxis * pin1.getQDDYoVariable().getDoubleValue(); assertEquals(pin1.getTauYoVariable().getDoubleValue(), torqueFromDynamics, pin1.getTauYoVariable().getDoubleValue() * 1e-3); }
private double[] computeTotalUnsignedMechanicalPower() { int dataLength = dataBuffer.getEntry(robot.getYoTime()).getData().length; double[] ret = new double[dataLength]; for (int i = 0; i < dataLength; i++) ret[i] = 0.0; for (PinJoint joint : pinJoints) { double[] speed = dataBuffer.getEntry(joint.getQDYoVariable()).getData(); double[] torque = dataBuffer.getEntry(joint.getTauYoVariable()).getData(); double[] jointMechincalPower = computeMechanicalPower(speed, torque); for (int i = 0; i < dataLength; i++) { ret[i] += Math.abs(jointMechincalPower[i]); } } return ret; }
public void createJointTorqueSpeedGraphs(File directory, String fileHeader, boolean createJPG, boolean createPDF) { for (PinJoint pinJoint : pinJoints) { DataBufferEntry torque = dataBuffer.getEntry(pinJoint.getTauYoVariable()); DataBufferEntry speed = dataBuffer.getEntry(pinJoint.getQDYoVariable()); String timeLabel = "time [s]"; String torqueLabel = torque.getVariableName() + " [Nm]"; String speedLabel = speed.getVariableName() + " [rad/s]"; String torqueSpeedTitle = torque.getVariableName() + "_Vs_" + speed.getVariableName(); createDataVsTimeGraph(directory, fileHeader, torque, createJPG, createPDF, timeLabel, torqueLabel, Color.black); createDataVsTimeGraph(directory, fileHeader, speed, createJPG, createPDF, timeLabel, speedLabel, Color.black); createDataOneVsDataTwoGraph(directory, fileHeader, speed, torque, createJPG, createPDF, torqueSpeedTitle, speedLabel, torqueLabel, Color.black); } }
DataBufferEntry torque = dataBuffer.getEntry(joint.getTauYoVariable());
private void writeJointDataToWorkbook(WritableWorkbook workbook) { WritableSheet jointDataSheet = workbook.createSheet("Joint Data", workbook.getNumberOfSheets()); int column = 0; writeJointDataColumn(jointDataSheet, column++, dataBuffer.getEntry("t"), true); for (PinJoint joint : pinJoints) { DataBufferEntry position = dataBuffer.getEntry(joint.getQYoVariable()); DataBufferEntry speed = dataBuffer.getEntry(joint.getQDYoVariable()); DataBufferEntry torque = dataBuffer.getEntry(joint.getTauYoVariable()); writeJointDataColumn(jointDataSheet, column++, position, false); writeJointDataColumn(jointDataSheet, column++, speed, false); writeJointDataColumn(jointDataSheet, column++, speed, true); writeJointDataColumn(jointDataSheet, column++, torque, false); writeJointDataColumn(jointDataSheet, column++, torque, true); writeMechanicalPowerJointDataColumn(jointDataSheet, column++, speed, torque, joint.getName()); } }
assertEquals(pinJointOne.getTauYoVariable().getDoubleValue(), -jointTorque.getY(), 1e-7); assertEquals(pinJointTwo.getTauYoVariable().getDoubleValue(), -jointTorque.getX(), 1e-7);
pin2.setTau(-pin1.getTauYoVariable().getDoubleValue());