diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandler.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandler.java index 6cd872b9395..04b631b414d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandler.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandler.java @@ -17,7 +17,7 @@ import us.ihmc.robotics.math.trajectories.generators.MultipleWaypointsPoseTrajectoryGenerator; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalProvider; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceTools.java index 3f06f7e557e..50f2d3ec877 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceTools.java @@ -8,7 +8,7 @@ import us.ihmc.humanoidRobotics.footstep.FootstepTiming; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.commons.time.TimeIntervalTools; +import us.ihmc.robotics.time.TimeIntervalTools; import java.util.Comparator; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdater.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdater.java index 889858a9cdb..fbe1eb107cd 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdater.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdater.java @@ -14,7 +14,7 @@ import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.commons.time.TimeIntervalTools; +import us.ihmc.robotics.time.TimeIntervalTools; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedStepTransition.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedStepTransition.java index de6e780c40a..1d018cc8557 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedStepTransition.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedStepTransition.java @@ -5,7 +5,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.commons.time.TimeInterval; +import us.ihmc.robotics.time.TimeInterval; import java.util.ArrayList; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedTimedStep.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedTimedStep.java index c1f4322e6b9..4fad7f6c78e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedTimedStep.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedTimedStep.java @@ -3,10 +3,10 @@ import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; -import us.ihmc.commons.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalReadOnly; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/SimpleBipedContactPhase.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/SimpleBipedContactPhase.java index 2f8abf737fd..88d0e6b8cc7 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/SimpleBipedContactPhase.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/SimpleBipedContactPhase.java @@ -11,8 +11,8 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; import java.util.ArrayList; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ThreePotatoAngularMomentumCalculator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ThreePotatoAngularMomentumCalculator.java index 8202276974c..b20ee15519e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ThreePotatoAngularMomentumCalculator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ThreePotatoAngularMomentumCalculator.java @@ -28,8 +28,8 @@ import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.robotics.time.ExecutionTimer; -import us.ihmc.commons.time.TimeIntervalProvider; -import us.ihmc.commons.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalReadOnly; import us.ihmc.scs2.definition.visual.ColorDefinitions; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinition; import us.ihmc.scs2.definition.yoGraphic.YoGraphicDefinitionFactory; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectorySegment.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectorySegment.java index 0fff51dc6b9..244df5cad26 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectorySegment.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectorySegment.java @@ -11,9 +11,9 @@ import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.robotics.math.trajectories.interfaces.FixedFramePositionTrajectoryGenerator; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; import static us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CoMTrajectoryPlannerTools.sufficientlyLarge; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProvider.java index 7d6656938a1..daf3a4ada06 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProvider.java @@ -7,7 +7,7 @@ import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalProvider; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProviderTools.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProviderTools.java index 8ce8d049a22..7836458d403 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProviderTools.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ContactStateProviderTools.java @@ -1,6 +1,6 @@ package us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning; -import us.ihmc.commons.time.TimeIntervalTools; +import us.ihmc.robotics.time.TimeIntervalTools; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/LinearCoMTrajectoryHandler.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/LinearCoMTrajectoryHandler.java index 62d71b49ab3..f809bf535f9 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/LinearCoMTrajectoryHandler.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/LinearCoMTrajectoryHandler.java @@ -15,7 +15,7 @@ import us.ihmc.robotics.math.trajectories.generators.MultipleSegmentPositionTrajectoryGenerator; import us.ihmc.robotics.math.trajectories.interfaces.Polynomial3DBasics; import us.ihmc.robotics.math.trajectories.interfaces.Polynomial3DReadOnly; -import us.ihmc.commons.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeIntervalReadOnly; import us.ihmc.yoVariables.registry.YoRegistry; import java.util.ArrayList; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/MultipleCoMSegmentTrajectoryGenerator.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/MultipleCoMSegmentTrajectoryGenerator.java index 5471e2b293d..911a96ae928 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/MultipleCoMSegmentTrajectoryGenerator.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/MultipleCoMSegmentTrajectoryGenerator.java @@ -4,7 +4,7 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.robotics.math.trajectories.generators.MultipleSegmentPositionTrajectoryGenerator; -import us.ihmc.commons.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeIntervalReadOnly; import us.ihmc.yoVariables.registry.YoRegistry; import static us.ihmc.robotics.math.trajectories.generators.MultipleWaypointsTrajectoryGenerator.defaultMaximumNumberOfWaypoints; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/SettableContactStateProvider.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/SettableContactStateProvider.java index ed3d0cffbba..86a4ad22362 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/SettableContactStateProvider.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/SettableContactStateProvider.java @@ -6,9 +6,9 @@ import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalReadOnly; import java.util.ArrayList; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/TimedContactInterval.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/TimedContactInterval.java index 2f7039e5089..626033dc013 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/TimedContactInterval.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/TimedContactInterval.java @@ -2,8 +2,8 @@ import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalProvider; public class TimedContactInterval implements TimeIntervalProvider { diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandlerTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandlerTest.java index 543f8282a80..365fe8beb27 100644 --- a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandlerTest.java +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/AngularMomentumHandlerTest.java @@ -25,7 +25,7 @@ import us.ihmc.robotics.math.trajectories.generators.MultipleSegmentPositionTrajectoryGenerator; import us.ihmc.robotics.math.trajectories.interfaces.FramePolynomial3DBasics; import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalBasics; import us.ihmc.yoVariables.parameters.DefaultParameterReader; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdaterTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdaterTest.java index 6c8b4aaaaf5..4b2053c838f 100644 --- a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdaterTest.java +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/BipedContactSequenceUpdaterTest.java @@ -12,7 +12,7 @@ import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.commons.time.TimeInterval; +import us.ihmc.robotics.time.TimeInterval; import java.util.ArrayList; import java.util.List; diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ECMPTrajectoryCalculatorTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ECMPTrajectoryCalculatorTest.java index 638090823e3..15328de186e 100644 --- a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ECMPTrajectoryCalculatorTest.java +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/bipedPlanning/ECMPTrajectoryCalculatorTest.java @@ -18,8 +18,8 @@ import us.ihmc.robotics.math.trajectories.generators.MultipleSegmentPositionTrajectoryGenerator; import us.ihmc.robotics.math.trajectories.interfaces.*; import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; import us.ihmc.yoVariables.parameters.DefaultParameterReader; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectoryPlannerInterfaceTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectoryPlannerInterfaceTest.java index 4542c0c0455..f5c4420a5d3 100644 --- a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectoryPlannerInterfaceTest.java +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/CoMTrajectoryPlannerInterfaceTest.java @@ -16,7 +16,7 @@ import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.commons.time.TimeInterval; +import us.ihmc.robotics.time.TimeInterval; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ComTrajectoryPlannerTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ComTrajectoryPlannerTest.java index 0950d7a4d37..1c4e3a8ba2b 100644 --- a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ComTrajectoryPlannerTest.java +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/comPlanning/ComTrajectoryPlannerTest.java @@ -9,7 +9,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.commons.time.TimeInterval; +import us.ihmc.robotics.time.TimeInterval; import java.util.Random; diff --git a/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/visualizers/RDXPolynomial.java b/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/visualizers/RDXPolynomial.java index 335b00956ab..980a35ff277 100644 --- a/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/visualizers/RDXPolynomial.java +++ b/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/visualizers/RDXPolynomial.java @@ -13,9 +13,9 @@ import us.ihmc.graphicsDescription.appearance.YoAppearance; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPolynomial3D; import us.ihmc.robotics.math.trajectories.interfaces.PolynomialReadOnly; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.providers.IntegerProvider; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TimeIntervalCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TimeIntervalCommand.java index dfb0738f700..e83dd8d8d5a 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TimeIntervalCommand.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TimeIntervalCommand.java @@ -4,7 +4,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.communication.controllerAPI.command.Command; import us.ihmc.euclid.tools.EuclidCoreTools; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalBasics; public class TimeIntervalCommand implements Command { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/FixedFramePolynomialEstimator3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/FixedFramePolynomialEstimator3D.java index efc477614a6..f151193bc18 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/FixedFramePolynomialEstimator3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/FixedFramePolynomialEstimator3D.java @@ -10,8 +10,8 @@ import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.robotics.math.trajectories.core.Polynomial3DFrameFactories; import us.ihmc.robotics.math.trajectories.interfaces.FixedFramePositionTrajectoryGenerator; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; public class FixedFramePolynomialEstimator3D implements FixedFramePositionTrajectoryGenerator, TimeIntervalProvider, Settable { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator.java index 67b091836df..121cb740451 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator.java @@ -6,9 +6,9 @@ import org.ejml.interfaces.linsol.LinearSolverDense; import us.ihmc.euclid.interfaces.Settable; import us.ihmc.matrixlib.MatrixTools; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; public class PolynomialEstimator implements TimeIntervalProvider, Settable { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator3D.java index f7d8eca50bf..9f4255e69e1 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/PolynomialEstimator3D.java @@ -7,8 +7,8 @@ import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.robotics.math.trajectories.core.Trajectory3DFactories; import us.ihmc.robotics.math.trajectories.interfaces.PositionTrajectoryGenerator; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; public class PolynomialEstimator3D implements PositionTrajectoryGenerator, TimeIntervalProvider, Settable { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractFramePolynomial3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractFramePolynomial3D.java index e01b5a508b6..0a9eb566103 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractFramePolynomial3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractFramePolynomial3D.java @@ -13,7 +13,7 @@ import us.ihmc.robotics.math.trajectories.interfaces.Polynomial3DBasics; import us.ihmc.robotics.math.trajectories.core.Polynomial3DFrameFactories; import us.ihmc.robotics.math.trajectories.interfaces.PolynomialBasics; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalBasics; public class AbstractFramePolynomial3D implements FramePolynomial3DBasics { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractPolynomial3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractPolynomial3D.java index 883c37b8ba5..9b47a236fa4 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractPolynomial3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/abstracts/AbstractPolynomial3D.java @@ -11,7 +11,7 @@ import us.ihmc.robotics.math.trajectories.interfaces.PolynomialBasics; import us.ihmc.robotics.math.trajectories.interfaces.PositionTrajectoryGenerator; import us.ihmc.robotics.math.trajectories.yoVariables.YoPolynomial; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalBasics; import java.util.List; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Polynomial.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Polynomial.java index 40f43d817e3..0fe85ec157b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Polynomial.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Polynomial.java @@ -4,8 +4,8 @@ import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; import org.ejml.interfaces.linsol.LinearSolverDense; import us.ihmc.robotics.math.trajectories.interfaces.PolynomialBasics; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; /** * Used to compute the value and 1st and 2nd order derivatives of a polynomial. diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Trajectory3DFactories.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Trajectory3DFactories.java index 5e7e2bad0a7..83d6d6cb678 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Trajectory3DFactories.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/core/Trajectory3DFactories.java @@ -5,8 +5,8 @@ import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; import us.ihmc.yoVariables.providers.DoubleProvider; import java.util.function.DoubleConsumer; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleSegmentPositionTrajectoryGenerator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleSegmentPositionTrajectoryGenerator.java index bf710665689..5c4c5cc14d3 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleSegmentPositionTrajectoryGenerator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleSegmentPositionTrajectoryGenerator.java @@ -6,9 +6,9 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.robotics.math.trajectories.interfaces.FixedFramePositionTrajectoryGenerator; -import us.ihmc.commons.time.TimeIntervalProvider; -import us.ihmc.commons.time.TimeIntervalReadOnly; -import us.ihmc.commons.time.TimeIntervalTools; +import us.ihmc.robotics.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalReadOnly; +import us.ihmc.robotics.time.TimeIntervalTools; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/Polynomial3DReadOnly.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/Polynomial3DReadOnly.java index 2e32ab45394..46e9db21df0 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/Polynomial3DReadOnly.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/Polynomial3DReadOnly.java @@ -3,8 +3,8 @@ import org.ejml.data.DMatrixRMaj; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.commons.time.TimeIntervalBasics; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeIntervalProvider; public interface Polynomial3DReadOnly extends PositionTrajectoryGenerator, TimeIntervalProvider { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/PolynomialReadOnly.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/PolynomialReadOnly.java index 3b24d07c910..f533d9fc544 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/PolynomialReadOnly.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/interfaces/PolynomialReadOnly.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.trajectories.interfaces; -import us.ihmc.commons.time.TimeIntervalProvider; +import us.ihmc.robotics.time.TimeIntervalProvider; public interface PolynomialReadOnly extends TimeIntervalProvider, DoubleTrajectoryGenerator { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/yoVariables/YoPolynomial.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/yoVariables/YoPolynomial.java index 04dadf057c2..5eb37d9c101 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/yoVariables/YoPolynomial.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/yoVariables/YoPolynomial.java @@ -7,8 +7,8 @@ import us.ihmc.graphicsDescription.yoGraphics.YoGraphicPolynomial3D.PolynomialVariableHolder; import us.ihmc.robotics.math.trajectories.core.PolynomialTools; import us.ihmc.robotics.math.trajectories.interfaces.PolynomialBasics; -import us.ihmc.commons.time.TimeInterval; -import us.ihmc.commons.time.TimeIntervalBasics; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalBasics; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeInterval.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeInterval.java new file mode 100644 index 00000000000..b6be76d193e --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeInterval.java @@ -0,0 +1,59 @@ +package us.ihmc.robotics.time; + +import us.ihmc.euclid.tools.EuclidCoreIOTools; + +public class TimeInterval implements TimeIntervalBasics +{ + private double startTime; + private double endTime; + + public TimeInterval() + { + this(Double.NaN, Double.NaN); + } + + public TimeInterval(TimeInterval timeInterval) + { + this(timeInterval.getStartTime(), timeInterval.getEndTime()); + } + + public TimeInterval(double startTime, double endTime) + { + setInterval(startTime, endTime); + checkInterval(); + } + + public double getStartTime() + { + return startTime; + } + + /** + * Use the interval setter when using this method! Otherwise, an incorrect interval may be set. + */ + @Deprecated + public void setStartTime(double startTime) + { + this.startTime = startTime; + } + + public double getEndTime() + { + return endTime; + } + + /** + * Use the interval setter when using this method! Otherwise, an incorrect interval may be set. + */ + @Deprecated + public void setEndTime(double endTime) + { + this.endTime = endTime; + } + + @Override + public String toString() + { + return EuclidCoreIOTools.getStringOf("(", " )", ", ", getStartTime(), getEndTime()); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalBasics.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalBasics.java new file mode 100644 index 00000000000..f1547271de2 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalBasics.java @@ -0,0 +1,47 @@ +package us.ihmc.robotics.time; + +import us.ihmc.euclid.tools.EuclidCoreIOTools; + +public interface TimeIntervalBasics extends TimeIntervalReadOnly +{ + /** + * Use the interval setter when using this method! Otherwise, an incorrect interval may be set. + */ + @Deprecated + void setStartTime(double startTime); + + /** + * Use the interval setter when using this method! Otherwise, an incorrect interval may be set. + */ + @Deprecated + void setEndTime(double endTime); + + default void reset() + { + setIntervalUnsafe(Double.NaN, Double.NaN); + } + + default void setInterval(double startTime, double endTime) + { + setIntervalUnsafe(startTime, endTime); + checkInterval(); + } + + default void setIntervalUnsafe(double startTime, double endTime) + { + setStartTime(startTime); + setEndTime(endTime); + } + + default void set(TimeIntervalReadOnly timeInterval) + { + setIntervalUnsafe(timeInterval.getStartTime(), timeInterval.getEndTime()); + } + + default TimeIntervalBasics shiftInterval(double shiftTime) + { + setIntervalUnsafe(getStartTime() + shiftTime, getEndTime() + shiftTime); + return this; + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalProvider.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalProvider.java new file mode 100644 index 00000000000..e8e54af2943 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalProvider.java @@ -0,0 +1,6 @@ +package us.ihmc.robotics.time; + +public interface TimeIntervalProvider +{ + TimeIntervalBasics getTimeInterval(); +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalReadOnly.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalReadOnly.java new file mode 100644 index 00000000000..e2b1dec4cc8 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalReadOnly.java @@ -0,0 +1,42 @@ +package us.ihmc.robotics.time; + +import us.ihmc.commons.MathTools; + +public interface TimeIntervalReadOnly +{ + double getStartTime(); + + double getEndTime(); + + default double getDuration() + { + return getEndTime() - getStartTime(); + } + + default boolean epsilonEquals(TimeIntervalReadOnly other, double epsilon) + { + return MathTools.epsilonEquals(getStartTime(), other.getStartTime(), epsilon) && MathTools.epsilonEquals(getEndTime(), other.getEndTime(), epsilon); + } + + default boolean intervalContains(double time) + { + return MathTools.intervalContains(time, getStartTime(), getEndTime()); + } + + default boolean epsilonContains(double time, double epsilon) + { + return MathTools.intervalContains(time, getStartTime() - epsilon, getEndTime() + epsilon); + } + + default void checkInterval() + { + checkInterval(this); + } + + static void checkInterval(TimeIntervalReadOnly timeInterval) + { + if (timeInterval.getEndTime() < timeInterval.getStartTime()) + throw new IllegalArgumentException( + "The end time is not valid! End time " + timeInterval.getEndTime() + " must be greater than start time " + timeInterval.getStartTime()); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalTools.java new file mode 100644 index 00000000000..e3fdfe29754 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeIntervalTools.java @@ -0,0 +1,239 @@ +package us.ihmc.robotics.time; + +import us.ihmc.commons.MathTools; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; + +@SuppressWarnings("unchecked") +public class TimeIntervalTools +{ + + public static boolean areTimeIntervalsConsecutive(TimeIntervalProvider intervalA, TimeIntervalProvider intervalB) + { + return areTimeIntervalsConsecutive(intervalA, intervalB, 5e-3); + } + + public static boolean areTimeIntervalsConsecutive(TimeIntervalProvider intervalA, TimeIntervalProvider intervalB, double epsilon) + { + return areTimeIntervalsConsecutive(intervalA.getTimeInterval(), intervalB.getTimeInterval(), epsilon); + } + + public static boolean areTimeIntervalsConsecutive(TimeIntervalReadOnly intervalA, TimeIntervalReadOnly intervalB, double epsilon) + { + return MathTools.epsilonEquals(intervalA.getEndTime(), intervalB.getStartTime(), epsilon); + } + + public static boolean isTimeSequenceContinuous(List contactStateSequence) + { + return isTimeSequenceContinuous(contactStateSequence, 5e-3); + } + + public static boolean isTimeSequenceContinuous(List contactStateSequence, double epsilon) + { + for (int index = 0; index < contactStateSequence.size() - 1; index++) + { + if (!areTimeIntervalsConsecutive(contactStateSequence.get(index), contactStateSequence.get(index + 1), epsilon)) + return false; + } + + return true; + } + + public static boolean doIntervalsOverlap(TimeIntervalReadOnly intervalA, TimeIntervalReadOnly intervalB) + { + if (intervalA.intervalContains(intervalB.getStartTime())) + return true; + + if (intervalA.intervalContains(intervalB.getEndTime())) + return true; + + if (intervalB.intervalContains(intervalA.getStartTime())) + return true; + + return intervalB.intervalContains(intervalA.getEndTime()); + } + + static public void sortByStartTime(List timeIntervalProviders) + { + sort((List) timeIntervalProviders, startTimeComparator); + } + + static public void sortByReverseStartTime(List timeIntervalProviders) + { + sort((List) timeIntervalProviders, startTimeComparator.reversed()); + } + + static public void sortByEndTime(List timeIntervalProviders) + { + sort((List) timeIntervalProviders, endTimeComparator); + } + + static public void sortByReverseEndTime(List timeIntervalProviders) + { + sort((List) timeIntervalProviders, endTimeComparator.reversed()); + } + + public static void sort(List ts, Comparator comparator) + { + boolean ordered = false; + + while (!ordered) + { + ordered = true; + for (int i = 0; i < ts.size() - 1; i++) + { + T a = ts.get(i); + T b = ts.get(i + 1); + + if (comparator.compare(a, b) > 0) + { + ordered = false; + swap(ts, i, i + 1); + } + } + } + } + + private static void swap(List ts, int a, int b) + { + T tmp = ts.get(a); + ts.set(a, ts.get(b)); + ts.set(b, tmp); + } + + static public void removeStartTimesLessThan(double time, List timeIntervalProviders) + { + for (int i = 0; i < timeIntervalProviders.size(); i++) + { + if (timeIntervalProviders.get(i).getTimeInterval().getStartTime() < time) + { + timeIntervalProviders.remove(i); + i--; + } + } + } + + static public void removeStartTimesLessThanOrEqualTo(double time, List timeIntervalProviders) + { + for (int i = 0; i < timeIntervalProviders.size(); i++) + { + if (timeIntervalProviders.get(i).getTimeInterval().getStartTime() <= time) + { + timeIntervalProviders.remove(i); + i--; + } + } + } + + static public void removeStartTimesGreaterThan(double time, List timeIntervalProviders) + { + for (int i = timeIntervalProviders.size() - 1; i >= 0; i--) + { + if (timeIntervalProviders.get(i).getTimeInterval().getStartTime() > time) + { + timeIntervalProviders.remove(i); + } + } + } + + static public void removeStartTimesGreaterThanOrEqualTo(double time, List timeIntervalProviders) + { + for (int i = timeIntervalProviders.size() - 1; i >= 0; i--) + { + if (timeIntervalProviders.get(i).getTimeInterval().getStartTime() >= time) + { + timeIntervalProviders.remove(i); + } + } + } + + static public void removeEndTimesLessThan(double time, List timeIntervalProviders) + { + for (int i = 0; i < timeIntervalProviders.size(); i++) + { + if (timeIntervalProviders.get(i).getTimeInterval().getEndTime() < time) + { + timeIntervalProviders.remove(i); + i--; + } + } + } + + static public void removeEndTimesLessThanOrEqualTo(double time, List timeIntervalProviders) + { + for (int i = 0; i < timeIntervalProviders.size(); i++) + { + if (timeIntervalProviders.get(i).getTimeInterval().getEndTime() <= time) + { + timeIntervalProviders.remove(i); + i--; + } + } + } + + static public void removeEndTimesGreaterThan(double time, List timeIntervalProviders) + { + for (int i = timeIntervalProviders.size() - 1; i >= 0; i--) + { + if (timeIntervalProviders.get(i).getTimeInterval().getEndTime() > time) + { + timeIntervalProviders.remove(i); + } + } + } + + static public void removeEndTimesGreaterThanOrEqualTo(double time, List timeIntervalProviders) + { + for (int i = timeIntervalProviders.size() - 1; i >= 0; i--) + { + if (timeIntervalProviders.get(i).getTimeInterval().getEndTime() >= time) + { + timeIntervalProviders.remove(i); + } + } + } + + static public List removeAndReturnEndTimesLessThan(double time, List timeIntervalProviders) + { + List timeIntervalProvidersToRemove = new ArrayList<>(); + + for (int i = 0; i < timeIntervalProviders.size(); i++) + { + if (timeIntervalProviders.get(i).getTimeInterval().getEndTime() < time) + { + timeIntervalProvidersToRemove.add(timeIntervalProviders.get(i)); + } + } + + return timeIntervalProvidersToRemove; + } + + static public List getIntervalsContainingTime(double time, List timeIntervalProviders) + { + List timeIntervalProvidersToRemove = new ArrayList<>(); + + for (int i = 0; i < timeIntervalProviders.size(); i++) + { + if (timeIntervalProviders.get(i).getTimeInterval().intervalContains(time)) + { + timeIntervalProvidersToRemove.add(timeIntervalProviders.get(i)); + } + } + + return timeIntervalProvidersToRemove; + } + + public static Comparator startTimeComparator = (TimeIntervalProvider a, TimeIntervalProvider b) -> { + double startTimeA = a.getTimeInterval().getStartTime(); + double startTimeB = b.getTimeInterval().getStartTime(); + return Double.compare(startTimeA, startTimeB); + }; + + public static Comparator endTimeComparator = (TimeIntervalProvider a, TimeIntervalProvider b) -> { + double endTimeA = a.getTimeInterval().getEndTime(); + double endTimeB = b.getTimeInterval().getEndTime(); + return Double.compare(endTimeA, endTimeB); + }; +} diff --git a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeIntervalTest.java b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeIntervalTest.java new file mode 100644 index 00000000000..a81641d6bfc --- /dev/null +++ b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeIntervalTest.java @@ -0,0 +1,50 @@ +package us.ihmc.robotics.time; + +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.Tag; +import org.junit.jupiter.api.Disabled; +import us.ihmc.robotics.time.TimeInterval; + +import static us.ihmc.robotics.Assert.*; + +import static us.ihmc.robotics.Assert.*; + +public class TimeIntervalTest +{ + @Test + public void testAccessors() + { + double epsilon = 1e-6; + + TimeInterval timeInterval = new TimeInterval(0.0, 1.0); + assertEquals(timeInterval.getStartTime(), 0.0, epsilon); + assertEquals(timeInterval.getEndTime(), 1.0, epsilon); + assertEquals(timeInterval.getDuration(), 1.0, epsilon); + + timeInterval.setInterval(2.0, 3.0); + assertEquals(timeInterval.getStartTime(), 2.0, epsilon); + assertEquals(timeInterval.getEndTime(), 3.0, epsilon); + assertEquals(timeInterval.getDuration(), 1.0, epsilon); + + timeInterval.setStartTime(4.0); + timeInterval.setEndTime(5.0); + assertEquals(timeInterval.getStartTime(), 4.0, epsilon); + assertEquals(timeInterval.getEndTime(), 5.0, epsilon); + assertEquals(timeInterval.getDuration(), 1.0, epsilon); + + TimeInterval other = new TimeInterval(6.0, 7.0); + timeInterval.set(other); + assertTrue(timeInterval.epsilonEquals(other, epsilon)); + } + + @Test + public void testShiftInterval() + { + double epsilon = 1e-6; + + TimeInterval timeInterval = new TimeInterval(0.0, 1.0); + timeInterval.shiftInterval(10.0); + assertEquals(timeInterval.getStartTime(), 10.0, epsilon); + assertEquals(timeInterval.getEndTime(), 11.0, epsilon); + } +} diff --git a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeIntervalToolsTest.java b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeIntervalToolsTest.java new file mode 100644 index 00000000000..a5c674ed692 --- /dev/null +++ b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeIntervalToolsTest.java @@ -0,0 +1,98 @@ +package us.ihmc.robotics.time; + +import us.ihmc.robotics.Assert; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.Tag; +import org.junit.jupiter.api.Disabled; +import us.ihmc.robotics.time.TimeInterval; +import us.ihmc.robotics.time.TimeIntervalTools; + +import java.util.ArrayList; + +import static us.ihmc.robotics.Assert.*; + +public class TimeIntervalToolsTest +{ + @Test + public void testSortMethods() + { + double epsilon = 1e-6; + + int size = 10; + ArrayList arrayValues = new ArrayList<>(size); + + for (int i = 0; i < size; i++) + { + TimedValue tv = new TimedValue(i, new TimeInterval(i, i + 1)); + arrayValues.add(tv); + } + + TimeIntervalTools.sortByReverseStartTime(arrayValues); + for (int i = 0; i < size; i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), size - 1 - i, epsilon); + } + + TimeIntervalTools.sortByStartTime(arrayValues); + for (int i = 0; i < size; i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), i, epsilon); + } + + TimeIntervalTools.sortByReverseEndTime(arrayValues); + for (int i = 0; i < size; i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), size - 1 - i, epsilon); + } + + + TimeIntervalTools.sortByEndTime(arrayValues); + for (int i = 0; i < arrayValues.size(); i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), i, epsilon); + } + } + + @Test + public void testRemoveMethods() + { + double epsilon = 1e-6; + + int size = 10; + ArrayList arrayValues = new ArrayList<>(size); + + for (int i = 0; i < size; i++) + { + TimedValue tv = new TimedValue(i, new TimeInterval(i, i + 1)); + arrayValues.add(tv); + } + + TimeIntervalTools.removeEndTimesGreaterThan(8.5, arrayValues); + size = 8; + assertEquals(arrayValues.size(), size); + for (int i = 0; i < size; i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), i, epsilon); + } + + TimeIntervalTools.removeStartTimesGreaterThan(6.5, arrayValues); + size = 7; + assertEquals(arrayValues.size(), size); + for (int i = 0; i < size; i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), i, epsilon); + } + + TimeIntervalTools.removeStartTimesLessThan(1.5, arrayValues); + size = 5; + assertEquals(arrayValues.size(), size); + for (int i = 0; i < size; i++) + { + Assert.assertEquals(arrayValues.get(i).getValue(), i + 2, epsilon); + } + + TimeIntervalTools.removeEndTimesLessThan(6.5, arrayValues); + assertEquals(arrayValues.size(), 1); + Assert.assertEquals(arrayValues.get(0).getValue(), 6, epsilon); + } +} diff --git a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimedValue.java b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimedValue.java new file mode 100644 index 00000000000..9219bd88181 --- /dev/null +++ b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimedValue.java @@ -0,0 +1,36 @@ +package us.ihmc.robotics.time; + +public class TimedValue implements TimeIntervalProvider +{ + private int value; + private final TimeInterval timeInterval; + + public TimedValue() + { + this.value = 0; + this.timeInterval = new TimeInterval(); + } + + public TimedValue(int value, TimeInterval timeInterval) + { + this.value = value; + this.timeInterval = new TimeInterval(timeInterval); + } + + public void set(TimedValue other) + { + this.value = other.value; + this.timeInterval.set(other.timeInterval); + } + + public int getValue() + { + return value; + } + + @Override + public TimeInterval getTimeInterval() + { + return timeInterval; + } +}