@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout = 30000) public void testRead() { for (int i = 0; i < 10000; i++) { generateAppliedOrientation(); generateAppliedVelocity(); generateAppliedAcceleration(); fullRobotModel.update(randomTransformBodyToWorld, randomLinearVelocity, randomAngularVelocity, randomLinearAcceleration, randomAngularAcceleration); simulatedIMURawSensorReader.read(); rawSensors.getOrientation(actualIMUOrientation, IMU_INDEX); rawSensors.getAngularVelocity(actualAngularVelocity, IMU_INDEX); rawSensors.getAcceleration(actualLinearAcceleration, IMU_INDEX); generateExpectedOrientation(); generateExpectedAngularVelocity(); generateExpectedLinearAcceleration(); assertEqualsRotationMatrix(expectedIMUOrientation, actualIMUOrientation, 1e-3); assertEqualsVector(expectedAngularVelocityInIMUFrame, actualAngularVelocity, 1e-3); assertEqualsVector(expectedLinearAccelerationOfIMUInIMUFrame, actualLinearAcceleration, 1e-3); } }
public ForceSensorHysteresisCreator(double totalRobotMass, String parentJointName, WrenchCalculatorInterface wrenchCalculatorInterface) { super(wrenchCalculatorInterface.getName() + "HysteresisCreator"); this.wrenchCalculatorInterface = wrenchCalculatorInterface; this.wrenchCalculatorInterface.setDoWrenchCorruption(true); this.totalRobotWeightInNewtons = totalRobotMass * 9.81; this.normalForceThreshold = totalRobotWeightInNewtons * PERCENT_OF_FULL_WEIGHT_TO_TRIGGER_HYSTERESIS/100; hysteresisInZDirection = new YoDouble(parentJointName + "ForceSensorZHysteresis", registry); hysteresisInZDirection.set(0); this.isForcePastThresholdFiltered = new GlitchFilteredYoBoolean(parentJointName + "ForceSensorZHysteresisIsForcePastThreshold", registry, ITERS_BEFORE_HYSTERESIS_TRIGGERS); }
private static LidarMount getSensor(FloatingRootJointRobot robot, String sensorName) { ArrayList<LidarMount> lidarSensors = robot.getSensors(LidarMount.class); if(lidarSensors.size() == 0) { System.err.println("DRCLidar: No LIDAR units found on SDF Robot."); return null; } for(LidarMount lidarMount : lidarSensors) { if(lidarMount.getName().equals(sensorName)) { return lidarMount; } } System.err.println("DRCLidar: Could Not find " + sensorName + "Using " + lidarSensors.get(0).getName() + "Instead! FIX THIS!"); return lidarSensors.get(0); }
private void updateMeasurement() { wrenchCalculatorInterface.calculate(); MatrixTools.extractFrameTupleFromEJMLVector(measuredForce, wrenchCalculatorInterface.getWrench(), measurementFrame, 3); measuredForceWorld.setIncludingFrame(measuredForce); measuredForceWorld.changeFrame(worldFrame); yoMeasuredForceWorld.setAndMatchFrame(measuredForce); }
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { OneDegreeOfFreedomJoint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJoint sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(forceTorqueSensorJoint); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
public static void setupDRCRobotLidar(FloatingRootJointRobot robot, Graphics3DAdapter graphics3dAdapter, LocalObjectCommunicator objectCommunicator, DRCRobotJointMap jointMap, DRCRobotLidarParameters lidarParams, TimestampProvider timestampProvider, boolean startLidar) { if (graphics3dAdapter != null) { LidarMount lidarMount = getSensor(robot, lidarParams.getSensorNameInSdf()); LidarScanParameters lidarScanParameters = lidarMount.getLidarScanParameters(); int horizontalRays = lidarScanParameters.pointsPerSweep; int scanHeight = lidarScanParameters.scanHeight; float fov = lidarScanParameters.sweepYawMax - lidarScanParameters.sweepYawMin; float near = lidarScanParameters.minRange; float far = lidarScanParameters.maxRange; DRCLidarCallback callback = new DRCLidarCallback(objectCommunicator, lidarScanParameters, lidarParams.getSensorId()); GPULidar lidar = graphics3dAdapter.createGPULidar(callback, horizontalRays, scanHeight, fov, near, far); lidarMount.setLidar(lidar); } }
@Override public void doControl() { super.doControl(); wrench = wrenchCalculatorInterface.getWrench(); isNormalForcePastHysteresisThreshold(); if(isForcePastThresholdFiltered.getBooleanValue() && unfilteredIsForcePastThresh) //take lots of true readings to turn on, only 1 to turn off { hysteresisSampleCounter++; tmpHysteresis += wrench.get(Wrench.SIZE-1); hasNormalForceGonePastLimit = true; } else { if(hasNormalForceGonePastLimit) { hysteresisInZDirection.set(hysteresisInZDirection.getDoubleValue() + (tmpHysteresis/hysteresisSampleCounter)*HYSTERESIS_PERCENT_OF_LOAD/100); wrenchCalculatorInterface.corruptWrenchElement(Wrench.SIZE-1,hysteresisInZDirection.getDoubleValue()); tmpHysteresis = 0; hysteresisSampleCounter = 0; hasNormalForceGonePastLimit = false; } } }
public static void setupDRCRobotLidar(FloatingRootJointRobot robot, Graphics3DAdapter graphics3dAdapter, LocalObjectCommunicator objectCommunicator, DRCRobotJointMap jointMap, DRCRobotLidarParameters lidarParams, TimestampProvider timestampProvider, boolean startLidar) { if (graphics3dAdapter != null) { LidarMount lidarMount = getSensor(robot, lidarParams.getSensorNameInSdf()); LidarSensorDescription desciption = lidarMount.getDescription(); int horizontalRays = desciption.getPointsPerSweep(); int scanHeight = desciption.getScanHeight(); float fov = (float) (desciption.getSweepYawMax() - desciption.getSweepYawMin()); float near = (float) desciption.getMinRange(); float far = (float) desciption.getMaxRange(); LidarScanParameters lidarScanParameters = new LidarScanParameters(desciption.getPointsPerSweep(), desciption.getScanHeight(), (float) desciption.getSweepYawMin(), (float) desciption.getSweepYawMax(), (float) desciption.getHeightPitchMin(), (float) desciption.getHeightPitchMax(), 0, (float) desciption.getMinRange(), (float) desciption.getMaxRange(), 0, 0); DRCLidarCallback callback = new DRCLidarCallback(objectCommunicator, lidarScanParameters, lidarParams.getSensorId()); GPULidar lidar = graphics3dAdapter.createGPULidar(callback, horizontalRays, scanHeight, fov, near, far); lidarMount.setLidar(lidar); } }
public void packFootForceSensors(SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors) { ArrayList<WrenchCalculatorInterface> forceSensors = new ArrayList<WrenchCalculatorInterface>(); sdfRobot.getForceSensors(forceSensors); SideDependentList<String> jointNamesBeforeFeet = sdfRobot.getJointNamesBeforeFeet(); for (RobotSide robotSide : RobotSide.values) { footForceSensors.put(robotSide, new ArrayList<WrenchCalculatorInterface>()); for (int i = 0; i < forceSensors.size(); i++) { if (forceSensors.get(i).getJoint().getName().equals(jointNamesBeforeFeet.get(robotSide))) { footForceSensors.get(robotSide).add(forceSensors.get(i)); } } } }
public ArrayList<RobotController> getFootForceSensorHysteresisCreators() { SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors = new SideDependentList<ArrayList<WrenchCalculatorInterface>>(); packFootForceSensors(footForceSensors); ArrayList<RobotController> footForceSensorSignalCorruptors = new ArrayList<RobotController>(); for (RobotSide robotSide : RobotSide.values) { for (int i = 0; i < footForceSensors.get(robotSide).size(); i++) { ForceSensorHysteresisCreator forceSensorSignalCorruptor = new ForceSensorHysteresisCreator(sdfRobot.computeCenterOfMass(new Point3D()), footForceSensors.get(robotSide).get(i).getName(), footForceSensors.get(robotSide).get(i)); footForceSensorSignalCorruptors.add(forceSensorSignalCorruptor); } } return footForceSensorSignalCorruptors; }
@Before public void setUp() throws Exception { transformIMUToJoint.setRotation(jointToIMURotation); transformIMUToJoint.setTranslation(jointToIMUOffset); transformJointToIMU.setAndInvert(transformIMUToJoint); imuFrame = fullRobotModel.createOffsetFrame(fullRobotModel.getBodyLink().getParentJoint(), transformIMUToJoint, "imuFrame"); Vector3D linearAcceleration = new Vector3D(0.0, 0.0, GRAVITY); Vector3D angularAcceleration = new Vector3D(); ReferenceFrame rootBodyFrame = fullRobotModel.getElevatorFrame(); SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBodyFrame, ReferenceFrame.getWorldFrame(), rootBodyFrame, angularAcceleration, linearAcceleration); simulatedIMURawSensorReader = new PerfectSimulatedIMURawSensorReader(rawSensors, IMU_INDEX, rigidBody, imuFrame, fullRobotModel.getElevator(), rootAcceleration); simulatedIMURawSensorReader.initialize(); }
public void update(RigidBodyTransform transformBodyToWorld, FrameVector3D linearVelocity, FrameVector3D angularVelocity, FrameVector3D linearAcceleration, FrameVector3D angularAcceleration) { // Update Body Pose rootJoint.setJointConfiguration(transformBodyToWorld); // TODO correct? updateFrames(); // Update Body Velocity Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocity, linearVelocity); rootJoint.setJointTwist(bodyTwist); // Update Body Acceleration SpatialAcceleration accelerationOfChestWithRespectToWorld = new SpatialAcceleration(bodyFrame, worldFrame, bodyFrame, angularAcceleration, linearAcceleration); accelerationOfChestWithRespectToWorld.setBaseFrame(getElevatorFrame()); rootJoint.setJointAcceleration(accelerationOfChestWithRespectToWorld); updateFrames(); }
private void generateExpectedLinearAcceleration() { FrameVector3D centerAppliedAccelerationPart = new FrameVector3D(randomLinearAcceleration); FrameVector3D centerCoriolisAccelerationPart = new FrameVector3D(bodyFrame); centerCoriolisAccelerationPart.cross(randomAngularVelocity, randomLinearVelocity); FrameVector3D gravitationalAccelerationPart = new FrameVector3D(fullRobotModel.getWorldFrame()); gravitationalAccelerationPart.setZ(GRAVITY); gravitationalAccelerationPart.changeFrame(bodyFrame); FrameVector3D centripedalAccelerationPart = new FrameVector3D(bodyFrame); centripedalAccelerationPart.cross(randomAngularVelocity, jointToIMUOffset); centripedalAccelerationPart.cross(randomAngularVelocity, centripedalAccelerationPart); FrameVector3D angularAccelerationPart = new FrameVector3D(bodyFrame); angularAccelerationPart.cross(randomAngularAcceleration, jointToIMUOffset); expectedLinearAccelerationOfIMUInIMUFrame.set(centerAppliedAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(centerCoriolisAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(gravitationalAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(centripedalAccelerationPart); expectedLinearAccelerationOfIMUInIMUFrame.add(angularAccelerationPart); transformJointToIMU.transform(expectedLinearAccelerationOfIMUInIMUFrame); }
@Override public void read() { // Think about adding root body acceleration to the fullrobotmodel readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations(); readAndUpdateRootJointPositionAndOrientation(); updateReferenceFrames(); readAndUpdateRootJointAngularAndLinearVelocity(); long timestamp = TimeTools.secondsToNanoSeconds(robot.getTime()); this.timestamp.set(timestamp); this.visionSensorTimestamp.set(timestamp); this.sensorHeadPPSTimetamp.set(timestamp); if (forceSensorDataHolderToUpdate != null) { for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet()) { final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue(); forceTorqueSensor.calculate(); forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench()); } } }
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { Joint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJointBasics sensorParentJoint; if (forceTorqueSensorJoint instanceof OneDegreeOfFreedomJoint) sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint((OneDegreeOfFreedomJoint) forceTorqueSensorJoint); else throw new RuntimeException("Force sensor is only supported for OneDegreeOfFreedomJoint."); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
public static void setupDRCRobotLidar(FloatingRootJointRobot robot, Graphics3DAdapter graphics3dAdapter, LocalObjectCommunicator objectCommunicator, DRCRobotJointMap jointMap, DRCRobotLidarParameters lidarParams, TimestampProvider timestampProvider, boolean startLidar) { if (graphics3dAdapter != null) { LidarMount lidarMount = getSensor(robot, lidarParams.getSensorNameInSdf()); LidarScanParameters lidarScanParameters = lidarMount.getLidarScanParameters(); int horizontalRays = lidarScanParameters.pointsPerSweep; int scanHeight = lidarScanParameters.scanHeight; float fov = lidarScanParameters.sweepYawMax - lidarScanParameters.sweepYawMin; float near = lidarScanParameters.minRange; float far = lidarScanParameters.maxRange; DRCLidarCallback callback = new DRCLidarCallback(objectCommunicator, lidarScanParameters, lidarParams.getSensorId()); GPULidar lidar = graphics3dAdapter.createGPULidar(callback, horizontalRays, scanHeight, fov, near, far); lidarMount.setLidar(lidar); } }
public static LidarMount getSensor(FloatingRootJointRobot robot, String sensorName) { ArrayList<LidarMount> lidarSensors = robot.getSensors(LidarMount.class); if (lidarSensors.size() == 0) { System.err.println("DRCLidar: No LIDAR units found on SDF Robot."); return null; } for (LidarMount lidarMount : lidarSensors) { if (lidarMount.getName().equals(sensorName)) { return lidarMount; } } System.err.println("DRCLidar: Could Not find " + sensorName + "Using " + lidarSensors.get(0).getName() + "Instead! FIX THIS!"); return lidarSensors.get(0); }
@Override public void read() { readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations(); readAndUpdateRootJointPositionAndOrientation(); readAndUpdateRootJointAngularAndLinearVelocity(); readAndUpdateIMUSensors(); long timestamp = Conversions.secondsToNanoseconds(robot.getTime()); sensorOutputMap.setTimestamp(timestamp); sensorOutputMap.setVisionSensorTimestamp(timestamp); sensorOutputMap.setSensorHeadPPSTimetamp(timestamp); if (forceSensorDataHolderToUpdate != null) { for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet()) { final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue(); forceTorqueSensor.calculate(); forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench()); } } }
private static LidarMount getSensor(FloatingRootJointRobot robot, String sensorName) { ArrayList<LidarMount> lidarSensors = robot.getSensors(LidarMount.class); if(lidarSensors.size() == 0) { System.err.println("DRCLidar: No LIDAR units found on SDF Robot."); return null; } for(LidarMount lidarMount : lidarSensors) { if(lidarMount.getName().equals(sensorName)) { return lidarMount; } } System.err.println("DRCLidar: Could Not find " + sensorName + "Using " + lidarSensors.get(0).getName() + "Instead! FIX THIS!"); return lidarSensors.get(0); }
@Override public void read() { // Think about adding root body acceleration to the fullrobotmodel readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations(); readAndUpdateRootJointPositionAndOrientation(); readAndUpdateRootJointAngularAndLinearVelocity(); // Update frames after setting angular and linear velocities to correctly update zup frames updateReferenceFrames(); long timestamp = Conversions.secondsToNanoseconds(robot.getTime()); this.timestamp.set(timestamp); this.visionSensorTimestamp.set(timestamp); this.sensorHeadPPSTimetamp.set(timestamp); if (forceSensorDataHolderToUpdate != null) { for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet()) { final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue(); forceTorqueSensor.calculate(); forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench()); } } }