public AbstractThreadedRobotController(String name, Robot simulatedRobot) { this.name = name; this.registry = new YoVariableRegistry(name); this.currentControlTick = new YoLong("currentControlTick", registry); this.yoTime = simulatedRobot.getYoTime(); this.simulatedRobot = simulatedRobot; }
public TorqueSpeedDataExporterGraphCreator(Robot robot, DataBuffer dataBuffer) { super(robot.getYoTime(), dataBuffer); for (Joint rootJoint : robot.getRootJoints()) { recursivelyAddPinJoints(rootJoint, pinJoints); } }
SinusoidalTorqueController(Robot robot) { this.t = robot.getYoTime(); this.registry = new YoVariableRegistry(robot.getName() + getClass().getSimpleName()); Joint rootJoint = robot.getRootJoints().get(0); for(Joint childJoint : rootJoint.getChildrenJoints()) { recursivelyAddJointTorqueProfile(childJoint); } }
private double computeTotalMechanicalEnergy() { double ret = 0.0; double[] mechanicalPower = computeTotalUnsignedMechanicalPower(); double simulationTime = dataBuffer.getEntry(robot.getYoTime()).getMax(); double simulationDT = simulationTime / dataBuffer.getBufferSize(); for (int i = 0; i < mechanicalPower.length; i++) { ret += simulationDT * Math.abs(mechanicalPower[i]); } return ret; }
private void setupSCS() { SimulationTestingParameters simulationTestingParameters = SimulationTestingParameters.createFromSystemProperties(); simulationTestingParameters.setKeepSCSUp(visualize); Robot robot = new Robot("Dummy"); yoTime = robot.getYoTime(); scs = new SimulationConstructionSet(robot, simulationTestingParameters); }
YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, joint.getQYoVariable(), robot.getYoTime(), valueDataCheckerParameters, joint.getQDYoVariable());
@Override public void build(FloatingInverseDynamicsJoint rootJoint, IMUDefinition[] imuDefinition, ForceSensorDefinition[] forceSensorDefinitions, ContactSensorHolder contactSensorHolder, RawJointSensorDataHolderMap rawJointSensorDataHolderMap, DesiredJointDataHolder estimatorDesiredJointDataHolder, YoVariableRegistry parentRegistry) { ArrayList<Joint> rootJoints = robot.getRootJoints(); if (rootJoints.size() > 1) { throw new RuntimeException("Robot has more than 1 rootJoint"); } final Joint scsRootJoint = rootJoints.get(0); if (!(scsRootJoint instanceof FloatingJoint)) throw new RuntimeException("Not FloatingJoint rootjoint found"); SCSToInverseDynamicsJointMap scsToInverseDynamicsJointMap = SCSToInverseDynamicsJointMap.createByName((FloatingJoint) scsRootJoint, rootJoint); StateEstimatorSensorDefinitionsFromRobotFactory stateEstimatorSensorDefinitionsFromRobotFactory = new StateEstimatorSensorDefinitionsFromRobotFactory( scsToInverseDynamicsJointMap, imuMounts, groundContactPointBasedWrenchCalculators); this.stateEstimatorSensorDefinitions = stateEstimatorSensorDefinitionsFromRobotFactory.getStateEstimatorSensorDefinitions(); Map<IMUMount, IMUDefinition> imuDefinitions = stateEstimatorSensorDefinitionsFromRobotFactory.getIMUDefinitions(); Map<WrenchCalculatorInterface, ForceSensorDefinition> forceSensors = stateEstimatorSensorDefinitionsFromRobotFactory.getForceSensorDefinitions(); this.simulatedSensorHolderAndReader = new SimulatedSensorHolderAndReader(stateEstimatorSensorDefinitions, sensorProcessingConfiguration, robot.getYoTime(), registry); createAndAddOrientationSensors(imuDefinitions, registry); createAndAddAngularVelocitySensors(imuDefinitions, registry); createAndAddForceSensors(forceSensors, registry); createAndAddLinearAccelerationSensors(imuDefinitions, registry); createAndAddOneDoFPositionAndVelocitySensors(scsToInverseDynamicsJointMap); parentRegistry.addChild(registry); }
@Override public void build(FloatingJointBasics rootJoint, IMUDefinition[] imuDefinition, ForceSensorDefinition[] forceSensorDefinitions, ContactSensorHolder contactSensorHolder, RawJointSensorDataHolderMap rawJointSensorDataHolderMap, JointDesiredOutputList estimatorDesiredJointDataHolder, YoVariableRegistry parentRegistry) { ArrayList<Joint> rootJoints = robot.getRootJoints(); if (rootJoints.size() > 1) { throw new RuntimeException("Robot has more than 1 rootJoint"); } final Joint scsRootJoint = rootJoints.get(0); if (!(scsRootJoint instanceof FloatingJoint)) throw new RuntimeException("Not FloatingJoint rootjoint found"); SCSToInverseDynamicsJointMap scsToInverseDynamicsJointMap = SCSToInverseDynamicsJointMap.createByName((FloatingJoint) scsRootJoint, rootJoint); StateEstimatorSensorDefinitionsFromRobotFactory stateEstimatorSensorDefinitionsFromRobotFactory = new StateEstimatorSensorDefinitionsFromRobotFactory(scsToInverseDynamicsJointMap, imuMounts, groundContactPointBasedWrenchCalculators); this.stateEstimatorSensorDefinitions = stateEstimatorSensorDefinitionsFromRobotFactory.getStateEstimatorSensorDefinitions(); Map<IMUMount, IMUDefinition> imuDefinitions = stateEstimatorSensorDefinitionsFromRobotFactory.getIMUDefinitions(); Map<WrenchCalculatorInterface, ForceSensorDefinition> forceSensors = stateEstimatorSensorDefinitionsFromRobotFactory.getForceSensorDefinitions(); this.simulatedSensorHolderAndReader = new SimulatedSensorHolderAndReader(stateEstimatorSensorDefinitions, sensorProcessingConfiguration, robot.getYoTime(), registry); createAndAddOrientationSensors(imuDefinitions, registry); createAndAddAngularVelocitySensors(imuDefinitions, registry); createAndAddForceSensors(forceSensors, registry); createAndAddLinearAccelerationSensors(imuDefinitions, registry); createAndAddOneDoFPositionAndVelocitySensors(scsToInverseDynamicsJointMap); parentRegistry.addChild(registry); }
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; }
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++; }
@Override public void attachRobot(Robot robot) { registry = new YoVariableRegistry("controllerRegistry"); badGreekVariable = new YoEnum<BadGreekEnum>("badGreekVariable", registry, BadGreekEnum.class); badGreekVariable.set(BadGreekEnum.ALPHA); largeEnumVariable = new YoEnum<LargeEnum>("largeEnumVariable", registry, LargeEnum.class); largeEnumVariable .set(LargeEnum .THE_FOLLOWING_IS_TAKEN_FROM_RAIBERT_1986_ONE_PART_OF_THE_CONTROL_SYSTEM_EXCITED_THE_CYCLIC_MOTION_THAT_UNDERLIES_RUNNING_WHILE_REGULATING_THE_HEIGHT_TO_WHICH_THE_MACHINE_HOPPED); smallEnumVariable = new YoEnum<SmallEnum>("smallEnumVariable", registry, SmallEnum.class); smallEnumVariable.set(SmallEnum.IF); numberVariable = new YoDouble("numberVariable", registry); numberVariable.set(42.0); time = robot.getYoTime(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testMaximumDerivative() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationTestingParameters); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); Random random = new Random(1776L); double value = random.nextDouble(); yoVariableValueDataChecker.setMaximumDerivative(value); assertEquals(yoVariableValueDataChecker.getValueDataCheckerParametersCopy().getMaximumDerivative(), value, EPSILON); yoVariableValueDataChecker.setMaximumDerivative(-value); assertEquals(yoVariableValueDataChecker.getValueDataCheckerParametersCopy().getMaximumDerivative(), value, EPSILON); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testMaximumValue() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationTestingParameters); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); Random random = new Random(1776L); double value = random.nextDouble(); yoVariableValueDataChecker.setMaximumValue(value); assertEquals(yoVariableValueDataChecker.getValueDataCheckerParametersCopy().getMaximumValue(), value, EPSILON); yoVariableValueDataChecker.setMaximumValue(-value); assertFalse(yoVariableValueDataChecker.getValueDataCheckerParametersCopy().getMaximumValue() == value); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testErrorThresholdOnDerivativeComparison() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationTestingParameters); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); Random random = new Random(1776L); double value = random.nextDouble(); yoVariableValueDataChecker.setErrorThresholdOnDerivativeComparison(value); assertEquals(yoVariableValueDataChecker.getValueDataCheckerParametersCopy().getErrorThresholdOnDerivativeComparison(), value, EPSILON); yoVariableValueDataChecker.setErrorThresholdOnDerivativeComparison(-value); assertEquals(yoVariableValueDataChecker.getValueDataCheckerParametersCopy().getErrorThresholdOnDerivativeComparison(), value, EPSILON); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000, expected=RuntimeException.class) public void testSetMinGreaterThanMax() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationTestingParameters); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); double value = 10.0; yoVariableValueDataChecker.setMaximumValue(value); yoVariableValueDataChecker.setMinimumValue(value + 1.0); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000, expected=RuntimeException.class) public void testSetMaxLessThanMin() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationTestingParameters); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); double value = 10.0; yoVariableValueDataChecker.setMinimumValue(value); yoVariableValueDataChecker.setMaximumValue(value - 10.0); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000, expected=RuntimeException.class) public void testMinGreaterThanMax() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSetParameters simulationConstructionSetParameters = SimulationConstructionSetParameters.createFromSystemProperties(); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationConstructionSetParameters); scs.hideViewport(); scs.startOnAThread(); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); yoVariableValueDataChecker.setMaximumValue(1.0); yoVariableValueDataChecker.setMinimumValue(2.0); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000, expected=RuntimeException.class) public void testMaxGreaterThanMin() { Robot robot = new Robot("Derivative"); YoVariableRegistry registry = new YoVariableRegistry("variables"); YoDouble position = new YoDouble("position", registry); robot.addYoVariableRegistry(registry); SimulationConstructionSetParameters simulationConstructionSetParameters = SimulationConstructionSetParameters.createFromSystemProperties(); SimulationConstructionSet scs = new SimulationConstructionSet(robot, simulationConstructionSetParameters); scs.hideViewport(); scs.startOnAThread(); ValueDataCheckerParameters valueDataCheckerParameters = new ValueDataCheckerParameters(); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); yoVariableValueDataChecker.setMinimumValue(2.0); yoVariableValueDataChecker.setMaximumValue(1.0); }
valueDataCheckerParameters.setMinimumValue(-0.9); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters);
valueDataCheckerParameters.setMinimumValue(offset - amplitude - 1.0); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, position, robot.getYoTime(), valueDataCheckerParameters); yoVariableValueDataChecker.cropFirstPoint();