From 7485a7ac8367ae08416ce7f4d3b09201e8ab1486 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Thu, 1 May 2025 11:09:28 -0500 Subject: [PATCH] Generate Javadoc for sensor data context. --- .../simulatedSensors/SensorDataContext.java | 127 ++++++++++++++++-- 1 file changed, 117 insertions(+), 10 deletions(-) diff --git a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SensorDataContext.java b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SensorDataContext.java index 1dbbc4475079..b0f28bca5533 100644 --- a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SensorDataContext.java +++ b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SensorDataContext.java @@ -17,49 +17,84 @@ import us.ihmc.sensorProcessing.outputData.ImuData; import us.ihmc.sensorProcessing.outputData.LowLevelState; +/** + * A class for managing sensor data from joints, IMUs, and force sensors in a robotic system. + * It provides methods to register sensors, retrieve their measurements, and manage the data context. + * Implements {@link Settable} to allow copying data from another {@code SensorDataContext}. + */ public class SensorDataContext implements Settable { + /** List of registered joint names. */ private final List jointNames = new ArrayList<>(); + /** List of joint measurements. */ private final List jointMeasurements = new ArrayList<>(); + /** Set of joint name hash codes for quick lookup. */ private final transient TLongSet jointSet = new TLongHashSet(); + /** Map of joint names to their measurements. */ private final transient Map jointMeasurementMap = new HashMap<>(); + /** List of registered IMU names. */ private final List imuNames = new ArrayList<>(); + /** List of IMU measurements. */ private final List imuMeasurements = new ArrayList<>(); + /** Set of IMU name hash codes for quick lookup. */ private final transient TLongSet imuSet = new TLongHashSet(); + /** Map of IMU names to their measurements. */ private final transient Map imuMeasurementMap = new HashMap<>(); + /** List of registered force sensor names. */ private final List forceSensorNames = new ArrayList<>(); + /** List of force sensor measurements. */ private final List forceSensorMeasurements = new ArrayList<>(); + /** Set of force sensor name hash codes for quick lookup. */ private final transient TLongSet forceSensorSet = new TLongHashSet(); + /** Map of force sensor names to their measurements. */ private final transient Map forceSensorMeasurementMap = new HashMap<>(); + /** + * Constructs an empty {@code SensorDataContext}. + */ public SensorDataContext() { } + /** + * Constructs a {@code SensorDataContext} and registers all joints, IMUs, and force sensors from the provided robot model. + * + * @param fullRobotModel The robot model containing joint, IMU, and force sensor definitions. + */ public SensorDataContext(FullRobotModel fullRobotModel) { Arrays.asList(fullRobotModel.getOneDoFJoints()).forEach(joint -> registerJoint(joint.getName())); Arrays.asList(fullRobotModel.getIMUDefinitions()).forEach(imu -> registerImu(imu.getName())); - Arrays.asList(fullRobotModel.getForceSensorDefinitions()).forEach(forceSensor -> registerForceSensor(forceSensor.getSensorName())); + Arrays.asList(fullRobotModel.getForceSensorDefinitions()).forEach(sensor -> registerForceSensor(sensor.getSensorName())); } + /** + * Constructs a {@code SensorDataContext} and registers the provided joints. + * + * @param joints The list of joints to register. + */ public SensorDataContext(List joints) { joints.forEach(joint -> registerJoint(joint.getName())); } + /** + * Registers a joint with the given name and returns its associated measurement object. + * Reuses existing measurement data if the joint was previously registered. + * + * @param jointName The name of the joint to register. + * @return The {@link LowLevelState} object for the joint's measurements. + * @throws RuntimeException If a joint with the same name hash is already registered. + */ public LowLevelState registerJoint(String jointName) { - // Check if a joint with the same name is already registered. - // It is very unlikely but it could happen that two joint names produce the same hash code. if (isJointRegistered(jointName)) { throw new RuntimeException("Joint with same name hash as " + jointName + " is already registered."); } - // If the joint was ever registered in the past reuse the existing data structure. LowLevelState lowLevelState = jointMeasurementMap.get(jointName); if (lowLevelState == null) { @@ -74,16 +109,21 @@ public LowLevelState registerJoint(String jointName) return lowLevelState; } + /** + * Registers an IMU with the given name and returns its associated measurement object. + * Reuses existing measurement data if the IMU was previously registered. + * + * @param imuName The name of the IMU to register. + * @return The {@link ImuData} object for the IMU's measurements. + * @throws RuntimeException If an IMU with the same name hash is already registered. + */ public ImuData registerImu(String imuName) { - // Check if an IMU with the same name is already registered. - // It is very unlikely but it could happen that two IMU names produce the same hash code. if (isImuRegistered(imuName)) { throw new RuntimeException("IMU with same name hash as " + imuName + " is already registered."); } - // If the IMU was ever registered in the past reuse the existing data structure. ImuData imuData = imuMeasurementMap.get(imuName); if (imuData == null) { @@ -98,16 +138,21 @@ public ImuData registerImu(String imuName) return imuData; } + /** + * Registers a force sensor with the given name and returns its associated measurement matrix. + * Reuses existing measurement data if the force sensor was previously registered. + * + * @param forceSensorName The name of the force sensor to register. + * @return The {@link DMatrixRMaj} object for the force sensor's measurements (6x1 matrix). + * @throws RuntimeException If a force sensor with the same name hash is already registered. + */ public DMatrixRMaj registerForceSensor(String forceSensorName) { - // Check if an force sensor with the same name is already registered. - // It is very unlikely but it could happen that two force sensor names produce the same hash code. if (isForceSensorRegistered(forceSensorName)) { throw new RuntimeException("Force Sensor with same name hash as " + forceSensorName + " is already registered."); } - // If the force sensor was ever registered in the past reuse the existing data structure. DMatrixRMaj forceSensorData = forceSensorMeasurementMap.get(forceSensorName); if (forceSensorData == null) { @@ -122,6 +167,13 @@ public DMatrixRMaj registerForceSensor(String forceSensorName) return forceSensorData; } + /** + * Retrieves the measurement data for the specified joint. + * + * @param jointName The name of the joint. + * @return The {@link LowLevelState} object containing the joint's measurements. + * @throws RuntimeException If the joint is not registered. + */ public LowLevelState getMeasuredJointState(String jointName) { if (!isJointRegistered(jointName)) @@ -131,6 +183,13 @@ public LowLevelState getMeasuredJointState(String jointName) return jointMeasurementMap.get(jointName); } + /** + * Retrieves the measurement data for the specified IMU. + * + * @param imuName The name of the IMU. + * @return The {@link ImuData} object containing the IMU's measurements. + * @throws RuntimeException If the IMU is not registered. + */ public ImuData getImuMeasurement(String imuName) { if (!isImuRegistered(imuName)) @@ -140,6 +199,13 @@ public ImuData getImuMeasurement(String imuName) return imuMeasurementMap.get(imuName); } + /** + * Retrieves the measurement data for the specified force sensor. + * + * @param forceSensorName The name of the force sensor. + * @return The {@link DMatrixRMaj} object containing the force sensor's measurements. + * @throws RuntimeException If the force sensor is not registered. + */ public DMatrixRMaj getForceSensorMeasurement(String forceSensorName) { if (!isForceSensorRegistered(forceSensorName)) @@ -149,21 +215,42 @@ public DMatrixRMaj getForceSensorMeasurement(String forceSensorName) return forceSensorMeasurementMap.get(forceSensorName); } + /** + * Checks if a joint with the given name is registered. + * + * @param jointName The name of the joint. + * @return {@code true} if the joint is registered, {@code false} otherwise. + */ public boolean isJointRegistered(String jointName) { return jointSet.contains(jointName.hashCode()); } + /** + * Checks if an IMU with the given name is registered. + * + * @param imuName The name of the IMU. + * @return {@code true} if the IMU is registered, {@code false} otherwise. + */ public boolean isImuRegistered(String imuName) { return imuSet.contains(imuName.hashCode()); } + /** + * Checks if a force sensor with the given name is registered. + * + * @param forceSensorName The name of the force sensor. + * @return {@code true} if the force sensor is registered, {@code false} otherwise. + */ public boolean isForceSensorRegistered(String forceSensorName) { return forceSensorSet.contains(forceSensorName.hashCode()); } + /** + * Clears all registered sensors and their measurements. + */ public void clear() { jointNames.clear(); @@ -179,6 +266,12 @@ public void clear() forceSensorSet.clear(); } + /** + * Copies the data from another {@code SensorDataContext} into this instance. + * Clears existing data and registers all sensors from the other context. + * + * @param other The {@code SensorDataContext} to copy from. + */ @Override public void set(SensorDataContext other) { @@ -197,6 +290,14 @@ public void set(SensorDataContext other) } } + /** + * Compares this {@code SensorDataContext} with another object for equality. + * Two {@code SensorDataContext} instances are equal if they have the same registered sensors + * with equal measurements. + * + * @param obj The object to compare with. + * @return {@code true} if the objects are equal, {@code false} otherwise. + */ @Override public boolean equals(Object obj) { @@ -249,6 +350,12 @@ else if (obj instanceof SensorDataContext) } } + /** + * Returns a string representation of this {@code SensorDataContext}, listing all registered + * sensors and their measurements. + * + * @return A string describing the sensor data. + */ @Override public String toString() {