From 6c0a63b688b00350bce4951f586d8e0a7f759f2d Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Fri, 12 Dec 2025 16:38:09 -0600 Subject: [PATCH 1/3] Cast to publish all joints. --- .../main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java index f36d38f250a8..c78a04297adf 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java @@ -24,6 +24,7 @@ import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics; import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.robotModels.FullRobotModel; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SegmentDependentList; import us.ihmc.robotics.robotSide.SideDependentList; @@ -793,7 +794,7 @@ public RobotConfigurationDataPublisher getRobotConfigurationDataPublisher() forceSensorDataHolderToSend = getMainStateEstimator().getForceSensorOutputWithGravityCancelled(); RobotConfigurationDataPublisherFactory factory = new RobotConfigurationDataPublisherFactory(); - factory.setDefinitionsToPublish(getEstimatorFullRobotModel()); + factory.setDefinitionsToPublish((FullRobotModel) getEstimatorFullRobotModel()); factory.setSensorSource(getEstimatorFullRobotModel(), forceSensorDataHolderToSend, getRawSensorOutputMap()); factory.setRobotMotionStatusHolder(getRobotMotionStatusFromController()); factory.setROS2Info(realtimeROS2NodeField.get(), outputTopicField.get()); From 9f070edce48330cd9fadcf432bed539b1f0ea0e8 Mon Sep 17 00:00:00 2001 From: ihmc-rosie Date: Fri, 12 Dec 2025 16:41:41 -0600 Subject: [PATCH 2/3] Expect all one DoF joints in RCD. --- .../ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java index 8a666bd76f17..14fa2388c48d 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java @@ -13,7 +13,6 @@ import us.ihmc.log.LogTools; import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics; import us.ihmc.robotModels.FullHumanoidRobotModel; -import us.ihmc.robotModels.FullRobotModelUtils; import us.ihmc.robotics.partNames.HandJointName; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; @@ -53,7 +52,7 @@ public CommunicationsSyncedRobotModel(DRCRobotModel robotModel, this.handModels = handModels; robotConfigurationData = new RobotConfigurationData(); referenceFrames = new HumanoidReferenceFrames(fullRobotModel, sensorInformation); - allJoints = FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel); + allJoints = fullRobotModel.getOneDoFJoints(); if (handModels != null) HandModelUtils.getHandJoints(handModels, fullRobotModel, handJoints); From a505438b57496841c8b593e795169c4ea0cdb0ac Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Fri, 12 Dec 2025 16:47:46 -0600 Subject: [PATCH 3/3] Try something simpler. --- .../main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java | 3 +-- .../producers/RobotConfigurationDataPublisherFactory.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java index c78a04297adf..f36d38f250a8 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/AvatarEstimatorThreadFactory.java @@ -24,7 +24,6 @@ import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics; import us.ihmc.robotModels.FullHumanoidRobotModel; -import us.ihmc.robotModels.FullRobotModel; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SegmentDependentList; import us.ihmc.robotics.robotSide.SideDependentList; @@ -794,7 +793,7 @@ public RobotConfigurationDataPublisher getRobotConfigurationDataPublisher() forceSensorDataHolderToSend = getMainStateEstimator().getForceSensorOutputWithGravityCancelled(); RobotConfigurationDataPublisherFactory factory = new RobotConfigurationDataPublisherFactory(); - factory.setDefinitionsToPublish((FullRobotModel) getEstimatorFullRobotModel()); + factory.setDefinitionsToPublish(getEstimatorFullRobotModel()); factory.setSensorSource(getEstimatorFullRobotModel(), forceSensorDataHolderToSend, getRawSensorOutputMap()); factory.setRobotMotionStatusHolder(getRobotMotionStatusFromController()); factory.setROS2Info(realtimeROS2NodeField.get(), outputTopicField.get()); diff --git a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisherFactory.java b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisherFactory.java index bf1cc081f805..23a3b47ad524 100644 --- a/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisherFactory.java +++ b/ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/communication/producers/RobotConfigurationDataPublisherFactory.java @@ -138,7 +138,7 @@ public void setSensorSource(SensorTimestampHolder sensorTimestampHolder, */ public void setDefinitionsToPublish(FullHumanoidRobotModel fullHumanoidRobotModel) { - setDefinitionsToPublish(FullRobotModelUtils.getAllJointsExcludingHands(fullHumanoidRobotModel), + setDefinitionsToPublish(fullHumanoidRobotModel.getOneDoFJoints(), fullHumanoidRobotModel.getForceSensorDefinitions(), fullHumanoidRobotModel.getIMUDefinitions()); }