diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java index 2e7cb1ae6f24..28aa175a8df8 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/HumanoidKinematicsToolboxController.java @@ -69,7 +69,6 @@ public class HumanoidKinematicsToolboxController extends KinematicsToolboxController { private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); - private static final Vector3D zeroVector = new Vector3D(); private static final double FOOT_COEFFICIENT_OF_FRICTION = 0.8; private static final double HAND_COEFFICIENT_OF_FRICTION = 0.4; @@ -245,6 +244,9 @@ public HumanoidKinematicsToolboxController(CommandInputManager commandInputManag setupVisualization(desiredFullRobotModel.getFoot(robotSide)); } + setupVisualization(desiredFullRobotModel.getPelvis()); + setupVisualization(desiredFullRobotModel.getChest()); + for (RobotSide robotSide : RobotSide.values) { String side = robotSide.getCamelCaseNameForMiddleOfExpression(); @@ -529,14 +531,10 @@ public void updateInternal() if (multiContactRegionCalculator.hasSolvedWholeRegion()) { activeContactPointPositions.clear(); - getSolution().getSupportRegion().clear(); - for (int i = 0; i < multiContactRegionCalculator.getNumberOfVertices(); i++) { activeContactPointPositions.add().set(multiContactRegionCalculator.getOptimizedVertex(i)); - getSolution().getSupportRegion().add().set(multiContactRegionCalculator.getOptimizedVertex(i)); } - updateSupportPolygonConstraint(activeContactPointPositions); } } @@ -733,6 +731,7 @@ private void processCapturabilityBasedStatus(CapturabilityBasedStatus capturabil } // CoM constraint polygon is the convex hull of the feet contact points. Even when upper body is load-bearing, initialize to this. + activeContactPointPositions.clear(); Object leftFootSupportPolygon2d = capturabilityBasedStatus.getLeftFootSupportPolygon3d(); Object rightFootSupportPolygon2d = capturabilityBasedStatus.getRightFootSupportPolygon3d(); for (int i = 0; i < leftFootSupportPolygon2d.size(); i++) @@ -813,7 +812,9 @@ protected void getAdditionalFeedbackControlCommands(FeedbackControlCommandBuffer { addHoldSupportEndEffectorCommands(bufferToPack); addHoldCenterOfMassXYCommand(bufferToPack); - stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack); + + double supportRegionSensitivity = stabilityCostCalculator.addPostureFeedbackCommands(bufferToPack); + getSolution().setSupportRegionSensitivity(supportRegionSensitivity); } @Override diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java index 435627ddcf73..dc3d20e9ead2 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsToolboxModule/StabilityMarginKinematicsCostCalculator.java @@ -25,6 +25,7 @@ public class StabilityMarginKinematicsCostCalculator { private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + private static final double NULL_POSTURAL_SENSITIVITY = -1.0; private final WholeBodyContactState wholeBodyContactState; private final StabilityMarginRegionCalculator multiContactRegionCalculator; @@ -65,7 +66,10 @@ public StabilityMarginKinematicsCostCalculator(WholeBodyContactState wholeBodyCo pelvisControlFramePose.setToZero(afterRootJointFrame); pelvisControlFramePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); - this.stabilityGradientCalculator = new SensitivityBasedStabilityGradientCalculator(fullRobotModel, wholeBodyContactState, multiContactRegionCalculator, registry); + this.stabilityGradientCalculator = new SensitivityBasedStabilityGradientCalculator(fullRobotModel, + wholeBodyContactState, + multiContactRegionCalculator, + registry); parentRegistry.addChild(registry); } @@ -74,26 +78,32 @@ public void setEnabled(boolean enable) isEnabled.set(enable); } - public void addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack) + public boolean isEnabled() { - if (!isEnabled.getBooleanValue() || !isUpperBodyLoadBearing.getValue()) - return; + return isEnabled.getBooleanValue(); + } - double stabilityMargin = multiContactRegionCalculator.getStabilityMargin(); - if (stabilityMargin > maxMarginToPenalize.getValue()) - return; + /** + * Computes and packs the feedback objective. Returns the postural sensitivity + */ + public double addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack) + { + if (!isUpperBodyLoadBearing.getValue() || !multiContactRegionCalculator.hasSolvedWholeRegion()) + return NULL_POSTURAL_SENSITIVITY; + double stabilityMargin = multiContactRegionCalculator.getStabilityMargin(); double deltaStabilityMargin = stabilityMargin - minStabilityMargin.getValue(); double alpha = EuclidCoreTools.clamp(deltaStabilityMargin / minStabilityMargin.getValue(), 0.0, 1.0); double weight = alpha * stabilityMarginWeight.getValue(); stabilityGradientCalculator.update(); - DMatrixRMaj stabilityMarginGradient = stabilityGradientCalculator.getStabilityMarginGradient(); - double stabilityGradientMagnitude = CommonOps_DDRM.dot(stabilityMarginGradient, stabilityMarginGradient); - if (stabilityGradientMagnitude < 1.0e-5) - return; + double posturalSensitivity = stabilityGradientCalculator.getPostureSensitivity(); + + if (!isEnabled.getValue() || posturalSensitivity < 1.0e-3) + return posturalSensitivity; - double gradientScalar = desiredStabilityMarginVelocity.getValue() / stabilityGradientMagnitude; + DMatrixRMaj stabilityMarginGradient = stabilityGradientCalculator.getStabilityMarginGradient(); + double gradientScalar = desiredStabilityMarginVelocity.getValue() / EuclidCoreTools.square(posturalSensitivity); // Feed-forward joint velocities OneDoFJointBasics[] oneDoFJoints = wholeBodyContactState.getOneDoFJoints(); @@ -131,5 +141,7 @@ public void addPostureFeedbackCommands(FeedbackControlCommandBuffer bufferToPack spatialFeedbackControlCommand.getGains().getPositionGains().setDerivativeGains(0.0); spatialFeedbackControlCommand.getGains().getOrientationGains().setProportionalGains(0.0); spatialFeedbackControlCommand.getGains().getOrientationGains().setDerivativeGains(0.0); + + return posturalSensitivity; } } diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java index d9aa76278522..c99e42423ca3 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/IKStreamingRTPluginFactory.java @@ -224,6 +224,13 @@ public IKStreamingRTThread(String robotName, fullRobotModelFactory, yoGraphicsListRegistry, registry); + + List inactiveJoints = parameters.getInactiveJoints(); + for (int i = 0; i < inactiveJoints.size(); i++) + { + kinematicsStreamingToolboxController.getActiveOptimizationSettings().deactivateJoint(desiredFullRobotModel.getOneDoFJointByName(inactiveJoints.get(i))); + } + kinematicsStreamingToolboxController.setCollisionModel(collisionModel); MessageUnpacker wholeBodyStreamingMessageUnpacker = MessageUnpackingTools.createWholeBodyStreamingMessageUnpacker(); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java index 0f28542f294c..74f138be4bed 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxController.java @@ -8,6 +8,7 @@ import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicsToolboxModule; import us.ihmc.avatar.networkProcessor.kinemtaticsStreamingToolboxModule.KinematicsStreamingToolboxParameters.ClockType; import us.ihmc.avatar.networkProcessor.modules.ToolboxController; +import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseKinematics.InverseKinematicsOptimizationSettingsCommand; import us.ihmc.commons.Conversions; import us.ihmc.communication.controllerAPI.CommandInputManager; import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; @@ -253,6 +254,11 @@ public void setRobotStateUpdater(IKRobotStateUpdater robotStateUpdater) tools.setRobotStateUpdater(robotStateUpdater); } + public InverseKinematicsOptimizationSettingsCommand getActiveOptimizationSettings() + { + return tools.getIKController().getActiveOptimizationSettings(); + } + public void updateCapturabilityBasedStatus(CapturabilityBasedStatus newStatus) { tools.updateCapturabilityBasedStatus(newStatus); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java index 626707f08e10..8298d4748b8f 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxModule.java @@ -82,6 +82,11 @@ public KinematicsStreamingToolboxModule(DRCRobotModel robotModel, registry); controller.setRobotStateUpdater(robotStateUpdater); controller.setCollisionModel(robotModel.getHumanoidRobotKinematicsCollisionModel()); + List inactiveJoints = parameters.getInactiveJoints(); + for (int i = 0; i < inactiveJoints.size(); i++) + { + controller.getActiveOptimizationSettings().deactivateJoint(controller.getDesiredFullRobotModel().getOneDoFJointByName(inactiveJoints.get(i))); + } Map initialConfiguration = fromStandPrep(robotModel); if (initialConfiguration != null) controller.setInitialRobotConfigurationNamedMap(initialConfiguration); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java index 28cbe1d59c1c..842c396609ee 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinemtaticsStreamingToolboxModule/KinematicsStreamingToolboxParameters.java @@ -9,6 +9,8 @@ import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; import us.ihmc.commons.UnitConversions; +import java.util.ArrayList; +import java.util.List; import java.util.Map; public class KinematicsStreamingToolboxParameters @@ -269,6 +271,8 @@ public enum ClockType private double solverPrivilegedDefaultWeight; private double solverPrivilegedDefaultGain; + private final List inactiveJoints = new ArrayList<>(); + public static KinematicsStreamingToolboxParameters defaultParameters() { KinematicsStreamingToolboxParameters parameters = new KinematicsStreamingToolboxParameters(); @@ -1059,4 +1063,9 @@ public void setSolverPrivilegedDefaultGain(double solverPrivilegedDefaultGain) { this.solverPrivilegedDefaultGain = solverPrivilegedDefaultGain; } + + public List getInactiveJoints() + { + return inactiveJoints; + } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java index f82c67ec5798..694643cc2034 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXMultiContactRegionGraphic.java @@ -31,21 +31,22 @@ public class RDXMultiContactRegionGraphic implements RenderableProvider { - private static final Color POLYGON_GRAPHIC_COLOR = Color.valueOf("DEE933"); + private static final Color NOMINAL_POLYGON_GRAPHIC_COLOR = Color.valueOf("DEE933"); + private static final Color LOW_STABILITY_POLYGON_GRAPHIC_COLOR = Color.valueOf("EB3D40"); private static final double STABILITY_GRAPHIC_HEIGHT = 2.0; - private final ConvexPolygon2D multiContactSupportRegion = new ConvexPolygon2D(); + private final ConvexPolygon2D supportRegion = new ConvexPolygon2D(); private final FramePoint3D comCurrent = new FramePoint3D(); + private final FramePoint3D comDesired = new FramePoint3D(); private final FramePoint3D comXYAtFootHeight = new FramePoint3D(); - private final ConvexPolygon2D closestProximityEdge = new ConvexPolygon2D(); + private final FramePoint3D desiredCoMXYAtFootHeight = new FramePoint3D(); private int minimumEdgeIndex; private double minimumEdgeDistance; private final ModelBuilder modelBuilder = new ModelBuilder(); private final RDXMultiColorMeshBuilder meshBuilder = new RDXMultiColorMeshBuilder(); - private final FullHumanoidRobotModel ghostFullRobotModel; private final CenterOfMassReferenceFrame centerOfMassFrame; private final MidFrameZUpFrame midFeetZUpFrame; private final RigidBodyTransform transform = new RigidBodyTransform(); @@ -55,7 +56,6 @@ public class RDXMultiContactRegionGraphic implements RenderableProvider public RDXMultiContactRegionGraphic(FullHumanoidRobotModel ghostFullRobotModel) { - this.ghostFullRobotModel = ghostFullRobotModel; this.centerOfMassFrame = new CenterOfMassReferenceFrame("ghostCoMFrame", ReferenceFrame.getWorldFrame(), ghostFullRobotModel.getRootBody()); this.midFeetZUpFrame = new MidFrameZUpFrame("midFeedZUpWhost", ReferenceFrame.getWorldFrame(), @@ -63,60 +63,90 @@ public RDXMultiContactRegionGraphic(FullHumanoidRobotModel ghostFullRobotModel) ghostFullRobotModel.getSoleFrame(RobotSide.RIGHT)); } - public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus) + public void update(KinematicsToolboxOutputStatus kinematicsToolboxOutputStatus, FramePoint3D desiredCoMPosition) { - multiContactSupportRegion.clear(); - Object supportRegion = kinematicsToolboxOutputStatus.getSupportRegion(); - - for (int i = 0; i < supportRegion.size(); i++) - { - multiContactSupportRegion.addVertex(supportRegion.get(i)); - } - - multiContactSupportRegion.update(); - - if (multiContactSupportRegion.getNumberOfVertices() < 3) - { - modelInstance = null; - lastModel = null; - return; - } - meshBuilder.clear(); + // Update frames and compute mid-feet height centerOfMassFrame.update(); midFeetZUpFrame.update(); - updateMinimumEdge(); FramePoint3D midFoot = new FramePoint3D(midFeetZUpFrame); midFoot.changeFrame(ReferenceFrame.getWorldFrame()); double footZ = midFoot.getZ(); - transform.setTranslationAndIdentityRotation(0.0, 0.0, footZ); - - for (int i = 0; i < multiContactSupportRegion.getNumberOfVertices(); i++) + // Update desired and achieved CoM graphic { - Point2DReadOnly v0 = multiContactSupportRegion.getVertex(i); - Point2DReadOnly v1 = multiContactSupportRegion.getNextVertex(i); + comCurrent.setToZero(centerOfMassFrame); + comCurrent.changeFrame(ReferenceFrame.getWorldFrame()); + + meshBuilder.addSphere(0.03f, comCurrent, Color.BLACK); + + comXYAtFootHeight.setIncludingFrame(comCurrent); + comXYAtFootHeight.setZ(footZ); + meshBuilder.addSphere(0.03f, comXYAtFootHeight, Color.BLACK); + + FramePoint3D comXYElevated = new FramePoint3D(comXYAtFootHeight); + comXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT); + meshBuilder.addLine(comXYAtFootHeight, comXYElevated, 0.005f, Color.BLACK); + + if (desiredCoMPosition != null) + { + comDesired.setMatchingFrame(desiredCoMPosition); + meshBuilder.addSphere(0.03f, comDesired, Color.GREEN); + + desiredCoMXYAtFootHeight.setIncludingFrame(comDesired); + desiredCoMXYAtFootHeight.setZ(footZ); + meshBuilder.addSphere(0.03f, desiredCoMXYAtFootHeight, Color.GREEN); - Color color = i == minimumEdgeIndex ? Color.RED : POLYGON_GRAPHIC_COLOR; - meshBuilder.addLine(v0.getX(), v0.getY(), footZ, v1.getX(), v1.getY(), footZ, 0.01f, color); + FramePoint3D desiredComXYElevated = new FramePoint3D(desiredCoMXYAtFootHeight); + desiredComXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT); + meshBuilder.addLine(desiredCoMXYAtFootHeight, desiredComXYElevated, 0.005f, Color.GREEN); + } } - meshBuilder.addPolygon(transform, multiContactSupportRegion, POLYGON_GRAPHIC_COLOR); + // Update region graphic + Object ikSupportRegion = kinematicsToolboxOutputStatus.getSupportRegion(); + if (ikSupportRegion.size() >= 3) + { + this.supportRegion.clear(); + + for (int i = 0; i < ikSupportRegion.size(); i++) + { + this.supportRegion.addVertex(ikSupportRegion.get(i)); + } + + this.supportRegion.update(); - comCurrent.setToZero(centerOfMassFrame); - comCurrent.changeFrame(ReferenceFrame.getWorldFrame()); + if (this.supportRegion.getNumberOfVertices() < 3) + { + modelInstance = null; + lastModel = null; + return; + } - meshBuilder.addSphere(0.03f, comCurrent, Color.BLACK); + updateMinimumEdge(); - comXYAtFootHeight.setIncludingFrame(comCurrent); - comXYAtFootHeight.setZ(footZ); - meshBuilder.addSphere(0.03f, comXYAtFootHeight, Color.BLACK); + transform.setTranslationAndIdentityRotation(0.0, 0.0, footZ); - FramePoint3D comXYElevated = new FramePoint3D(comXYAtFootHeight); - comXYElevated.addZ(STABILITY_GRAPHIC_HEIGHT); - meshBuilder.addLine(comXYAtFootHeight, comXYElevated, 0.005f, Color.BLACK); + for (int i = 0; i < this.supportRegion.getNumberOfVertices(); i++) + { + Point2DReadOnly v0 = this.supportRegion.getVertex(i); + Point2DReadOnly v1 = this.supportRegion.getNextVertex(i); + + Color color = i == minimumEdgeIndex ? Color.RED : NOMINAL_POLYGON_GRAPHIC_COLOR; + meshBuilder.addLine(v0.getX(), v0.getY(), footZ, v1.getX(), v1.getY(), footZ, 0.01f, color); + } + + double postureSensitivityThreshold = 0.045; + double stabilityMarginThreshold = 0.12; + + boolean isPostureHighlySensitive = kinematicsToolboxOutputStatus.getSupportRegionSensitivity() > postureSensitivityThreshold; + boolean hasLowStabilityMargin = minimumEdgeDistance < stabilityMarginThreshold; + + Color polygonGraphicColor = (isPostureHighlySensitive && hasLowStabilityMargin) ? LOW_STABILITY_POLYGON_GRAPHIC_COLOR : NOMINAL_POLYGON_GRAPHIC_COLOR; + meshBuilder.addPolygon(transform, this.supportRegion, polygonGraphicColor); + } modelBuilder.begin(); Mesh mesh = meshBuilder.generateMesh(); @@ -139,10 +169,10 @@ private void updateMinimumEdge() { minimumEdgeDistance = Double.POSITIVE_INFINITY; - for (int i = 0; i < multiContactSupportRegion.getNumberOfVertices(); i++) + for (int i = 0; i < supportRegion.getNumberOfVertices(); i++) { - Point2DReadOnly v0 = multiContactSupportRegion.getVertex(i); - Point2DReadOnly v1 = multiContactSupportRegion.getNextVertex(i); + Point2DReadOnly v0 = supportRegion.getVertex(i); + Point2DReadOnly v1 = supportRegion.getNextVertex(i); double margin = EuclidGeometryTools.distanceFromPoint2DToLine2D(comXYAtFootHeight.getX(), comXYAtFootHeight.getY(), v0, v1); if (margin < minimumEdgeDistance) @@ -156,7 +186,7 @@ private void updateMinimumEdge() @Override public void getRenderables(Array renderables, Pool pool) { - if (multiContactSupportRegion.isEmpty()) + if (supportRegion.isEmpty()) { return; } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/tools/KinematicsRecordReplay.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/tools/KinematicsRecordReplay.java index 618918a944ed..f9f4a4c5d1f8 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/tools/KinematicsRecordReplay.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/tools/KinematicsRecordReplay.java @@ -3,24 +3,24 @@ import imgui.ImGui; import imgui.flag.ImGuiCol; import imgui.type.ImBoolean; -import imgui.type.ImInt; import imgui.type.ImString; -import org.lwjgl.openvr.InputDigitalActionData; import us.ihmc.behaviors.tools.TrajectoryRecordReplay; -import us.ihmc.euclid.geometry.Pose3D; -import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.log.LogTools; -import us.ihmc.perception.sceneGraph.SceneGraph; -import us.ihmc.perception.sceneGraph.SceneNode; +import us.ihmc.motionRetargeting.VRTrackedSegmentType; import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import java.io.File; import java.nio.file.Paths; -import java.util.*; +import java.util.Arrays; +import java.util.function.Consumer; /** * Class for recording or replaying motions in the UI. @@ -28,216 +28,134 @@ */ public class KinematicsRecordReplay { - private final ImInt numberOfParts = new ImInt(2); - private final TrajectoryRecordReplay trajectoryRecorder = new TrajectoryRecordReplay("", 1); - private final ImString recordPath = new ImString(Paths.get(System.getProperty("user.home"), ".ihmc/logs").toString()); - private final ImBoolean enablerRecording = new ImBoolean(false); + private final TrajectoryRecordReplay trajectoryRecorder = new TrajectoryRecordReplay(""); + private final ImString recordPath = new ImString(Paths.get(System.getProperty("user.home"), ".ihmc/logs").toString(), 100); + private final ImBoolean enableRecording = new ImBoolean(false); private boolean isRecording = false; - private final ImString replayPath = new ImString(Paths.get(System.getProperty("user.home"), ".ihmc/logs/1.csv").toString()); - private final ImBoolean enablerReplay = new ImBoolean(false); + private final ImString replayPath = new ImString(Paths.get(System.getProperty("user.home"), ".ihmc/logs/1.csv").toString(), 100); + private final ImBoolean enableReplay = new ImBoolean(false); private boolean isReplaying = false; private final ImBoolean enabledKinematicsStreaming; - private boolean isUserMoving = false; - private final List> framesToRecordHistory = new ArrayList<>(); - private int partId = 0; // identifier of current frame, used to now what body part among numberOfParts we are currently handling - private final SceneGraph sceneGraph; - private ReferenceFrame sceneNodeFrame; - private List selectableSceneNodeNames = new ArrayList<>(); - private int selectedNodeIndex = 0; - private final Map previousFramePose = new HashMap<>(); - private final Map firstFramePose = new HashMap<>(); - public KinematicsRecordReplay(SceneGraph sceneGraph, ImBoolean enabledKinematicsStreaming) + private final SideDependentList handDesiredControlFrames; + private boolean requestRecordReplay; + private Consumer replayCallback = b -> {}; + + public KinematicsRecordReplay(ImBoolean enabledKinematicsStreaming, SideDependentList handDesiredControlFrames) { - this.sceneGraph = sceneGraph; this.enabledKinematicsStreaming = enabledKinematicsStreaming; - trajectoryRecorder.setNumberOfParts(numberOfParts.get()); // default to 2 - for (int n = 0; n < numberOfParts.get(); n++) - framesToRecordHistory.add(new ArrayList<>()); + this.handDesiredControlFrames = handDesiredControlFrames; - trajectoryRecorder.setDoneReplay(true); + loadLatestReplayFile(); } - public void updateNumberOfParts() + public void loadLatestReplayFile() { - trajectoryRecorder.setNumberOfParts(numberOfParts.get()); - framesToRecordHistory.clear(); - for (int n = 0; n < numberOfParts.get(); n++) - framesToRecordHistory.add(new ArrayList<>()); + File logDirectory = new File(System.getProperty("user.home"), ".ihmc/logs"); + if (!logDirectory.exists()) + return; + + File[] files = logDirectory.listFiles(f -> f.isFile() && f.getName().endsWith(".csv")); + if (files == null || files.length == 0) + return; + + Arrays.sort(files, (f1, f2) -> Long.valueOf(f2.lastModified()).compareTo(f1.lastModified())); + replayPath.set(files[0].getAbsoluteFile()); } - public void processRecordReplayInput(InputDigitalActionData triggerButton) + public void requestRecordReplay() { - // check streaming is on, recording is on and trigger button has been pressed once. if button is pressed again recording is stopped - if (enabledKinematicsStreaming.get() && enablerRecording.get() && triggerButton.bChanged() && !triggerButton.bState()) - { - isRecording = !isRecording; - - // check if recording file path has been set to a different one from previous recording. In case update file path. - if (trajectoryRecorder.hasSavedRecording() && !(trajectoryRecorder.getPath().equals(recordPath.get()))) - trajectoryRecorder.setPath(recordPath.get()); //recorder is reset when changing path - } - // check replay is on and trigger button has been pressed once. if button is pressed again replay is stopped - if (enablerReplay.get() && triggerButton.bChanged() && !triggerButton.bState()) - { - isReplaying = !isReplaying; + /* Processed in onUpdateEnd, depending on if enableRecord or enableReplay are selected */ + LogTools.info("record/replay requested"); + requestRecordReplay = true; + } - // check if replay file has been set to a different one from previous replay. In case update file path. - if (trajectoryRecorder.hasDoneReplay() && !(trajectoryRecorder.getPath().equals(replayPath.get()))) - trajectoryRecorder.setPath(replayPath.get()); // replayer is reset when changing path - } + public void setReplayCallback(Consumer replayCallback) + { + this.replayCallback = replayCallback; } - /** - * Record frame - */ - public void framePoseToRecord(FramePose3DReadOnly framePose, String frameName) + public void onUpdateStart() { - if (isRecording) - { - if (isMoving(framePose)) //check from framePose if the user is moving - { // we want to start the recording as soon as the user starts moving, recordings with different initial pauses can lead to bad behaviors when used for learning - framePose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); - FramePose3D frameToRecord = new FramePose3D(framePose); - // transform to object reference frame if using object detection - if (sceneNodeFrame != null) - frameToRecord.changeFrame(sceneNodeFrame); + requestRecordReplay = false; - ensureOrientationContinuity(frameToRecord, frameName); - double[] dataTrajectories = new double[7]; - frameToRecord.getOrientation().get(dataTrajectories); - frameToRecord.getPosition().get(4, dataTrajectories); - trajectoryRecorder.record(dataTrajectories); + if (isReplaying) + { + isReplaying = trajectoryRecorder.onUpdateStartReplay(); + if (!isReplaying) + { // Finished replaying + LogTools.info("Finished replayed!"); + isReplaying = false; + replayCallback.accept(false); } } - else if (!(trajectoryRecorder.hasSavedRecording())) + else if (isRecording) { - trajectoryRecorder.concatenateData(); - trajectoryRecorder.saveRecording(); - if (!firstFramePose.isEmpty() && !previousFramePose.isEmpty()) - { - for (String frame : previousFramePose.keySet()) - previousFramePose.replace(frame, firstFramePose.get(frame)); - } - isUserMoving = false; + trajectoryRecorder.onUpdateStartRecord(); } } /** - * Ensure that every recorded trajectory has the same quaternion sign (-pi/2,pi/2 range) to the previous recording - * introduce a rule that allows us to likely get the same quaternion sign if we ever want to expand the dataset + * Called each tick regardless of record/replay status. */ - private void ensureOrientationContinuity(FramePose3D frameToCheck, String frameName) + public void onUpdateEnd(ReferenceFrame recordFrame) { - QuaternionReadOnly quaternionToCheck = frameToCheck.getOrientation(); - if (previousFramePose.containsKey(frameName)) - { - if (sceneNodeFrame != null) - previousFramePose.get(frameName).changeFrame(sceneNodeFrame); - - QuaternionReadOnly previousQuaternion = previousFramePose.get(frameName).getOrientation(); - // Check that quaternion is not changing 2pi range. Even if q = -q, the observed motion has to be continuous - frameToCheck.getOrientation().interpolate(previousQuaternion, quaternionToCheck, 1.0); - - previousFramePose.get(frameName).set(frameToCheck); - - if (Math.abs(frameToCheck.getOrientation().getX() - previousQuaternion.getX()) > 0.05 || - Math.abs(frameToCheck.getOrientation().getY() - previousQuaternion.getY()) > 0.05 || - Math.abs(frameToCheck.getOrientation().getZ() - previousQuaternion.getZ()) > 0.05 || - Math.abs(frameToCheck.getOrientation().getS() - previousQuaternion.getS()) > 0.05) + if (requestRecordReplay && enableRecording.get()) + { // Toggle record state + if (isRecording) { - LogTools.error("Quaternion discontinuity asymmetric wrt zero. Check recorded part was not disconnected nor occluded during recording."); - frameToCheck.getOrientation().set(previousQuaternion); + LogTools.info("Finished recording!"); + isRecording = false; + trajectoryRecorder.setPath(recordPath.get()); + trajectoryRecorder.onRecordEnd(); } - } - else - { - double x = quaternionToCheck.getX(); - double y = quaternionToCheck.getY(); - double z = quaternionToCheck.getZ(); - double s = quaternionToCheck.getS(); - - // Calculate the maximum absolute value - double max = Math.max(Math.abs(x), Math.max(Math.abs(y), Math.max(Math.abs(z), Math.abs(s)))); - - // Check if the maximum absolute value is negative - if ((Math.abs(x) == max && x < 0) || - (Math.abs(y) == max && y < 0) || - (Math.abs(z) == max && z < 0) || - (Math.abs(s) == max && s < 0)) + else { - frameToCheck.getOrientation().negate(); + LogTools.info("Starting to record!"); + isRecording = true; + trajectoryRecorder.onRecordStart(); } + } - previousFramePose.put(frameName, new FramePose3D(frameToCheck)); - firstFramePose.put(frameName, new FramePose3D(frameToCheck)); + else if (requestRecordReplay && enableReplay.get() && !isReplaying) + { // Start to replay + LogTools.info("Starting to replay!"); + isReplaying = trajectoryRecorder.onReplayStart(replayPath.get(), recordFrame); + replayCallback.accept(true); } } - /** - * Check if user is moving a certain bodyPart. Useful to start a recording only after the user started actually moving - */ - private boolean isMoving(FramePose3DReadOnly framePose) + public void recordControllerData(RobotSide robotSide, boolean aButtonPressed, boolean bButtonPressed, boolean triggerPressed, Vector3D angularVelocity, Vector3D linearVelocity, ReferenceFrame recordFrame) { - if (!isUserMoving) - { - Pose3D lastFramePose = new Pose3D(framePose); - framesToRecordHistory.get(partId).add(lastFramePose); - // check if last value of frame pose translated by 4cm with respect to first value of frame pose - if (framesToRecordHistory.get(partId).size() > 1) - { - double distance = (framesToRecordHistory.get(partId).get(framesToRecordHistory.get(partId).size() - 1)).getTranslation() - .distance(framesToRecordHistory.get(partId) - .get(0) - .getTranslation()); - isUserMoving = distance > 0.04; - } - if (!isUserMoving) // if still not moving analyze next frame at next call - { - partId++; - if (partId >= framesToRecordHistory.size()) - partId = 0; - } - } - if (isUserMoving && partId > 0) // preventing start recording while skipping the frame of other parts, wait to have all parts in the next frame - { - partId++; - if (partId >= framesToRecordHistory.size()) - partId = 0; - return false; - } - return isUserMoving && partId == 0; + if (!isRecording) + return; + + trajectoryRecorder.recordControllerData(robotSide, + aButtonPressed, + bButtonPressed, + triggerPressed, + handDesiredControlFrames.get(robotSide).getReferenceFrame(), + angularVelocity, + linearVelocity, + recordFrame); } /** * Pack frame with frame from replay */ - public void framePoseToPack(FramePose3D framePose) + public void packLoggedData(VRTrackedSegmentType segmentType, FramePose3D framePose, FrameVector3D angularVelocity, FrameVector3D linearVelocity) { - framePose.setFromReferenceFrame(ReferenceFrame.getWorldFrame()); - // Read file with stored trajectories: read set point per timestep until file is over - double[] dataPoint = trajectoryRecorder.play(true); //play split data (a body part per time) - if (sceneNodeFrame != null) - framePose.changeFrame(sceneNodeFrame); - // [0,1,2,3] quaternion of body segment; [4,5,6] position of body segment - framePose.getOrientation().set(dataPoint); - framePose.getPosition().set(4, dataPoint); - if (sceneNodeFrame != null) - framePose.changeFrame(ReferenceFrame.getWorldFrame()); - if (trajectoryRecorder.hasDoneReplay()) + if (isReplaying) { - isReplaying = false; - enablerReplay.set(false); + trajectoryRecorder.packTrackedData(segmentType, framePose, angularVelocity, linearVelocity); } } public void renderRecordWidgets(ImGuiUniqueLabelMap labels) { - if (ImGuiTools.volatileInputInt(labels.get("Number of Robot Parts to Record/Replay"), numberOfParts)) - updateNumberOfParts(); - if (ImGui.checkbox(labels.get("Record Motion"), enablerRecording)) + if (ImGui.checkbox(labels.get("Record Motion"), enableRecording)) { - setRecording(enablerRecording.get()); + // setRecording(enablerRecording.get()); } ImGui.sameLine(); ImGui.inputText(labels.get("Record Folder"), recordPath); @@ -251,9 +169,9 @@ public void renderRecordWidgets(ImGuiUniqueLabelMap labels) public void renderReplayWidgets(ImGuiUniqueLabelMap labels) { - if (ImGui.checkbox(labels.get("Replay Motion"), enablerReplay)) + if (ImGui.checkbox(labels.get("Replay Motion"), enableReplay)) { - setReplay(enablerReplay.get()); + setReplay(enableReplay.get()); } ImGui.sameLine(); ImGui.inputText(labels.get("Replay File"), replayPath); @@ -265,77 +183,35 @@ public void renderReplayWidgets(ImGuiUniqueLabelMap labels) } } - public void renderReferenceFrameSelection(ImGuiUniqueLabelMap labels) + private void setRecording(boolean enableRecording) { - if (sceneGraph.getNodeNameList().size() > 0) + if (enableRecording != this.enableRecording.get()) + this.enableRecording.set(enableRecording); + if (enableRecording) { - if (sceneGraph.getNodeNameList().size() != selectableSceneNodeNames.size()) - selectableSceneNodeNames = sceneGraph.getNodeNameList(); - if(sceneNodeFrame == null) - selectedNodeIndex = 0; - - ImGui.text("Select Record/Replay Reference Frame"); - if (ImGui.beginCombo(labels.get("Reference Frame"), selectedNodeIndex != 0 ? selectableSceneNodeNames.get(selectedNodeIndex) : "World")) - { - for (int i = 0; i < selectableSceneNodeNames.size(); i++) - { - // use world frame instead of root node - String selectableSceneNodeName; - if (i != 0) - selectableSceneNodeName = selectableSceneNodeNames.get(i); - else - selectableSceneNodeName = "World"; - - if (ImGui.selectable(selectableSceneNodeName, selectedNodeIndex == i)) - { - selectedNodeIndex = i; - - SceneNode selectedNode = sceneGraph.getNamesToNodesMap().get(selectableSceneNodeNames.get(selectedNodeIndex)); - if (selectedNode.getID() != 0) - sceneNodeFrame = selectedNode.getNodeFrame(); - else - sceneNodeFrame = null; - } - } - ImGui.endCombo(); - } - } - } - - private void setRecording(boolean enablerRecording) - { - if (enablerRecording != this.enablerRecording.get()) - this.enablerRecording.set(enablerRecording); - if (enablerRecording) - { - this.enablerReplay.set(false); // check no concurrency replay and record - } - else - { - firstFramePose.clear(); - previousFramePose.clear(); + this.enableReplay.set(false); // check no concurrency replay and record } } public void setReplay(boolean enablerReplay) { - if (enablerReplay != this.enablerReplay.get()) - this.enablerReplay.set(enablerReplay); + if (enablerReplay != this.enableReplay.get()) + this.enableReplay.set(enablerReplay); if (enablerReplay) { - if (enablerRecording.get() || enabledKinematicsStreaming.get()) - this.enablerReplay.set(false); // check no concurrency replay and record/streaming + if (enableRecording.get() || enabledKinematicsStreaming.get()) + this.enableReplay.set(false); // check no concurrency replay and record/streaming } } public ImBoolean isRecordingEnabled() { - return enablerRecording; + return enableRecording; } public ImBoolean isReplayingEnabled() { - return enablerReplay; + return enableReplay; } public boolean isRecording() @@ -347,4 +223,24 @@ public boolean isReplaying() { return isReplaying; } + + public boolean getAButtonPressed(RobotSide robotSide) + { + return trajectoryRecorder.getAButtonPressed(robotSide); + } + + public boolean getBButtonPressed(RobotSide robotSide) + { + return trajectoryRecorder.getBButtonPressed(robotSide); + } + + public boolean getTriggerPressed(RobotSide robotSide) + { + return trajectoryRecorder.getTriggerPressed(robotSide); + } + + public String getReplayPath() + { + return replayPath.get(); + } } \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java index 1ef2012adf82..ea42a62d46ca 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRKinematicsStreamingMode.java @@ -5,6 +5,8 @@ import com.badlogic.gdx.utils.Pool; import controller_msgs.msg.dds.CapturabilityBasedStatus; import controller_msgs.msg.dds.HandLoadBearingMessage; +import ihmc_common_msgs.msg.dds.SelectionMatrix3DMessage; +import ihmc_common_msgs.msg.dds.WeightMatrix3DMessage; import imgui.ImGui; import imgui.type.ImBoolean; import org.apache.commons.lang.mutable.MutableBoolean; @@ -37,6 +39,7 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.KinematicsToolboxMessageFactory; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HandConfiguration; @@ -68,7 +71,6 @@ import us.ihmc.robotics.referenceFrames.ReferenceFrameMissingTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.robotics.weightMatrices.WeightMatrix3D; import us.ihmc.ros2.ROS2Input; import us.ihmc.scs2.definition.robot.RobotDefinition; import us.ihmc.scs2.definition.visual.ColorDefinitions; @@ -82,6 +84,7 @@ import java.util.List; import java.util.Map; import java.util.Set; +import java.util.concurrent.atomic.AtomicBoolean; import static us.ihmc.communication.packets.MessageTools.toFrameId; import static us.ihmc.motionRetargeting.VRTrackedSegmentType.*; @@ -89,6 +92,8 @@ public class RDXVRKinematicsStreamingMode { public static final double FRAME_AXIS_GRAPHICS_LENGTH = 0.2; + private static final int NUM_TRACKERS = 1; + private final ROS2SyncedRobotModel syncedRobot; private final ROS2ControllerHelper ros2ControllerHelper; private final RetargetingParameters retargetingParameters; @@ -102,7 +107,6 @@ public class RDXVRKinematicsStreamingMode private ROS2Input capturabilityBasedStatus; private final double streamPeriod = UnitConversions.hertzToSeconds(120.0); private final Throttler toolboxInputStreamRateLimiter = new Throttler(); - private final FramePose3D tempFramePose = new FramePose3D(); private final ImGuiFrequencyPlot statusFrequencyPlot = new ImGuiFrequencyPlot(); private final ImGuiFrequencyPlot outputFrequencyPlot = new ImGuiFrequencyPlot(); private final SideDependentList handDesiredControlFrames = new SideDependentList<>(); @@ -116,6 +120,7 @@ public class RDXVRKinematicsStreamingMode private MutableReferenceFrame headsetReferenceFrame; private final ImBoolean showReferenceFrameGraphics = new ImBoolean(false); private final ImBoolean streamToController = new ImBoolean(false); + private final AtomicBoolean requestRecordReplay = new AtomicBoolean(false); private final Notification streamingDisabled = new Notification(); private final Throttler messageThrottler = new Throttler(); private KinematicsRecordReplay kinematicsRecorder; @@ -153,6 +158,11 @@ public class RDXVRKinematicsStreamingMode private final SideDependentList handsAreLoaded = new SideDependentList<>(false, false); private final SideDependentList handControlModes = new SideDependentList<>(RDXHandControlMode.GRIPPER, RDXHandControlMode.GRIPPER); + private final FramePose3D tempFramePose = new FramePose3D(); + private final FrameVector3D tempFrameVector0 = new FrameVector3D(); + private final FrameVector3D tempFrameVector1 = new FrameVector3D(); + private static final Vector3D zeroVector = new Vector3D(); + public RDXVRKinematicsStreamingMode(ROS2SyncedRobotModel syncedRobot, ROS2ControllerHelper ros2ControllerHelper, RDXVRContext vrContext, @@ -212,7 +222,7 @@ public void create(boolean createToolbox) status = ros2ControllerHelper.subscribe(KinematicsStreamingToolboxModule.getOutputStatusTopic(syncedRobot.getRobotModel().getSimpleRobotName())); capturabilityBasedStatus = ros2ControllerHelper.subscribeToController(CapturabilityBasedStatus.class); - kinematicsRecorder = new KinematicsRecordReplay(sceneGraph, enabled); + kinematicsRecorder = new KinematicsRecordReplay(enabled, handDesiredControlFrames); motionRetargeting = new RDXVRMotionRetargeting(syncedRobot, handDesiredControlFrames, trackerReferenceFrames, headsetReferenceFrame, retargetingParameters); prescientFootstepStreaming = new RDXVRPrescientFootstepStreaming(syncedRobot, footstepPlacer); @@ -327,90 +337,82 @@ private void reduceElbowJointLimits(KinematicsStreamingToolboxParameters paramet } parameters.setJointCustomPositionUpperLimits(jointUpperLimits); parameters.setJointCustomPositionLowerLimits(jointLowerLimits); -} + } public void processVRInput() { - vrContext.getController(RobotSide.LEFT).runIfConnected(controller -> - { - InputDigitalActionData aButton = controller.getAButtonActionData(); - if (aButton.bChanged() && !aButton.bState()) - { - streamToController.set(!streamToController.get()); - if (!streamToController.get()) - streamingDisabled.set(); - } - - // NOTE: Implement hand open close for controller trigger button. - InputDigitalActionData clickTriggerButton = controller.getClickTriggerActionData(); - if (clickTriggerButton.bChanged() && !clickTriggerButton.bState()) - { - performHandAction(RobotSide.LEFT); - } + if (!toolboxInputStreamRateLimiter.run(streamPeriod)) + return; - // Check if left joystick is pressed in order to trigger recording or replay of motion - InputDigitalActionData leftJoystickButton = controller.getJoystickPressActionData(); - gripButtonsValue.put(RobotSide.LEFT, controller.getGripActionData().x()); + kinematicsRecorder.onUpdateStart(); - kinematicsRecorder.processRecordReplayInput(leftJoystickButton); - if (kinematicsRecorder.isReplayingEnabled().get()) - wakeUpToolbox(); + // Handle left joystick input + if (kinematicsRecorder.isReplaying()) + { + boolean leftAButtonPressed = kinematicsRecorder.getAButtonPressed(RobotSide.LEFT); + boolean leftBButtonPressed = kinematicsRecorder.getBButtonPressed(RobotSide.LEFT); + boolean leftTriggerPressed = kinematicsRecorder.getTriggerPressed(RobotSide.LEFT); + handleLeftControllerJoystickInput(leftAButtonPressed, leftBButtonPressed, leftTriggerPressed, false); + } + else + { + vrContext.getController(RobotSide.LEFT).runIfConnected(controller -> + { + InputDigitalActionData aButton = controller.getAButtonActionData(); + InputDigitalActionData bButton = controller.getBButtonActionData(); + InputDigitalActionData clickTriggerButton = controller.getClickTriggerActionData(); + InputDigitalActionData leftJoystickButton = controller.getJoystickPressActionData(); + boolean leftJoystickButtonClicked = leftJoystickButton.bChanged() && !leftJoystickButton.bState(); + + boolean leftAButtonPressed = aButton.bChanged() && !aButton.bState(); + boolean leftBButtonPressed = bButton.bChanged() && !bButton.bState(); + boolean leftTriggerPressed = clickTriggerButton.bChanged() && !clickTriggerButton.bState(); + handleLeftControllerJoystickInput(leftAButtonPressed, leftBButtonPressed, leftTriggerPressed, leftJoystickButtonClicked); + + // Check if left joystick is pressed in order to trigger recording or replay of motion + gripButtonsValue.put(RobotSide.LEFT, controller.getGripActionData().x()); + kinematicsRecorder.recordControllerData(RobotSide.LEFT, leftAButtonPressed, leftBButtonPressed, leftTriggerPressed, controller.getAngularVelocity(), controller.getLinearVelocity(), getTrajectoryRecordFrame()); + }); + } - if (leftJoystickButton.bChanged() && !leftJoystickButton.bState() && - !kinematicsRecorder.isReplayingEnabled().get() && !kinematicsRecorder.isRecordingEnabled().get()) - { // reinitialize toolbox - LogTools.warn("Reinitializing toolbox. Forcing initial lower-body IK configuration to current robot configuration"); - if (enabled.get()) - { - reinitializeToolboxRobotConfiguration(); - } - } - }); + if (requestRecordReplay.getAndSet(false)) + { + kinematicsRecorder.requestRecordReplay(); + } - vrContext.getController(RobotSide.RIGHT).runIfConnected(controller -> + // Handle right joystick input + if (kinematicsRecorder.isReplaying()) + { + boolean rightAButtonPressed = kinematicsRecorder.getAButtonPressed(RobotSide.RIGHT); + boolean rightBButtonPressed = kinematicsRecorder.getBButtonPressed(RobotSide.RIGHT); + boolean rightTriggerPressed = kinematicsRecorder.getTriggerPressed(RobotSide.RIGHT); + handleRightControllerJoystickInput(rightAButtonPressed, rightBButtonPressed, rightTriggerPressed); + } + else { - InputDigitalActionData aButton = controller.getAButtonActionData(); - if (aButton.bChanged() && !aButton.bState()) - { - setEnabled(!enabled.get()); - } - - // NOTE: Implement hand open close for controller trigger button. - InputDigitalActionData clickTriggerButton = controller.getClickTriggerActionData(); - if (clickTriggerButton.bChanged() && !clickTriggerButton.bState()) - { // do not want to close grippers while interacting with the panel - performHandAction(RobotSide.RIGHT); - - // TODO discuss and possibly remap to different button... - - // double trajectoryTime = 1.5; - // GoHomeMessage homePelvis = new GoHomeMessage(); - // homePelvis.setHumanoidBodyPart(GoHomeMessage.HUMANOID_BODY_PART_PELVIS); - // homePelvis.setTrajectoryTime(trajectoryTime); - // ros2ControllerHelper.publishToController(homePelvis); - // - // GoHomeMessage homeChest = new GoHomeMessage(); - // homeChest.setHumanoidBodyPart(GoHomeMessage.HUMANOID_BODY_PART_CHEST); - // homeChest.setTrajectoryTime(trajectoryTime); - // - // RDXBaseUI.pushNotification("Commanding home pose..."); - // ros2ControllerHelper.publishToController(homeChest); - // - // prescientFootstepStreaming.reset(); - // pausedForWalking = false; - // reintializingToolbox = false; - } - - gripButtonsValue.put(RobotSide.RIGHT, controller.getGripActionData().x()); - }); - - if ((enabled.get() || kinematicsRecorder.isReplaying()) && toolboxInputStreamRateLimiter.run(streamPeriod)) + vrContext.getController(RobotSide.RIGHT).runIfConnected(controller -> + { + InputDigitalActionData aButton = controller.getAButtonActionData(); + InputDigitalActionData bButton = controller.getBButtonActionData(); + InputDigitalActionData clickTriggerButton = controller.getClickTriggerActionData(); + + boolean rightAButtonPressed = aButton.bChanged() && !aButton.bState(); + boolean rightBButtonPressed = bButton.bChanged() && !bButton.bState(); + boolean rightTriggerPressed = clickTriggerButton.bChanged() && !clickTriggerButton.bState(); + handleRightControllerJoystickInput(rightAButtonPressed, rightBButtonPressed, rightTriggerPressed); + + gripButtonsValue.put(RobotSide.RIGHT, controller.getGripActionData().x()); + kinematicsRecorder.recordControllerData(RobotSide.RIGHT, rightAButtonPressed, rightBButtonPressed, rightTriggerPressed, controller.getAngularVelocity(), controller.getLinearVelocity(), getTrajectoryRecordFrame()); + }); + } + + if (enabled.get()) { KinematicsStreamingToolboxInputMessage toolboxInputMessage = new KinematicsStreamingToolboxInputMessage(); // ---------- VR Trackers ------------ additionalTrackedSegments = vrContext.getAssignedTrackerRoles(); - for (VRTrackedSegmentType segmentType : VRTrackedSegmentType.getTrackerTypes()) + for (VRTrackedSegmentType segmentType : VRTrackedSegmentType.TRACKER_TYPES) { if (additionalTrackedSegments.contains(segmentType.getSegmentName()) && !controlArmsOnly.get()) { @@ -435,17 +437,16 @@ public void processVRInput() RigidBodyBasics controlledSegment = getControlledSegment(segmentType); if (controlledSegment != null) { - KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(controlledSegment, + KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(segmentType, + controlledSegment, trackerReferenceFrames.get(segmentType.getSegmentName()).getReferenceFrame(), - segmentType.getSegmentName(), + tracker.getAngularVelocity(), + tracker.getLinearVelocity(), retargetingParameters.getPositionWeight(segmentType), retargetingParameters.getOrientationWeight(segmentType), retargetingParameters.getLinearRateLimitation(segmentType), retargetingParameters.getAngularRateLimitation(segmentType)); - message.setHasDesiredLinearVelocity(true); - message.getDesiredLinearVelocityInWorld().set(tracker.getLinearVelocity()); - message.setHasDesiredAngularVelocity(true); - message.getDesiredAngularVelocityInWorld().set(tracker.getAngularVelocity()); + toolboxInputMessage.getInputs().add().set(message); // TODO. figure out how we can set this correctly after retargeting computation, or if we even need it //toolboxInputMessage.setTimestamp(tracker.getLastPollTimeNanos()); @@ -461,42 +462,61 @@ public void processVRInput() // ---------- end VR Trackers ------------ // ---------- VR Controllers ------------ - for (VRTrackedSegmentType segmentType : VRTrackedSegmentType.getControllerTypes()) + for (VRTrackedSegmentType segmentType : VRTrackedSegmentType.CONTROLLER_TYPES) { - vrContext.getController(segmentType.getSegmentSide()).runIfConnected(controller -> - { - MovingReferenceFrame endEffectorFrame = ghostFullRobotModel.getEndEffectorFrame(segmentType.getSegmentSide(), LimbName.ARM); - if (endEffectorFrame == null) - return; - controller.getXForwardZUpControllerFrame().update(); - controllerFrameGraphics.get(segmentType.getSegmentSide()) - .setToReferenceFrame(controller.getXForwardZUpControllerFrame()); - handFrameGraphics.get(segmentType.getSegmentSide()).setToReferenceFrame(endEffectorFrame); - if (!armScaling.get()) - { - KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(ghostFullRobotModel.getHand( - segmentType.getSegmentSide()), - handDesiredControlFrames.get( - segmentType.getSegmentSide()).getReferenceFrame(), - segmentType.getSegmentName(), - retargetingParameters.getPositionWeight(segmentType), - retargetingParameters.getOrientationWeight(segmentType), - retargetingParameters.getLinearRateLimitation(segmentType), - retargetingParameters.getAngularRateLimitation(segmentType)); - message.getControlFramePositionInEndEffector().set(ikControlFramePoses.get(segmentType.getSegmentSide()).getPosition()); - message.getControlFrameOrientationInEndEffector().set(ikControlFramePoses.get(segmentType.getSegmentSide()).getOrientation()); + boolean handIsLoaded = (segmentType == LEFT_HAND && handsAreLoaded.get(RobotSide.LEFT)) || (segmentType == RIGHT_HAND && handsAreLoaded.get(RobotSide.RIGHT)); + Vector3D positionWeight = handIsLoaded ? zeroVector : retargetingParameters.getPositionWeight(segmentType); - message.setHasDesiredLinearVelocity(true); - message.getDesiredLinearVelocityInWorld().set(controller.getLinearVelocity()); - message.setHasDesiredAngularVelocity(true); - message.getDesiredAngularVelocityInWorld().set(controller.getAngularVelocity()); + if (kinematicsRecorder.isReplaying()) + { + KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(segmentType, + ghostFullRobotModel.getHand(segmentType.getSegmentSide()), + null, + null, + null, + positionWeight, + retargetingParameters.getOrientationWeight(segmentType), + retargetingParameters.getLinearRateLimitation(segmentType), + retargetingParameters.getAngularRateLimitation(segmentType)); - toolboxInputMessage.getInputs().add().set(message); - toolboxInputMessage.setTimestamp(controller.getLastPollTimeNanos()); - } - else - controllerLastPollTimeNanos = controller.getLastPollTimeNanos(); - }); + message.getControlFramePositionInEndEffector().set(ikControlFramePoses.get(segmentType.getSegmentSide()).getPosition()); + message.getControlFrameOrientationInEndEffector().set(ikControlFramePoses.get(segmentType.getSegmentSide()).getOrientation()); + toolboxInputMessage.getInputs().add().set(message); + toolboxInputMessage.setTimestamp(System.nanoTime()); + } + else + { + vrContext.getController(segmentType.getSegmentSide()).runIfConnected(controller -> + { + MovingReferenceFrame endEffectorFrame = ghostFullRobotModel.getEndEffectorFrame(segmentType.getSegmentSide(), LimbName.ARM); + if (endEffectorFrame == null) + return; + + controller.getXForwardZUpControllerFrame().update(); + controllerFrameGraphics.get(segmentType.getSegmentSide()).setToReferenceFrame(controller.getXForwardZUpControllerFrame()); + handFrameGraphics.get(segmentType.getSegmentSide()).setToReferenceFrame(endEffectorFrame); + if (!armScaling.get()) + { + KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage( + segmentType, + ghostFullRobotModel.getHand(segmentType.getSegmentSide()), + handDesiredControlFrames.get(segmentType.getSegmentSide()).getReferenceFrame(), + controller.getAngularVelocity(), + controller.getLinearVelocity(), + positionWeight, + retargetingParameters.getOrientationWeight(segmentType), + retargetingParameters.getLinearRateLimitation(segmentType), + retargetingParameters.getAngularRateLimitation(segmentType)); + + message.getControlFramePositionInEndEffector().set(ikControlFramePoses.get(segmentType.getSegmentSide()).getPosition()); + message.getControlFrameOrientationInEndEffector().set(ikControlFramePoses.get(segmentType.getSegmentSide()).getOrientation()); + toolboxInputMessage.getInputs().add().set(message); + toolboxInputMessage.setTimestamp(controller.getLastPollTimeNanos()); + } + else + controllerLastPollTimeNanos = controller.getLastPollTimeNanos(); + }); + } } // ---------- end VR Controllers ------------ @@ -504,6 +524,7 @@ public void processVRInput() { // Update headset pose, used for retargeting to estimate shoulder position vrContext.getHeadset().runIfConnected(headset -> headset.getXForwardZUpHeadsetFrame().update()); } + // Correct values from trackers/controllers using retargeting techniques motionRetargeting.computeDesiredValues(); for (VRTrackedSegmentType segmentType : motionRetargeting.getRetargetedSegments()) @@ -511,9 +532,11 @@ public void processVRInput() RigidBodyBasics controlledSegment = getControlledSegment(segmentType); if (controlledSegment != null) { - KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(controlledSegment, + KinematicsToolboxRigidBodyMessage message = createRigidBodyMessage(segmentType, + controlledSegment, motionRetargeting.getDesiredFrame(segmentType), - segmentType.getSegmentName(), + null, + null, retargetingParameters.getPositionWeight(segmentType), retargetingParameters.getOrientationWeight(segmentType), retargetingParameters.getLinearRateLimitation(segmentType), @@ -601,10 +624,87 @@ else if (!additionalTrackedSegments.contains(WAIST.getSegmentName()) && addition toolboxInputMessage.setStreamToController(streamToController.get()); else toolboxInputMessage.setStreamToController(kinematicsRecorder.isReplaying()); -// ros2ControllerHelper.publish(KinematicsStreamingToolboxModule.getInputToolboxConfigurationTopic(syncedRobot.getRobotModel().getSimpleRobotName()), ikSolverConfigurationMessage); + // ros2ControllerHelper.publish(KinematicsStreamingToolboxModule.getInputToolboxConfigurationTopic(syncedRobot.getRobotModel().getSimpleRobotName()), ikSolverConfigurationMessage); ros2ControllerHelper.publish(KinematicsStreamingToolboxModule.getInputCommandTopic(syncedRobot.getRobotModel().getSimpleRobotName()), toolboxInputMessage); outputFrequencyPlot.recordEvent(); } + + kinematicsRecorder.onUpdateEnd(getTrajectoryRecordFrame()); + } + + private ReferenceFrame getTrajectoryRecordFrame() + { + return syncedRobot.getReferenceFrames().getMidFeetZUpFrame(); + } + + private void handleLeftControllerJoystickInput(boolean leftAButtonPressed, boolean leftBButtonPressed, boolean leftTriggerPressed, boolean leftJoystickButtonClicked) + { + if (leftAButtonPressed) + { + streamToController.set(!streamToController.get()); + if (!streamToController.get()) + streamingDisabled.set(); + } + + if (leftTriggerPressed) + { + performHandAction(RobotSide.LEFT); + } + + if (leftJoystickButtonClicked) + { + kinematicsRecorder.requestRecordReplay(); + } + + boolean isReplaying = kinematicsRecorder.isReplayingEnabled().get(); + boolean isRecording = kinematicsRecorder.isRecordingEnabled().get(); + + if (leftJoystickButtonClicked && !isReplaying && !isRecording) + { // reinitialize toolbox + LogTools.warn("Reinitializing toolbox. Forcing initial lower-body IK configuration to current robot configuration"); + if (enabled.get()) + { + sleepToolbox(); + + // Update initial configuration of KST + KinematicsToolboxInitialConfigurationMessage initialConfigMessage = KinematicsToolboxMessageFactory.initialConfigurationFromFullRobotModel(syncedRobot.getFullRobotModel()); + List oneDoFJoints = Arrays.asList(syncedRobot.getFullRobotModel().getOneDoFJoints()); + for (RobotSide robotSide : RobotSide.values) + { + int shyIndex = oneDoFJoints.indexOf(syncedRobot.getFullRobotModel().getArmJoint(robotSide, ArmJointName.SHOULDER_PITCH)); + int shxIndex = oneDoFJoints.indexOf(syncedRobot.getFullRobotModel().getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL)); + int shzIndex = oneDoFJoints.indexOf(syncedRobot.getFullRobotModel().getArmJoint(robotSide, ArmJointName.SHOULDER_YAW)); + int elyIndex = oneDoFJoints.indexOf(syncedRobot.getFullRobotModel().getArmJoint(robotSide, ArmJointName.ELBOW_PITCH)); + + // TODO extract this initial configuration in robot specific class + initialConfigMessage.getInitialJointAngles().set(shyIndex, -0.5f); + initialConfigMessage.getInitialJointAngles().set(shxIndex, robotSide.negateIfRightSide(-0.3f)); + initialConfigMessage.getInitialJointAngles().set(shzIndex, robotSide.negateIfRightSide(-0.5f)); + initialConfigMessage.getInitialJointAngles().set(elyIndex, -2.2f); + // TODO add also default for wrist joints if they exist + } + + ros2ControllerHelper.publish(KinematicsStreamingToolboxModule.getInputStreamingInitialConfigurationTopic(syncedRobot.getRobotModel() + .getSimpleRobotName()), initialConfigMessage); + wakeUpToolbox(); + reinitializeToolbox(); + wakeUpToolbox(); + } + } + } + + private void handleRightControllerJoystickInput(boolean rightAButtonPressed, boolean rightBButtonPressed, boolean rightTriggerPressed) + { + if (rightAButtonPressed) + { + LogTools.info("Right A Pressed - Enabling"); + setEnabled(!enabled.get()); + } + + if (rightTriggerPressed) + { // do not want to close grippers while interacting with the panel + performHandAction(RobotSide.RIGHT); + } } // TODO. There is a KTConfigurationMessage to lock chest/pelvis. Use that message, and probably tune the weight of that message. This is equivalent @@ -661,9 +761,11 @@ private RigidBodyBasics getControlledSegment(VRTrackedSegmentType segmentType) }; } - private KinematicsToolboxRigidBodyMessage createRigidBodyMessage(RigidBodyBasics segment, + private KinematicsToolboxRigidBodyMessage createRigidBodyMessage(VRTrackedSegmentType segmentType, + RigidBodyBasics segment, ReferenceFrame desiredControlFrame, - String frameName, + Vector3DReadOnly angularVelocity, + Vector3DReadOnly linearVelocity, Vector3D positionWeight, Vector3D orientationWeight, double linearMomentumLimit, @@ -672,34 +774,43 @@ private KinematicsToolboxRigidBodyMessage createRigidBodyMessage(RigidBodyBasics KinematicsToolboxRigidBodyMessage message = new KinematicsToolboxRigidBodyMessage(); message.setEndEffectorHashCode(segment.hashCode()); - tempFramePose.setToZero(desiredControlFrame); - tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + boolean hasDesiredAngularVelocity = false; + boolean hasDesiredLinearVelocity = false; + + if (desiredControlFrame != null) + { + tempFramePose.setToZero(desiredControlFrame); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + } + + if (angularVelocity != null) + { + tempFrameVector0.set(ReferenceFrame.getWorldFrame(), angularVelocity); + hasDesiredAngularVelocity = true; + } + if (linearVelocity != null) + { + tempFrameVector1.set(ReferenceFrame.getWorldFrame(), linearVelocity); + hasDesiredLinearVelocity = true; + } - // Record motion if in recording mode - kinematicsRecorder.framePoseToRecord(tempFramePose, frameName); if (kinematicsRecorder.isReplaying()) - kinematicsRecorder.framePoseToPack(tempFramePose); //get values of tempFramePose from replay + { + kinematicsRecorder.packLoggedData(segmentType, tempFramePose, tempFrameVector0, tempFrameVector1); + hasDesiredAngularVelocity = true; + hasDesiredLinearVelocity = true; + } + + message.setHasDesiredLinearVelocity(hasDesiredAngularVelocity); + message.setHasDesiredAngularVelocity(hasDesiredLinearVelocity); message.getDesiredOrientationInWorld().set(tempFramePose.getOrientation()); message.getDesiredPositionInWorld().set(tempFramePose.getPosition()); + message.getDesiredAngularVelocityInWorld().set(tempFrameVector0); + message.getDesiredLinearVelocityInWorld().set(tempFrameVector1); - WeightMatrix3D linearWeightMatrix = new WeightMatrix3D(); - message.getLinearSelectionMatrix().setXSelected(positionWeight.getX() != 0.0); - message.getLinearSelectionMatrix().setYSelected(positionWeight.getY() != 0.0); - linearWeightMatrix.setXAxisWeight(positionWeight.getX()); - linearWeightMatrix.setYAxisWeight(positionWeight.getY()); - message.getLinearSelectionMatrix().setZSelected(positionWeight.getZ() != 0.0); - linearWeightMatrix.setZAxisWeight(positionWeight.getZ()); - message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(linearWeightMatrix)); - - WeightMatrix3D angularWeightMatrix = new WeightMatrix3D(); - message.getAngularSelectionMatrix().setXSelected(orientationWeight.getX() != 0.0); - angularWeightMatrix.setXAxisWeight(orientationWeight.getX()); - message.getAngularSelectionMatrix().setYSelected(orientationWeight.getY() != 0.0); - angularWeightMatrix.setYAxisWeight(orientationWeight.getY()); - message.getAngularSelectionMatrix().setZSelected(orientationWeight.getZ() != 0.0); - angularWeightMatrix.setZAxisWeight(orientationWeight.getZ()); - message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(angularWeightMatrix)); + configureWeightAndSelectionMatrices(positionWeight, message.getLinearSelectionMatrix(), message.getLinearWeightMatrix()); + configureWeightAndSelectionMatrices(orientationWeight, message.getAngularSelectionMatrix(), message.getAngularWeightMatrix()); message.setLinearRateLimitation(linearMomentumLimit); message.setAngularRateLimitation(angularMomentumLimit); @@ -707,6 +818,16 @@ private KinematicsToolboxRigidBodyMessage createRigidBodyMessage(RigidBodyBasics return message; } + private static void configureWeightAndSelectionMatrices(Vector3D weightVector, SelectionMatrix3DMessage selectionMatrixMessage, WeightMatrix3DMessage weightMatrixMessage) + { + selectionMatrixMessage.setXSelected(weightVector.getX() != 0.0); + selectionMatrixMessage.setYSelected(weightVector.getY() != 0.0); + selectionMatrixMessage.setZSelected(weightVector.getZ() != 0.0); + weightMatrixMessage.setXWeight(weightVector.getX()); + weightMatrixMessage.setYWeight(weightVector.getY()); + weightMatrixMessage.setZWeight(weightVector.getZ()); + } + public void update(boolean ikStreamingModeEnabled) { // Safety features! @@ -745,7 +866,7 @@ else if (latestStatus.getCurrentToolboxState() == KinematicsToolboxOutputStatus. ghostOneDoFJointsExcludingHands[i].setQ(latestStatus.getDesiredJointAngles().get(i)); } ghostFullRobotModel.getElevator().updateFramesRecursively(); - multiContactStabilityGraphic.update(latestStatus); + multiContactStabilityGraphic.update(latestStatus, null); } } if (capturabilityBasedStatus.getMessageNotification().poll()) @@ -799,6 +920,14 @@ public void renderImGuiWidgets() { setEnabled(false); } + if (ImGui.button(labels.get("Start record/replay"))) + { + requestRecordReplay.set(true); + } + if (ImGui.button(labels.get("Load latest csv"))) + { + kinematicsRecorder.loadLatestReplayFile(); + } Set connectedTrackers = vrContext.getAssignedTrackerRoles(); if (connectedTrackers.contains(CHEST.getSegmentName())) @@ -833,7 +962,6 @@ else if (comTracking.get()) kinematicsRecorder.renderRecordWidgets(labels); ImGui.text("Press Left Joystick - Start/Stop replay"); kinematicsRecorder.renderReplayWidgets(labels); - kinematicsRecorder.renderReferenceFrameSelection(labels); ImGui.text("Output:"); ImGui.sameLine(); outputFrequencyPlot.renderImGuiWidgets(); @@ -844,12 +972,14 @@ else if (comTracking.get()) ImGui.checkbox(labels.get("Show reference frames"), showReferenceFrameGraphics); } - public void setEnabled(boolean enabled) + public void setEnabled(boolean enable) { - if (enabled) + if (enable) { if (!this.enabled.get()) + { wakeUpToolbox(); + } else { reinitializeToolboxRobotConfiguration(); @@ -877,8 +1007,8 @@ public void setEnabled(boolean enabled) streamToController.set(false); } - if (enabled != this.enabled.get()) - this.enabled.set(enabled); + if (enable != this.enabled.get()) + this.enabled.set(enable); } private void reinitializeToolboxRobotConfiguration() @@ -909,7 +1039,7 @@ private void reinitializeToolboxRobotConfiguration() } ros2ControllerHelper.publish(KinematicsStreamingToolboxModule.getInputStreamingInitialConfigurationTopic(syncedRobot.getRobotModel() .getSimpleRobotName()), - initialConfigMessage); + initialConfigMessage); wakeUpToolbox(); reinitializeToolbox(); wakeUpToolbox(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/TrajectoryRecordReplay.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/TrajectoryRecordReplay.java index d1f558beddb0..59a5840521d8 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/TrajectoryRecordReplay.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/tools/TrajectoryRecordReplay.java @@ -1,276 +1,470 @@ package us.ihmc.behaviors.tools; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.log.LogTools; +import us.ihmc.motionRetargeting.VRTrackedSegmentType; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; + import java.io.*; import java.text.SimpleDateFormat; import java.util.ArrayList; import java.util.Date; +import java.util.EnumMap; +import java.util.HashMap; import java.util.List; +import java.util.function.IntFunction; +import java.util.function.IntPredicate; /** - * Class to record and replay multidimensional trajectories. - * The multi-dimension represents the number of elements that are considered. - * For each dimension, at each time step the class stores or reads a set point. - * The collection of set points defines the trajectory. - * The trajectories are saved and loaded from .csv files. - * Each column represents the trajectory of a distinct element (e.g., X position of the right hand, or Y position of the right hand, etc...). + * Class to record and replay raw VR joystick input. */ public class TrajectoryRecordReplay { private String filePath; - private int numberOfParts; // specify the number of parts you want to record (e.g., left hand, right hand, chest) - private final List dataMatrix = new ArrayList<>(); - private final List concatenatedDataMatrix = new ArrayList<>(); - private final List splitDataMatrix = new ArrayList<>(); - private int timeStepReplay = 0; - private boolean savedRecording = true; - private boolean doneReplaying = false; - private boolean concatenated = false; private String recordFileName = ""; - public TrajectoryRecordReplay(String filePath, int numberParts) + private final List vrInputData = new ArrayList<>(); + private int replayIndex = 0; + private double replayInterpolationAlpha = -1.0; + + private long replayStartTimeMillis; + private int lastReplayIndex = -1; + + public static final int numberOfTrackers = VRTrackedSegmentType.TRACKER_TYPES.length; + public static final HashMap trackerIndex = new HashMap<>(); + + static { - this.filePath = filePath; - this.numberOfParts = numberParts; + for (int i = 0; i < numberOfTrackers; i++) + { + trackerIndex.put(VRTrackedSegmentType.TRACKER_TYPES[i], i); + } } - public double[] play() + public TrajectoryRecordReplay(String filePath) { - return this.play(false); + this.filePath = filePath; } - public double[] play(boolean split) + /** + * Updates replay index and returns whether it is still replaying + */ + public boolean onUpdateStartReplay() { - if (timeStepReplay < 1) - { - this.readCSV(); - if (split) - this.splitData(); - } - double[] values; - int size; - if (split) - { - // read split data (a row for each body part) - values = splitDataMatrix.get(timeStepReplay); - size = splitDataMatrix.size(); + if (replayIndex == -1) + { // First replay tick, initialize + replayIndex = 0; + lastReplayIndex = -1; + replayStartTimeMillis = System.currentTimeMillis(); + LogTools.info("Replay started"); + return true; } else - { - // read default data as they are stored in the csv file - values = dataMatrix.get(timeStepReplay); - size = dataMatrix.size(); + { // Replaying, search for next index + long timeInTrajectory = System.currentTimeMillis() - replayStartTimeMillis; + lastReplayIndex = replayIndex; + replayIndex = findCurrentIndex(timeInTrajectory); + boolean isStillReplaying = replayIndex < vrInputData.size() - 1; + if (!isStillReplaying) + { + long duration = System.currentTimeMillis() - replayStartTimeMillis; + LogTools.info("Done replaying... duration = " + duration + "ms"); + } + return isStillReplaying; } - if (timeStepReplay >= size - 1) + } + + private int findCurrentIndex(long timeInTrajectory) + { + int searchIndex = lastReplayIndex; + while (searchIndex < vrInputData.size() && vrInputData.get(searchIndex).timeInTrajectoryMillis < timeInTrajectory) { - doneReplaying = true; - this.reset(); + searchIndex++; } - else + + // if first or last index, return it + if (searchIndex <= 0 || searchIndex >= vrInputData.size() - 1) { - timeStepReplay++; + replayInterpolationAlpha = -1.0; + return searchIndex; } - return values; + + // compute the interpolation between the two adjacent frames to smoothly replay + long t0 = vrInputData.get(searchIndex - 1).timeInTrajectoryMillis; + long t1 = vrInputData.get(searchIndex).timeInTrajectoryMillis; + replayInterpolationAlpha = ((double) (timeInTrajectory - t0)) / (t1 - t0); + + return searchIndex; } - public void record(double[] values) + public String getPath() { - if (savedRecording) - savedRecording = false; - double[] localValues = new double[values.length]; - System.arraycopy(values, 0, localValues, 0, localValues.length); - dataMatrix.add(localValues); + return this.filePath; } - /** - * Useful if we are recording trajectories of different parts but not in the same scope - * and we want to concatenate them into one single row to have a single csv file - * rather than having multiple TrajectoryRecordReplay objects and multiple csv files - */ - public void concatenateData() + public void setPath(String filePath) { - for (int i = 0; i < dataMatrix.size(); i = i + numberOfParts) - { - double[] concatenatedRow = dataMatrix.get(i); - for (int j = 1; j <= numberOfParts - 1; j++) - { - concatenatedRow = concatenateWithCopy(concatenatedRow, dataMatrix.get(i + j)); - } - concatenatedDataMatrix.add(concatenatedRow); + this.filePath = filePath; + } + + public void onUpdateStartRecord() + { + if (vrInputData.isEmpty()) + { // First record tick, initialize + replayStartTimeMillis = System.currentTimeMillis(); } - concatenated = true; + + long timeInTrajectoryMillis = System.currentTimeMillis() - replayStartTimeMillis; + vrInputData.add(new VRInputData(timeInTrajectoryMillis)); } - /** - * Useful if we are replaying a csv file where multiple parts have been concatenated in one single row - * and we want the info of each part in a separate row. - * Not useful if you have different parts with different number of elements - */ - private void splitData() + public void recordControllerData(RobotSide robotSide, + boolean aButtonPressed, + boolean bButtonPressed, + boolean triggerPressed, + ReferenceFrame desiredControlFrame, + Vector3D angularVelocity, + Vector3D linearVelocity, + ReferenceFrame recordFrame) + { + vrInputData.get(vrInputData.size() - 1).controllerData.get(robotSide) + .set(aButtonPressed, + bButtonPressed, + triggerPressed, + desiredControlFrame, + angularVelocity, + linearVelocity, + recordFrame); + } + + public void onRecordStart() { - for (int i = 0; i < dataMatrix.size(); i++) + vrInputData.clear(); + } + + public void onRecordEnd() + { + if (vrInputData.isEmpty()) + return; + + // save recording + // if recordFile name has not been set, generate file with current date and time as name + String fileName = ""; + if (recordFileName.isEmpty()) + { + fileName = new SimpleDateFormat("yyMMddHHmmss'.csv'").format(new Date()); + recordFileName = fileName; + } + else + fileName = recordFileName; + File csvFile = new File(filePath + "/" + fileName); + try (FileWriter writer = new FileWriter(csvFile)) { - double[] row = dataMatrix.get(i); - for (int n = 0; n <= numberOfParts - 1; n++) + // First line is just the tracked segments + for (int i = 0; i < numberOfTrackers; i++) { - double[] splitRow = new double[row.length / numberOfParts]; - for (int j = 0; j < splitRow.length; j++) - { - splitRow[j] = row[j + n * splitRow.length]; - } - splitDataMatrix.add(splitRow); + writer.write(VRTrackedSegmentType.TRACKER_TYPES[i] + (i < numberOfTrackers - 1 ? "," : "\n")); + } + + for (int row = 0; row < this.vrInputData.size(); row++) + { + VRInputData inputData = this.vrInputData.get(row); + writer.write(inputData.toString()); } } - } + catch (IOException e) + { + e.printStackTrace(); + } - public void saveRecording() - { - if (concatenated) // save concatenated data (a single row for every body part) - writeCSV(concatenatedDataMatrix); - else - writeCSV(dataMatrix); - this.reset(); - savedRecording = true; + long duration = System.currentTimeMillis() - replayStartTimeMillis; + LogTools.info("Record ended. Duration = " + duration + "ms"); } - public void readCSV() + public boolean onReplayStart(String replayFileToLoad, ReferenceFrame recordFrame) { - doneReplaying = false; + vrInputData.clear(); + File replayFile = new File(replayFileToLoad); + replayIndex = -1; + replayStartTimeMillis = -1; + try { - BufferedReader fileReader = new BufferedReader(new FileReader(filePath)); + BufferedReader fileReader = new BufferedReader(new FileReader(replayFile)); String line; + + // Check first line matches tracked segments + line = fileReader.readLine(); + + boolean matchesLogFile = true; + String[] segments = line.split(","); + if (segments.length != numberOfTrackers) + { + matchesLogFile = false; + } + for (int i = 0; i < numberOfTrackers; i++) + { + if (!VRTrackedSegmentType.TRACKER_TYPES[i].name().equals(segments[i])) + matchesLogFile = false; + } + + if (!matchesLogFile) + { + LogTools.info("Log file has the following trackers, which doesn't match configuration: " + line); + return false; + } + while ((line = fileReader.readLine()) != null) { - String[] values = line.split(","); - double[] dataValues = new double[values.length]; - for (int i = 0; i < values.length; i++) - { - dataValues[i] = Double.parseDouble(values[i]); - } - dataMatrix.add(dataValues); + vrInputData.add(new VRInputData(line.split(","), recordFrame)); } fileReader.close(); + + return true; } - catch (IOException e) + catch (Exception e) { e.printStackTrace(); + + return false; } } - public void writeCSV(List dataMatrix) + private static class VRInputData { - // if recordFile name has not been set, generate file with current date and time as name - String fileName = ""; - if (recordFileName.isEmpty()) + private final long timeInTrajectoryMillis; + private final SideDependentList controllerData = new SideDependentList<>(new ControllerData(), new ControllerData()); + private final List trackerData = new ArrayList<>(); + + public VRInputData(long timeInTrajectoryMillis) { - fileName = new SimpleDateFormat("yyMMddHHmmssZ'.csv'").format(new Date()); - recordFileName = fileName; + this.timeInTrajectoryMillis = timeInTrajectoryMillis; + + for (int i = 0; i < numberOfTrackers; i++) + { + trackerData.add(new TrackerData()); + } } - else - fileName = recordFileName; - File csvFile = new File(filePath + "/" + fileName); - try (PrintWriter writer = new PrintWriter(csvFile)) + + public VRInputData(String[] data, ReferenceFrame recordFrame) { - for (int row = 0; row < dataMatrix.size(); row++) + int index = 0; + timeInTrajectoryMillis = Long.parseLong(data[index++]); + + for (RobotSide robotSide : RobotSide.values()) { - double[] dataLine = dataMatrix.get(row); - for (int col = 0; col < dataLine.length; col++) - { - writer.print(dataLine[col]); - if (col < dataLine.length - 1) - writer.append(","); - } - if (row < dataMatrix.size() - 1) - { - writer.println(); - } + index = controllerData.get(robotSide).setFromCSV(index, data, recordFrame); + } + + for (int i = 0; i < numberOfTrackers; i++) + { + trackerData.add(new TrackerData()); + index = trackerData.get(i).setFromCSV(index, data, recordFrame); } } - catch (IOException e) + + @Override + public String toString() { - e.printStackTrace(); + StringBuilder builder = new StringBuilder(); + builder.append(timeInTrajectoryMillis); + + for (RobotSide robotSide : RobotSide.values()) + { + builder.append(","); + builder.append(controllerData.get(robotSide)); + } + for (int i = 0; i < numberOfTrackers; i++) + { + builder.append(","); + builder.append(trackerData.get(i)); + } + + builder.append("\n"); + return builder.toString(); + } + } + + private static class TrackerData + { + final FramePose3D pose = new FramePose3D(); + final FrameVector3D angularVelocity = new FrameVector3D(); + final FrameVector3D linearVelocity = new FrameVector3D(); + + public int setFromCSV(int index, String[] data, ReferenceFrame recordFrame) + { + index = loadCSV(index, data, pose, recordFrame, ReferenceFrame.getWorldFrame()); + index = loadCSV(index, data, angularVelocity, recordFrame, ReferenceFrame.getWorldFrame()); + index = loadCSV(index, data, linearVelocity, recordFrame, ReferenceFrame.getWorldFrame()); + return index; + } + + void set(ReferenceFrame trackerFrame, Vector3D angularVelocity, Vector3D linearVelocity, ReferenceFrame recordFrame) + { + this.pose.setToZero(trackerFrame); + this.pose.changeFrame(recordFrame); + this.angularVelocity.set(ReferenceFrame.getWorldFrame(), angularVelocity); + this.angularVelocity.changeFrame(recordFrame); + this.linearVelocity.set(ReferenceFrame.getWorldFrame(), linearVelocity); + this.linearVelocity.changeFrame(recordFrame); + } + + @Override + public String toString() + { + return poseToCSV(pose) + "," + tupleToCSV(angularVelocity) + "," + tupleToCSV(linearVelocity); } } - private void reset() + private static class ControllerData extends TrackerData { - timeStepReplay = 0; - dataMatrix.clear(); - concatenated = false; - concatenatedDataMatrix.clear(); - splitDataMatrix.clear(); - recordFileName = ""; + boolean aButtonPressed; + boolean bButtonPressed; + boolean triggerPressed; + + public int setFromCSV(int index, String[] data, ReferenceFrame recordFrame) + { + index = super.setFromCSV(index, data, recordFrame); + + aButtonPressed = Boolean.parseBoolean(data[index++]); + bButtonPressed = Boolean.parseBoolean(data[index++]); + triggerPressed = Boolean.parseBoolean(data[index++]); + + return index; + } + + void set(boolean aButtonPressed, boolean bButtonPressed, boolean triggerPressed, ReferenceFrame trackerFrame, Vector3D angularVelocity, Vector3D linearVelocity, ReferenceFrame recordFrame) + { + super.set(trackerFrame, angularVelocity, linearVelocity, recordFrame); + + this.aButtonPressed = aButtonPressed; + this.bButtonPressed = bButtonPressed; + this.triggerPressed = triggerPressed; + } + + @Override + public String toString() + { + return super.toString() + "," + aButtonPressed + "," + bButtonPressed + "," + triggerPressed; + } } - private double[] concatenateWithCopy(double[] array1, double[] array2) + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////// Getters for button presses ///////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + public boolean getAButtonPressed(RobotSide robotSide) { - double[] result = new double[array1.length + array2.length]; - System.arraycopy(array1, 0, result, 0, array1.length); - System.arraycopy(array2, 0, result, array1.length, array2.length); + return checkButtonPressOverInterval(lastReplayIndex, replayIndex, i -> getAButtonPressed(robotSide, i, vrInputData)); + } - return result; + public boolean getBButtonPressed(RobotSide robotSide) + { + return checkButtonPressOverInterval(lastReplayIndex, replayIndex, i -> getBButtonPressed(robotSide, i, vrInputData)); } - public boolean hasSavedRecording() + public boolean getTriggerPressed(RobotSide robotSide) { - return savedRecording; + return checkButtonPressOverInterval(lastReplayIndex, replayIndex, i -> getTriggerPressed(robotSide, i, vrInputData)); } - public boolean hasDoneReplay() + public void packTrackedData(VRTrackedSegmentType segmentType, FramePose3D poseToPack, FrameVector3D angularVelocityToPack, FrameVector3D linearVelocityToPack) { - return doneReplaying; + IntFunction trackerDataSupplier = index -> segmentType.getSegmentSide() == null ? vrInputData.get(index).trackerData.get(trackerIndex.get(segmentType)) : vrInputData.get(index).controllerData.get(segmentType.getSegmentSide()); + packTrackerDataInternal(trackerDataSupplier, poseToPack, angularVelocityToPack, linearVelocityToPack); } - public void setDoneReplay(boolean doneReplaying) + private void packTrackerDataInternal(IntFunction trackerSupplier, FramePose3D poseToPack, FrameVector3D angularVelocityToPack, FrameVector3D linearVelocityToPack) { - this.doneReplaying = doneReplaying; + if (replayInterpolationAlpha == -1.0) + { + TrackerData trackerData = trackerSupplier.apply(replayIndex); + poseToPack.set(trackerData.pose); + angularVelocityToPack.set(trackerData.angularVelocity); + linearVelocityToPack.set(trackerData.linearVelocity); + } + else + { + TrackerData trackerData0 = trackerSupplier.apply(replayIndex - 1); + TrackerData trackerData1 = trackerSupplier.apply(replayIndex); + + poseToPack.interpolate(trackerData0.pose, trackerData1.pose, replayInterpolationAlpha); + angularVelocityToPack.interpolate(trackerData0.angularVelocity, trackerData1.angularVelocity, replayInterpolationAlpha); + linearVelocityToPack.interpolate(trackerData0.linearVelocity, trackerData1.linearVelocity, replayInterpolationAlpha); + } } - public String getPath() + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////// Helper methods /////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + private static boolean getAButtonPressed(RobotSide robotSide, int index, List vrInputData) { - return this.filePath; + ControllerData controllerData = vrInputData.get(index).controllerData.get(robotSide); + return controllerData.aButtonPressed; } - public void setPath(String filePath) + private static boolean getBButtonPressed(RobotSide robotSide, int index, List vrInputData) { - this.filePath = filePath; - this.reset(); + ControllerData controllerData = vrInputData.get(index).controllerData.get(robotSide); + return controllerData.bButtonPressed; } - public void setPath(String filePath, boolean reset) + private static boolean getTriggerPressed(RobotSide robotSide, int index, List vrInputData) { - this.filePath = filePath; - if (reset) - this.reset(); + ControllerData controllerData = vrInputData.get(index).controllerData.get(robotSide); + return controllerData.triggerPressed; } - public void setNumberOfParts(int numberOfParts) + private static boolean checkButtonPressOverInterval(int indexStartExclusive, int indexEndInclusive, IntPredicate buttonPressProvider) { - this.numberOfParts = numberOfParts; + for (int index = indexEndInclusive; index > indexStartExclusive; index--) + { + if (buttonPressProvider.test(index)) + return true; + } + return false; } - public List getData() + private static String tupleToCSV(Tuple3DReadOnly tuple) { - return dataMatrix; + return tuple.getX() + "," + tuple.getY() + "," + tuple.getZ(); } - public List getConcatenatedData() + private static String poseToCSV(FramePose3DReadOnly pose) { - return concatenatedDataMatrix; + return pose.getOrientation().getX() + "," + pose.getOrientation().getY() + "," + pose.getOrientation().getZ() + "," + pose.getOrientation().getS() + "," + pose.getPosition().getX() + "," + pose.getPosition().getY() + "," + pose.getPosition().getZ(); } - public int getTimeStepReplay() + private static int loadCSV(int index, String[] data, FramePose3D poseToPack, ReferenceFrame saveFrame, ReferenceFrame loadFrame) { - return timeStepReplay; + poseToPack.setReferenceFrame(saveFrame); + poseToPack.getOrientation().set(Double.parseDouble(data[index++]), Double.parseDouble(data[index++]), Double.parseDouble(data[index++]), Double.parseDouble(data[index++])); + poseToPack.getPosition().set(Double.parseDouble(data[index++]), Double.parseDouble(data[index++]), Double.parseDouble(data[index++])); + poseToPack.changeFrame(loadFrame); + return index; } - public String getRecordFileName() + private static int loadCSV(int index, String[] data, FrameVector3D vectorToPack, ReferenceFrame saveFrame, ReferenceFrame loadFrame) { - return recordFileName; + vectorToPack.setReferenceFrame(saveFrame); + vectorToPack.set(Double.parseDouble(data[index++]), Double.parseDouble(data[index++]), Double.parseDouble(data[index++])); + vectorToPack.changeFrame(loadFrame); + return index; } - public void setRecordFileName(String recordFileName) + private static int loadCSV(int index, String[] data, FramePoint3D pointToPack, ReferenceFrame saveFrame, ReferenceFrame loadFrame) { - this.recordFileName = recordFileName; + pointToPack.setReferenceFrame(saveFrame); + pointToPack.set(Double.parseDouble(data[index++]), Double.parseDouble(data[index++]), Double.parseDouble(data[index++])); + pointToPack.changeFrame(loadFrame); + return index; } } \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/motionRetargeting/VRTrackedSegmentType.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/motionRetargeting/VRTrackedSegmentType.java index 4224a4641f96..14b033350ed7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/motionRetargeting/VRTrackedSegmentType.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/motionRetargeting/VRTrackedSegmentType.java @@ -1,6 +1,5 @@ package us.ihmc.motionRetargeting; -import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.robotics.robotSide.RobotSide; public enum VRTrackedSegmentType @@ -14,10 +13,12 @@ public enum VRTrackedSegmentType LEFT_ANKLE("Left Ankle", RobotSide.LEFT), RIGHT_ANKLE("Right Ankle", RobotSide.RIGHT); - private final String segmentName; private final RobotSide robotSide; + public static VRTrackedSegmentType[] TRACKER_TYPES = new VRTrackedSegmentType[] {LEFT_WRIST, RIGHT_WRIST, CHEST, WAIST, LEFT_ANKLE, RIGHT_ANKLE}; + public static VRTrackedSegmentType[] CONTROLLER_TYPES = new VRTrackedSegmentType[] {LEFT_HAND, RIGHT_HAND}; + VRTrackedSegmentType(String segmentName, RobotSide robotSide) { @@ -44,14 +45,4 @@ public boolean isHandRelated() { return segmentName.contains("Hand"); } - - public static VRTrackedSegmentType[] getTrackerTypes() - { - return new VRTrackedSegmentType[] {LEFT_WRIST, RIGHT_WRIST, CHEST, WAIST, LEFT_ANKLE, RIGHT_ANKLE}; - } - - public static VRTrackedSegmentType[] getControllerTypes() - { - return new VRTrackedSegmentType[] {LEFT_HAND, RIGHT_HAND}; - } } \ No newline at end of file diff --git a/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl b/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl index 4f8cba84bf74..efb942202d91 100644 --- a/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl +++ b/ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsToolboxOutputStatus_.idl @@ -80,6 +80,11 @@ module toolbox_msgs * Support region used by the toolbox */ sequence support_region; + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + @defaultValue(value=-1.0) + double support_region_sensitivity; @defaultValue(value=-1.0) double solution_quality; }; diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java index 7075835bb139..156e0556653a 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsPlanningToolboxOutputStatusPubSubType.java @@ -15,7 +15,7 @@ public class KinematicsPlanningToolboxOutputStatusPubSubType implements us.ihmc. @Override public final java.lang.String getDefinitionChecksum() { - return "a503326897dee5045f25aa53a759d78ab1301ea0b64e652c49ff3c85ee497579"; + return "b90a1333b8ecfd4b7d1781eb80ab2337d331638dd4ce7bd34568aab14a7c8ddb"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java index 7c74f7ca6c52..9cc71efd8f79 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatus.java @@ -66,6 +66,10 @@ public class KinematicsToolboxOutputStatus extends Packet support_region_; + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + public double support_region_sensitivity_ = -1.0; public double solution_quality_ = -1.0; public KinematicsToolboxOutputStatus() @@ -103,6 +107,8 @@ public void set(KinematicsToolboxOutputStatus other) geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.desired_root_linear_velocity_, desired_root_linear_velocity_); geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.desired_root_angular_velocity_, desired_root_angular_velocity_); support_region_.set(other.support_region_); + support_region_sensitivity_ = other.support_region_sensitivity_; + solution_quality_ = other.solution_quality_; } @@ -209,6 +215,21 @@ public us.ihmc.idl.IDLSequence.Object getSuppor return support_region_; } + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + public void setSupportRegionSensitivity(double support_region_sensitivity) + { + support_region_sensitivity_ = support_region_sensitivity; + } + /** + * Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. + */ + public double getSupportRegionSensitivity() + { + return support_region_sensitivity_; + } + public void setSolutionQuality(double solution_quality) { solution_quality_ = solution_quality; @@ -257,6 +278,8 @@ public boolean epsilonEquals(KinematicsToolboxOutputStatus other, double epsilon { if (!this.support_region_.get(i).epsilonEquals(other.support_region_.get(i), epsilon)) return false; } } + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.support_region_sensitivity_, other.support_region_sensitivity_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.solution_quality_, other.solution_quality_, epsilon)) return false; @@ -285,6 +308,8 @@ public boolean equals(Object other) if (!this.desired_root_linear_velocity_.equals(otherMyClass.desired_root_linear_velocity_)) return false; if (!this.desired_root_angular_velocity_.equals(otherMyClass.desired_root_angular_velocity_)) return false; if (!this.support_region_.equals(otherMyClass.support_region_)) return false; + if(this.support_region_sensitivity_ != otherMyClass.support_region_sensitivity_) return false; + if(this.solution_quality_ != otherMyClass.solution_quality_) return false; @@ -317,6 +342,8 @@ public java.lang.String toString() builder.append(this.desired_root_angular_velocity_); builder.append(", "); builder.append("support_region="); builder.append(this.support_region_); builder.append(", "); + builder.append("support_region_sensitivity="); + builder.append(this.support_region_sensitivity_); builder.append(", "); builder.append("solution_quality="); builder.append(this.solution_quality_); builder.append("}"); diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java index 9c8e961a1db0..2a9eaad7caac 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/KinematicsToolboxOutputStatusPubSubType.java @@ -15,7 +15,7 @@ public class KinematicsToolboxOutputStatusPubSubType implements us.ihmc.pubsub.T @Override public final java.lang.String getDefinitionChecksum() { - return "624cc92872f1d8473b47e098815669b6fb9c0a5c2459a80441afc9913a518f73"; + return "24e1c048ab9f7fc5b8bb75a4b51d96efd5aad483599cdaba27efc225e88dbebd"; } @Override @@ -75,6 +75,8 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);} current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + return current_alignment - initial_alignment; } @@ -121,6 +123,9 @@ public final static int getCdrSerializedSize(toolbox_msgs.msg.dds.KinematicsTool current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + return current_alignment - initial_alignment; } @@ -149,6 +154,8 @@ public static void write(toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus data cdr.write_type_e(data.getSupportRegion());else throw new RuntimeException("support_region field exceeds the maximum length"); + cdr.write_type_6(data.getSupportRegionSensitivity()); + cdr.write_type_6(data.getSolutionQuality()); } @@ -168,6 +175,8 @@ public static void read(toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus data, geometry_msgs.msg.dds.Vector3PubSubType.read(data.getDesiredRootLinearVelocity(), cdr); geometry_msgs.msg.dds.Vector3PubSubType.read(data.getDesiredRootAngularVelocity(), cdr); cdr.read_type_e(data.getSupportRegion()); + data.setSupportRegionSensitivity(cdr.read_type_6()); + data.setSolutionQuality(cdr.read_type_6()); @@ -190,6 +199,7 @@ public final void serialize(toolbox_msgs.msg.dds.KinematicsToolboxOutputStatus d ser.write_type_a("desired_root_angular_velocity", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getDesiredRootAngularVelocity()); ser.write_type_e("support_region", data.getSupportRegion()); + ser.write_type_6("support_region_sensitivity", data.getSupportRegionSensitivity()); ser.write_type_6("solution_quality", data.getSolutionQuality()); } @@ -210,6 +220,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, toolbox_msg ser.read_type_a("desired_root_angular_velocity", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getDesiredRootAngularVelocity()); ser.read_type_e("support_region", data.getSupportRegion()); + data.setSupportRegionSensitivity(ser.read_type_6("support_region_sensitivity")); data.setSolutionQuality(ser.read_type_6("solution_quality")); } diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java index 604856053625..44a60f628086 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WalkingControllerPreviewOutputMessagePubSubType.java @@ -15,7 +15,7 @@ public class WalkingControllerPreviewOutputMessagePubSubType implements us.ihmc. @Override public final java.lang.String getDefinitionChecksum() { - return "85f7193434b41d5e0acd89bb0f6e673559fd4d1937cb356ec8c9f5ba50ea4987"; + return "88099551935668d5c7e7176614054177319b76eb9daa4ed8e46971b48e26f2b8"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java index 404a923707a0..bf7202623638 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java @@ -15,7 +15,7 @@ public class WholeBodyTrajectoryToolboxConfigurationMessagePubSubType implements @Override public final java.lang.String getDefinitionChecksum() { - return "31e206ec5e6f4cb3bcc48761075d11243576488d1865e110f3a90183f043f2ee"; + return "eaee35e911cc5f906d7a1175653f936458205629c5b723a1196566e05a5ee4d4"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java index d85b7694b2ad..9a96363140a4 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java @@ -15,7 +15,7 @@ public class WholeBodyTrajectoryToolboxMessagePubSubType implements us.ihmc.pubs @Override public final java.lang.String getDefinitionChecksum() { - return "0507fa389750db026c3d95bc6976be87f4dec98a0413bc67f760b93e7438cf70"; + return "785094e2646fca4937cca843355b88163fd8518cae80ba813ab6247a83dddedd"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java index 81582744d25e..3ac539a27d7a 100644 --- a/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/toolbox_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatusPubSubType.java @@ -15,7 +15,7 @@ public class WholeBodyTrajectoryToolboxOutputStatusPubSubType implements us.ihmc @Override public final java.lang.String getDefinitionChecksum() { - return "a862a03a5fd7d21b9fce0b609911614c4abcb5d99da934102fd2d484b5356e48"; + return "2b8e6d1b94f9dfc4a0f3d2d6128d9a546cb73c0326c939e04ec1d8e0f5c9b081"; } @Override diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg index 2abce8796999..8c211c0eccf0 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg @@ -38,4 +38,7 @@ geometry_msgs/Vector3 desired_root_angular_velocity # Support region used by the toolbox geometry_msgs/Point[<=32] support_region +# Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. +float64 support_region_sensitivity -1 + float64 solution_quality -1 \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg b/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg index d6f0fb2795da..9c7d2a7af495 100644 --- a/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg +++ b/ihmc-interfaces/src/main/messages/ros1/toolbox_msgs/msg/KinematicsToolboxOutputStatus.msg @@ -43,6 +43,10 @@ geometry_msgs/Vector3 desired_root_angular_velocity # Support region used by the toolbox geometry_msgs/Point[] support_region +# Sensitivity of the support region with respect to the posture. Indicates postures where the StabilityMarginKinematicsCostCalculator is more effective. +# Field default value -1.0 +float64 support_region_sensitivity + # Field default value -1.0 float64 solution_quality