From 8f26714b7b436379f7b0ef4b3cf6480b5cfb02c7 Mon Sep 17 00:00:00 2001 From: lpenco Date: Thu, 12 Dec 2024 11:41:08 -0600 Subject: [PATCH 1/5] extracted from Coen's work, no reference spreading stuff --- .../WalkingControllerParameters.java | 20 + .../rigidBody/RigidBodyControlManager.java | 81 +++- .../RigidBodyOrientationControlHelper.java | 79 +++- .../RigidBodyOrientationController.java | 2 + .../rigidBody/RigidBodyPoseController.java | 139 ++++++ .../RigidBodyPositionControlHelper.java | 78 ++- .../RigidBodyPositionController.java | 2 + .../RigidBodyTaskspaceControlState.java | 23 + .../FeedbackControllerToolbox.java | 27 ++ .../OrientationFeedbackControlCommand.java | 23 + .../PointFeedbackControlCommand.java | 23 + .../SpatialFeedbackControlCommand.java | 24 +- .../HighLevelControlManagerFactory.java | 9 + .../ManipulationControllerState.java | 18 +- .../WalkingCommandConsumer.java | 32 +- .../FeedbackControllerInterface.java | 8 + .../OrientationFeedbackController.java | 140 ++++++ .../taskspace/PointFeedbackController.java | 155 ++++++ .../taskspace/SpatialFeedbackController.java | 300 +++++++++++- .../RigidBodyControlManagerTest.java | 6 + ...anceOrientationFeedbackControllerTest.java | 140 ++++++ .../ImpedancePointFeedbackControllerTest.java | 446 ++++++++++++++++++ ...mpedanceSpatialFeedbackControllerTest.java | 375 +++++++++++++++ ...dJointspaceTaskspaceTrajectoryCommand.java | 57 ++- ...E3PIDGainsTrajectoryControllerCommand.java | 160 +++++++ ...idJointspaceTaskspaceTrajectoryMessage.msg | 6 +- .../controller_msgs/msg/PID3DGains.msg | 11 + .../controller_msgs/msg/PIDGains.msg | 6 + .../msg/SE3PIDGainsTrajectoryMessage.msg | 10 + .../msg/SE3PIDGainsTrajectoryPointMessage.msg | 13 + .../us/ihmc/robotics/MatrixMissingTools.java | 198 ++++++++ .../controllers/pidGains/PID3DGains.java | 41 ++ .../pidGains/PID3DGainsReadOnly.java | 35 +- .../implementations/DefaultPID3DGains.java | 6 + .../implementations/DefaultYoPID3DGains.java | 7 + .../implementations/DefaultYoPIDSE3Gains.java | 6 + .../ParameterizedPID3DGains.java | 7 + .../implementations/SymmetricPID3DGains.java | 6 + .../implementations/ZeroablePID3DGains.java | 15 + ...ypointsOrientationTrajectoryGenerator.java | 58 ++- ...eWaypointsPositionTrajectoryGenerator.java | 10 +- .../SE3PIDGainsTrajectoryPoint.java | 67 +++ .../SE3PIDGainsTrajectoryPointBasics.java | 46 ++ .../SE3PIDGainsTrajectoryPointReadOnly.java | 24 + .../lists/SE3PIDGainsTrajectoryPointList.java | 68 +++ .../waypoints/SE3PIDGainsWaypoint.java | 42 ++ .../interfaces/SE3PIDGainsWaypointBasics.java | 51 ++ .../SE3PIDGainsWaypointReadOnly.java | 141 ++++++ .../screwTheory/SelectionMatrix6D.java | 66 +++ .../ihmc/robotics/MatrixMissingToolsTest.java | 30 ++ 50 files changed, 3316 insertions(+), 21 deletions(-) create mode 100644 ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceOrientationFeedbackControllerTest.java create mode 100644 ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedancePointFeedbackControllerTest.java create mode 100644 ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceSpatialFeedbackControllerTest.java create mode 100644 ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/SE3PIDGainsTrajectoryControllerCommand.java create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PID3DGains.msg create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PIDGains.msg create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/SE3PIDGainsTrajectoryPoint.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointBasics.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointReadOnly.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/lists/SE3PIDGainsTrajectoryPointList.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/SE3PIDGainsWaypoint.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointBasics.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointReadOnly.java diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/WalkingControllerParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/WalkingControllerParameters.java index 3a08c9e1b8b..59f7db25bc9 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/WalkingControllerParameters.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/WalkingControllerParameters.java @@ -18,6 +18,7 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.OneDoFJointPrivilegedConfigurationParameters; import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly; import us.ihmc.robotics.controllers.pidGains.PIDGainsReadOnly; import us.ihmc.robotics.controllers.pidGains.implementations.PDGains; import us.ihmc.robotics.controllers.pidGains.implementations.PID3DConfiguration; @@ -236,6 +237,16 @@ public List> getTaskspacePositionControlGains return new ArrayList<>(); } + public PID3DGainsReadOnly getImpedanceHandPositionControlGains() + { + return null; + } + + public PID3DGainsReadOnly getImpedanceHandOrientationControlGains() + { + return null; + } + /** * Returns a map with default control modes for each rigid body. *

@@ -272,6 +283,15 @@ public boolean enableFunctionGeneratorMode(String rigidBodyName) return false; } + /** + * If true, the rigid body spatial control state for the given rigid body will be setup + * with impedance control + */ + public boolean enableImpedanceControl(String rigidBodyName) + { + return false; + } + /** * The map returned contains the default rigid body poses in their respective base frame. For * example, if the base frame of the chest body is the pelvis z-up frame this should contain the diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java index 7fa86227667..359c12358c3 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java @@ -91,10 +91,13 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl, Vector3DReadOnly taskspaceLinearWeight, PID3DGainsReadOnly taskspaceOrientationGains, PID3DGainsReadOnly taskspacePositionGains, + PID3DGainsReadOnly taskspaceOrientationImpedanceGains, + PID3DGainsReadOnly taskspacePositionImpedanceGains, ContactablePlaneBody contactableBody, LoadBearingParameters loadBearingParameters, RigidBodyControlMode defaultControlMode, boolean enableFunctionGenerators, + YoBoolean isImpedanceEnabled, double nominalRhoWeight, WholeBodyPostureAdjustmentProvider postureAdjustmentProvider, YoDouble yoTime, @@ -129,6 +132,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl, yoTime, jointControlHelper, enableFunctionGenerators, + isImpedanceEnabled, parentRegistry); if (taskspaceOrientationGains == null) { @@ -137,7 +141,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl, taskspaceControlState.setGains(taskspaceOrientationGains); taskspaceControlState.setWeights(taskspaceAngularWeight); this.taskspaceControlState = taskspaceControlState; - LogTools.info("Creating manager for " + bodyName + " with orientation controller."); + LogTools.info("Creating manager for " + bodyName + " with orientation controller. (Impedance enabled: " + isImpedanceEnabled.getValue() + ")"); } else if (taskspaceAngularWeight == null && taskspaceLinearWeight != null) { @@ -148,6 +152,7 @@ else if (taskspaceAngularWeight == null && taskspaceLinearWeight != null) baseFrame, yoTime, enableFunctionGenerators, + isImpedanceEnabled, parentRegistry, graphicsListRegistry); if (taskspacePositionGains == null) @@ -157,7 +162,7 @@ else if (taskspaceAngularWeight == null && taskspaceLinearWeight != null) taskspaceControlState.setGains(taskspacePositionGains); taskspaceControlState.setWeights(taskspaceLinearWeight); this.taskspaceControlState = taskspaceControlState; - LogTools.info("Creating manager for " + bodyName + " with position controller."); + LogTools.info("Creating manager for " + bodyName + " with position controller. (Impedance enabled: " + isImpedanceEnabled.getValue() + ")"); } else if (taskspaceAngularWeight != null && taskspaceLinearWeight != null) { @@ -169,6 +174,7 @@ else if (taskspaceAngularWeight != null && taskspaceLinearWeight != null) yoTime, jointControlHelper, enableFunctionGenerators, + isImpedanceEnabled, graphicsListRegistry, registry); if (taskspaceOrientationGains == null || taskspacePositionGains == null) @@ -179,8 +185,10 @@ else if (taskspaceAngularWeight != null && taskspaceLinearWeight != null) } taskspaceControlState.setGains(taskspaceOrientationGains, taskspacePositionGains); taskspaceControlState.setWeights(taskspaceAngularWeight, taskspaceLinearWeight); + if (taskspaceOrientationImpedanceGains != null && taskspacePositionImpedanceGains != null) + taskspaceControlState.setImpedanceGains(taskspaceOrientationImpedanceGains, taskspacePositionImpedanceGains); this.taskspaceControlState = taskspaceControlState; - LogTools.info("Creating manager for " + bodyName + " with pose controller."); + LogTools.info("Creating manager for " + bodyName + " with pose controller. (Impedance enabled: " + isImpedanceEnabled.getValue() + ")"); } else { @@ -396,6 +404,73 @@ public void handleHybridTrajectoryCommand(SO3TrajectoryControllerCommand taskspa } } + public void handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand taskspaceCommand, + JointspaceTrajectoryCommand jointSpaceCommand, + WrenchTrajectoryControllerCommand feedForwardCommand) + { + computeDesiredJointPositions(initialJointPositions); + + if (stateMachine.getCurrentStateKey() == RigidBodyControlMode.LOADBEARING) + { // If in LOADBEARING mode, execute the trajectory in that state + loadBearingControlState.handleAsOrientationTrajectoryCommand(taskspaceCommand); + loadBearingControlState.handleJointTrajectoryCommand(jointSpaceCommand, initialJointPositions); + } + else if (taskspaceControlState.handleHybridTrajectoryCommand(taskspaceCommand, jointSpaceCommand, feedForwardCommand, initialJointPositions)) + { // Otherwise execute in TASKSPACE mode + requestState(taskspaceControlState.getControlMode()); + } + else + { + LogTools.warn(getClass().getSimpleName() + " for " + bodyName + " received invalid hybrid SE3 trajectory command."); + hold(); + } + } + + public void handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand taskspaceCommand, + JointspaceTrajectoryCommand jointSpaceCommand, + SE3PIDGainsTrajectoryControllerCommand gainsCommand) + { + computeDesiredJointPositions(initialJointPositions); + + if (stateMachine.getCurrentStateKey() == RigidBodyControlMode.LOADBEARING) + { // If in LOADBEARING mode, execute the trajectory in that state + loadBearingControlState.handleAsOrientationTrajectoryCommand(taskspaceCommand); + loadBearingControlState.handleJointTrajectoryCommand(jointSpaceCommand, initialJointPositions); + } + else if (taskspaceControlState.handleHybridTrajectoryCommand(taskspaceCommand, jointSpaceCommand, gainsCommand, initialJointPositions)) + { // Otherwise execute in TASKSPACE mode + requestState(taskspaceControlState.getControlMode()); + } + else + { + LogTools.warn(getClass().getSimpleName() + " for " + bodyName + " received invalid hybrid SE3 trajectory command."); + hold(); + } + } + + public void handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand taskspaceCommand, + JointspaceTrajectoryCommand jointSpaceCommand, + WrenchTrajectoryControllerCommand feedForwardCommand, + SE3PIDGainsTrajectoryControllerCommand gainsCommand) + { + computeDesiredJointPositions(initialJointPositions); + + if (stateMachine.getCurrentStateKey() == RigidBodyControlMode.LOADBEARING) + { // If in LOADBEARING mode, execute the trajectory in that state + loadBearingControlState.handleAsOrientationTrajectoryCommand(taskspaceCommand); + loadBearingControlState.handleJointTrajectoryCommand(jointSpaceCommand, initialJointPositions); + } + else if (taskspaceControlState.handleHybridTrajectoryCommand(taskspaceCommand, jointSpaceCommand, feedForwardCommand, gainsCommand, initialJointPositions)) + { // Otherwise execute in TASKSPACE mode + requestState(taskspaceControlState.getControlMode()); + } + else + { + LogTools.warn(getClass().getSimpleName() + " for " + bodyName + " received invalid hybrid SE3 trajectory command."); + hold(); + } + } + public void handleDesiredAccelerationsCommand(DesiredAccelerationsCommand command) { if (userControlState.handleDesiredAccelerationsCommand(command)) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationControlHelper.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationControlHelper.java index 8b3837b50da..aba497a7efd 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationControlHelper.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationControlHelper.java @@ -1,8 +1,10 @@ package us.ihmc.commonWalkingControlModules.controlModules.rigidBody; +import gnu.trove.list.array.TDoubleArrayList; import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.OrientationFeedbackControlCommand; import us.ihmc.commons.Conversions; import us.ihmc.commons.lists.RecyclingArrayDeque; +import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.communication.packets.ExecutionMode; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; @@ -19,11 +21,13 @@ import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand; import us.ihmc.log.LogTools; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.SpatialVector; import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly; import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorMode; import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorNew; import us.ihmc.robotics.math.trajectories.generators.MultipleWaypointsOrientationTrajectoryGenerator; import us.ihmc.robotics.math.trajectories.trajectorypoints.FrameSO3TrajectoryPoint; +import us.ihmc.robotics.math.trajectories.trajectorypoints.SE3PIDGainsTrajectoryPoint; import us.ihmc.robotics.math.trajectories.trajectorypoints.lists.FrameSO3TrajectoryPointList; import us.ihmc.robotics.screwTheory.SelectionMatrix3D; import us.ihmc.robotics.weightMatrices.WeightMatrix3D; @@ -31,6 +35,7 @@ import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoEnum; @@ -79,12 +84,19 @@ public class RigidBodyOrientationControlHelper private Vector3DReadOnly defaultWeight; private PID3DGainsReadOnly gains; + private PID3DGainsReadOnly impedanceGains; private final FrameQuaternion desiredOrientation = new FrameQuaternion(); private final FrameVector3D desiredVelocity = new FrameVector3D(); private final FrameVector3D feedForwardAcceleration = new FrameVector3D(); + TDoubleArrayList feedForwardTrajectoryTimes = new TDoubleArrayList(); + RecyclingArrayList feedForwardTrajectoryList = new RecyclingArrayList<>(200, SpatialVector.class); + RecyclingArrayList gainsTrajectoryPoints; + private final BooleanProvider useBaseFrameForControl; + private final YoBoolean isImpedanceEnabled; + private final YoBoolean isFeedforwardEnabled; private final FixedFrameQuaternionBasics previousControlFrameOrientation; private final FixedFrameQuaternionBasics controlFrameOrientation; @@ -109,6 +121,7 @@ public RigidBodyOrientationControlHelper(String warningPrefix, BooleanProvider useBaseFrameForControl, BooleanProvider useWeightFromMessage, boolean enableFunctionGenerators, + YoBoolean enableImpedanceControl, DoubleProvider time, YoRegistry registry) { @@ -131,6 +144,12 @@ public RigidBodyOrientationControlHelper(String warningPrefix, feedbackControlCommand.set(elevator, bodyToControl); feedbackControlCommand.setPrimaryBase(baseBody); + feedbackControlCommand.setImpedanceEnabled(enableImpedanceControl.getBooleanValue()); + isImpedanceEnabled = enableImpedanceControl; + isFeedforwardEnabled = new YoBoolean(prefix + "FeedforwardEnabled", registry); + isFeedforwardEnabled.set(true); + + gainsTrajectoryPoints = new RecyclingArrayList<>(200, SE3PIDGainsTrajectoryPoint.class); defaultControlFrame = controlFrame; bodyFrame = bodyToControl.getBodyFixedFrame(); @@ -165,6 +184,16 @@ public PID3DGainsReadOnly getGains() return gains; } + public void setImpedanceGains(PID3DGainsReadOnly impedanceGains) + { + this.impedanceGains = impedanceGains; + } + + public PID3DGainsReadOnly getImpedanceGains() + { + return impedanceGains; + } + public void setWeights(Vector3DReadOnly weights) { this.defaultWeight = weights; @@ -268,12 +297,45 @@ public boolean doAction(double timeInTrajectory) trajectoryGenerator.getAngularData(desiredOrientation, desiredVelocity, feedForwardAcceleration); updateFunctionGenerators(); + if (!feedForwardTrajectoryList.isEmpty()) + { + feedForwardAcceleration.set(feedForwardTrajectoryList.get(Math.max( + feedForwardTrajectoryList.size() - pointQueue.size() - trajectoryGenerator.getCurrentNumberOfWaypoints() + + trajectoryGenerator.getCurrentWaypointIndex(), 0)).getAngularPart()); + } + + if (!gainsTrajectoryPoints.isEmpty()) + { + gains = gainsTrajectoryPoints.get(Math.max(gainsTrajectoryPoints.size() - pointQueue.size() - trajectoryGenerator.getCurrentNumberOfWaypoints() + + trajectoryGenerator.getCurrentWaypointIndex(), 0)).getAngular(); + feedbackControlCommand.setGains(gains); + } + else if (!isImpedanceEnabled.getBooleanValue()) + { + feedbackControlCommand.setGains(gains); + } + else + { + if (impedanceGains != null) + { + feedbackControlCommand.setGains(impedanceGains); + } + else + { + isImpedanceEnabled.set(false); + LogTools.warn(warningPrefix + "Impedance gains are not set, impedance control is disabled."); + } + } + desiredOrientation.changeFrame(ReferenceFrame.getWorldFrame()); desiredVelocity.changeFrame(ReferenceFrame.getWorldFrame()); feedForwardAcceleration.changeFrame(ReferenceFrame.getWorldFrame()); + if (!isFeedforwardEnabled.getBooleanValue()) + feedForwardAcceleration.set(0.0, 0.0, 0.0); + feedbackControlCommand.setInverseDynamics(desiredOrientation, desiredVelocity, feedForwardAcceleration); - feedbackControlCommand.setGains(gains); + feedbackControlCommand.setImpedanceEnabled(isImpedanceEnabled.getBooleanValue()); // This will improve the tracking with respect to moving trajectory frames. if (useBaseFrameForControl.getValue()) @@ -620,6 +682,21 @@ else if (pointQueue.isEmpty()) } } + public TDoubleArrayList getFeedForwardTrajectoryTimes() + { + return feedForwardTrajectoryTimes; + } + + public RecyclingArrayList getFeedForwardTrajectoryList() + { + return feedForwardTrajectoryList; + } + + public RecyclingArrayList getGainsTrajectoryPoints() + { + return gainsTrajectoryPoints; + } + public void clear() { selectionMatrix.resetSelection(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java index bd782f4d6cd..331df4cf678 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java @@ -56,6 +56,7 @@ public RigidBodyOrientationController(RigidBodyBasics bodyToControl, YoDouble yoTime, RigidBodyJointControlHelper jointControlHelper, boolean enableFunctionGenerators, + YoBoolean enableImpedanceControl, YoRegistry parentRegistry) { super(RigidBodyControlMode.TASKSPACE, bodyToControl.getName(), yoTime, parentRegistry); @@ -80,6 +81,7 @@ public RigidBodyOrientationController(RigidBodyBasics bodyToControl, useBaseFrameForControl, usingWeightFromMessage, enableFunctionGenerators, + enableImpedanceControl, yoTime, registry); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java index 0817c810a15..cc2ffca142d 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java @@ -62,6 +62,7 @@ public class RigidBodyPoseController extends RigidBodyTaskspaceControlState private final RigidBodyOrientationControlHelper orientationHelper; private final YoBoolean hybridModeActive; + private final YoBoolean isImpedanceEnabled; private final RigidBodyJointControlHelper jointControlHelper; private final TaskspaceTrajectoryStatusMessageHelper statusHelper; @@ -74,6 +75,7 @@ public RigidBodyPoseController(RigidBodyBasics bodyToControl, YoDouble yoTime, RigidBodyJointControlHelper jointControlHelper, boolean enableFunctionGenerators, + YoBoolean enableImpedanceControl, YoGraphicsListRegistry graphicsListRegistry, YoRegistry parentRegistry) { @@ -96,6 +98,7 @@ public RigidBodyPoseController(RigidBodyBasics bodyToControl, useBaseFrameForControl, usingWeightFromMessage, enableFunctionGenerators, + enableImpedanceControl, yoTime, registry, graphicsListRegistry); @@ -108,6 +111,7 @@ public RigidBodyPoseController(RigidBodyBasics bodyToControl, useBaseFrameForControl, usingWeightFromMessage, enableFunctionGenerators, + enableImpedanceControl, yoTime, registry); @@ -119,6 +123,8 @@ public RigidBodyPoseController(RigidBodyBasics bodyToControl, feedbackControlCommand.set(elevator, bodyToControl); feedbackControlCommand.setPrimaryBase(baseBody); feedbackControlCommand.setSelectionMatrixToIdentity(); + feedbackControlCommand.setImpedanceEnabled(enableImpedanceControl.getBooleanValue()); + isImpedanceEnabled = enableImpedanceControl; } public void setWeights(Vector3DReadOnly angularWeight, Vector3DReadOnly linearWeight) @@ -153,6 +159,22 @@ public PID3DGainsReadOnly getPositionGains() return positionHelper.getGains(); } + public void setImpedanceGains(PID3DGainsReadOnly orientationGains, PID3DGainsReadOnly positionGains) + { + positionHelper.setImpedanceGains(positionGains); + orientationHelper.setImpedanceGains(orientationGains); + } + + public PID3DGainsReadOnly getOrientationImpedanceGains() + { + return orientationHelper.getImpedanceGains(); + } + + public PID3DGainsReadOnly getPositionImpedanceGains() + { + return positionHelper.getImpedanceGains(); + } + @Override public void doAction(double timeInState) { @@ -207,6 +229,7 @@ private void updateCommand() orientationCommand.getControlFrameOrientation()); feedbackControlCommand.setControlBaseFrame(positionCommand.getControlBaseFrame()); + feedbackControlCommand.setImpedanceEnabled(isImpedanceEnabled.getBooleanValue()); } @Override @@ -312,6 +335,13 @@ public boolean handleTrajectoryCommand(SE3TrajectoryControllerCommand command) hybridModeActive.set(false); if (command.getExecutionMode() != ExecutionMode.STREAM) statusHelper.registerNewTrajectory(command); + + positionHelper.getGainsTrajectoryPoints().clear(); + orientationHelper.getGainsTrajectoryPoints().clear(); + positionHelper.getFeedForwardTrajectoryList().clear(); + positionHelper.getFeedForwardTrajectoryTimes().clear(); + orientationHelper.getFeedForwardTrajectoryList().clear(); + orientationHelper.getFeedForwardTrajectoryTimes().clear(); return true; } @@ -330,6 +360,115 @@ public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand comm { hybridModeActive.set(true); statusHelper.registerNewTrajectory(command); + + positionHelper.getGainsTrajectoryPoints().clear(); + orientationHelper.getGainsTrajectoryPoints().clear(); + positionHelper.getFeedForwardTrajectoryList().clear(); + positionHelper.getFeedForwardTrajectoryTimes().clear(); + orientationHelper.getFeedForwardTrajectoryList().clear(); + orientationHelper.getFeedForwardTrajectoryTimes().clear(); + + return true; + } + + clear(); + positionHelper.clear(); + orientationHelper.clear(); + return false; + } + + public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand command, + JointspaceTrajectoryCommand jointspaceCommand, + WrenchTrajectoryControllerCommand feedForwardCommand, + double[] initialJointPositions) + { + if (handleTrajectoryCommand(command) && jointControlHelper.handleTrajectoryCommand(jointspaceCommand, initialJointPositions)) + { + hybridModeActive.set(true); + statusHelper.registerNewTrajectory(command); + + positionHelper.getGainsTrajectoryPoints().clear(); + orientationHelper.getGainsTrajectoryPoints().clear(); + positionHelper.getFeedForwardTrajectoryList().clear(); + positionHelper.getFeedForwardTrajectoryTimes().clear(); + orientationHelper.getFeedForwardTrajectoryList().clear(); + orientationHelper.getFeedForwardTrajectoryTimes().clear(); + + for (int i = 0; i < feedForwardCommand.getNumberOfTrajectoryPoints(); i++) + { + positionHelper.getFeedForwardTrajectoryList().add().set(feedForwardCommand.getTrajectoryPoint(i)); + positionHelper.getFeedForwardTrajectoryTimes().add(feedForwardCommand.getTrajectoryPointTime(i)); + orientationHelper.getFeedForwardTrajectoryList().add().set(feedForwardCommand.getTrajectoryPoint(i)); + orientationHelper.getFeedForwardTrajectoryTimes().add(feedForwardCommand.getTrajectoryPointTime(i)); + } + return true; + } + + clear(); + positionHelper.clear(); + orientationHelper.clear(); + return false; + } + + public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand command, + JointspaceTrajectoryCommand jointspaceCommand, + SE3PIDGainsTrajectoryControllerCommand gainsCommand, + double[] initialJointPositions) + { + if (handleTrajectoryCommand(command) && jointControlHelper.handleTrajectoryCommand(jointspaceCommand, initialJointPositions)) + { + hybridModeActive.set(true); + statusHelper.registerNewTrajectory(command); + + positionHelper.getGainsTrajectoryPoints().clear(); + orientationHelper.getGainsTrajectoryPoints().clear(); + positionHelper.getFeedForwardTrajectoryList().clear(); + positionHelper.getFeedForwardTrajectoryTimes().clear(); + orientationHelper.getFeedForwardTrajectoryList().clear(); + orientationHelper.getFeedForwardTrajectoryTimes().clear(); + + for (int i = 0; i < gainsCommand.getNumberOfTrajectoryPoints(); i++) + { + positionHelper.getGainsTrajectoryPoints().add().set(gainsCommand.getTrajectoryPoint(i)); + orientationHelper.getGainsTrajectoryPoints().add().set(gainsCommand.getTrajectoryPoint(i)); + } + return true; + } + + clear(); + positionHelper.clear(); + orientationHelper.clear(); + return false; + } + + public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand command, + JointspaceTrajectoryCommand jointspaceCommand, + WrenchTrajectoryControllerCommand feedForwardCommand, + SE3PIDGainsTrajectoryControllerCommand gainsCommand, + double[] initialJointPositions) + { + if (handleTrajectoryCommand(command) && jointControlHelper.handleTrajectoryCommand(jointspaceCommand, initialJointPositions)) + { + hybridModeActive.set(true); + statusHelper.registerNewTrajectory(command); + + positionHelper.getGainsTrajectoryPoints().clear(); + orientationHelper.getGainsTrajectoryPoints().clear(); + positionHelper.getFeedForwardTrajectoryList().clear(); + positionHelper.getFeedForwardTrajectoryTimes().clear(); + orientationHelper.getFeedForwardTrajectoryList().clear(); + orientationHelper.getFeedForwardTrajectoryTimes().clear(); + + for (int i = 0; i < feedForwardCommand.getNumberOfTrajectoryPoints(); i++) + { + positionHelper.getFeedForwardTrajectoryList().add().set(feedForwardCommand.getTrajectoryPoint(i)); + positionHelper.getFeedForwardTrajectoryTimes().add(feedForwardCommand.getTrajectoryPointTime(i)); + orientationHelper.getFeedForwardTrajectoryList().add().set(feedForwardCommand.getTrajectoryPoint(i)); + orientationHelper.getFeedForwardTrajectoryTimes().add(feedForwardCommand.getTrajectoryPointTime(i)); + + positionHelper.getGainsTrajectoryPoints().add().set(gainsCommand.getTrajectoryPoint(i)); + orientationHelper.getGainsTrajectoryPoints().add().set(gainsCommand.getTrajectoryPoint(i)); + } return true; } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionControlHelper.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionControlHelper.java index 23d9d67cea6..a044380d46b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionControlHelper.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionControlHelper.java @@ -1,8 +1,10 @@ package us.ihmc.commonWalkingControlModules.controlModules.rigidBody; +import gnu.trove.list.array.TDoubleArrayList; import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.PointFeedbackControlCommand; import us.ihmc.commons.Conversions; import us.ihmc.commons.lists.RecyclingArrayDeque; +import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.communication.packets.ExecutionMode; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.referenceFrame.FramePoint3D; @@ -22,12 +24,14 @@ import us.ihmc.humanoidRobotics.communication.controllerAPI.command.EuclideanTrajectoryControllerCommand; import us.ihmc.log.LogTools; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.SpatialVector; import us.ihmc.robotics.SCS2YoGraphicHolder; import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly; import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorMode; import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorNew; import us.ihmc.robotics.math.trajectories.generators.MultipleWaypointsPositionTrajectoryGenerator; import us.ihmc.robotics.math.trajectories.trajectorypoints.FrameEuclideanTrajectoryPoint; +import us.ihmc.robotics.math.trajectories.trajectorypoints.SE3PIDGainsTrajectoryPoint; import us.ihmc.robotics.math.trajectories.trajectorypoints.lists.FrameEuclideanTrajectoryPointList; import us.ihmc.robotics.screwTheory.SelectionMatrix3D; import us.ihmc.robotics.weightMatrices.WeightMatrix3D; @@ -40,6 +44,7 @@ import us.ihmc.yoVariables.providers.BooleanProvider; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import java.util.ArrayList; @@ -88,13 +93,20 @@ public class RigidBodyPositionControlHelper implements SCS2YoGraphicHolder private Vector3DReadOnly defaultWeight; private PID3DGainsReadOnly gains; + private PID3DGainsReadOnly impedanceGains; private final List functionGenerators = new ArrayList<>(); private final FramePoint3D desiredPosition = new FramePoint3D(); private final FrameVector3D desiredVelocity = new FrameVector3D(); private final FrameVector3D feedForwardAcceleration = new FrameVector3D(); + TDoubleArrayList feedForwardTrajectoryTimes = new TDoubleArrayList(); + RecyclingArrayList feedForwardTrajectoryList = new RecyclingArrayList<>(200, SpatialVector.class); + RecyclingArrayList gainsTrajectoryPoints; + private final BooleanProvider useBaseFrameForControl; + private final YoBoolean isImpedanceEnabled; + private final YoBoolean isFeedforwardEnabled; private final RigidBodyTransform previousControlFramePose = new RigidBodyTransform(); private final RigidBodyTransform controlFramePose = new RigidBodyTransform(); @@ -123,6 +135,7 @@ public RigidBodyPositionControlHelper(String warningPrefix, BooleanProvider useBaseFrameForControl, BooleanProvider useWeightFromMessage, boolean enableFunctionGenerators, + YoBoolean enableImpedanceControl, DoubleProvider time, YoRegistry registry, YoGraphicsListRegistry graphicsListRegistry) @@ -146,6 +159,12 @@ public RigidBodyPositionControlHelper(String warningPrefix, feedbackControlCommand.set(elevator, bodyToControl); feedbackControlCommand.setPrimaryBase(baseBody); + feedbackControlCommand.setImpedanceEnabled(enableImpedanceControl.getBooleanValue()); + isImpedanceEnabled = enableImpedanceControl; + isFeedforwardEnabled = new YoBoolean(prefix + "FeedforwardEnabled", registry); + isFeedforwardEnabled.set(true); + + gainsTrajectoryPoints = new RecyclingArrayList<>(200, SE3PIDGainsTrajectoryPoint.class); defaultControlFrame = controlFrame; bodyFrame = bodyToControl.getBodyFixedFrame(); @@ -187,6 +206,16 @@ public PID3DGainsReadOnly getGains() return gains; } + public void setImpedanceGains(PID3DGainsReadOnly impedanceGains) + { + this.impedanceGains = impedanceGains; + } + + public PID3DGainsReadOnly getImpedanceGains() + { + return impedanceGains; + } + public void setWeights(Vector3DReadOnly weights) { this.defaultWeight = weights; @@ -324,12 +353,44 @@ public boolean doAction(double timeInTrajectory) trajectoryGenerator.getLinearData(desiredPosition, desiredVelocity, feedForwardAcceleration); updateFunctionGenerators(); + if (!feedForwardTrajectoryList.isEmpty()) + { + feedForwardAcceleration.set(feedForwardTrajectoryList.get(Math.max( + feedForwardTrajectoryList.size() - pointQueue.size() - trajectoryGenerator.getCurrentNumberOfWaypoints() + + trajectoryGenerator.getCurrentWaypointIndex(), 0)).getLinearPart()); + } + + if (!gainsTrajectoryPoints.isEmpty()) + { + gains = gainsTrajectoryPoints.get(Math.max(gainsTrajectoryPoints.size() - pointQueue.size() - trajectoryGenerator.getCurrentNumberOfWaypoints() + + trajectoryGenerator.getCurrentWaypointIndex(), 0)).getAngular(); + feedbackControlCommand.setGains(gains); + } + else if (!isImpedanceEnabled.getBooleanValue()) + { + feedbackControlCommand.setGains(gains); + } + else + { + if (impedanceGains != null) + { + feedbackControlCommand.setGains(impedanceGains); + } + else + { + isImpedanceEnabled.set(false); + LogTools.warn(warningPrefix + "Impedance gains are not set, impedance control is disabled."); + } + } + desiredPosition.changeFrame(ReferenceFrame.getWorldFrame()); desiredVelocity.changeFrame(ReferenceFrame.getWorldFrame()); feedForwardAcceleration.changeFrame(ReferenceFrame.getWorldFrame()); + if (!isFeedforwardEnabled.getBooleanValue()) + feedForwardAcceleration.set(0.0, 0.0, 0.0); feedbackControlCommand.setInverseDynamics(desiredPosition, desiredVelocity, feedForwardAcceleration); - feedbackControlCommand.setGains(gains); + feedbackControlCommand.setImpedanceEnabled(isImpedanceEnabled.getBooleanValue()); // This will improve the tracking with respect to moving trajectory frames. if (useBaseFrameForControl.getValue()) @@ -659,6 +720,21 @@ else if (pointQueue.isEmpty()) } } + public TDoubleArrayList getFeedForwardTrajectoryTimes() + { + return feedForwardTrajectoryTimes; + } + + public RecyclingArrayList getFeedForwardTrajectoryList() + { + return feedForwardTrajectoryList; + } + + public RecyclingArrayList getGainsTrajectoryPoints() + { + return gainsTrajectoryPoints; + } + public void clear() { selectionMatrix.resetSelection(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java index ba60321f521..8d169ef3646 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java @@ -52,6 +52,7 @@ public RigidBodyPositionController(RigidBodyBasics bodyToControl, ReferenceFrame baseFrame, YoDouble yoTime, boolean enableFunctionGenerators, + YoBoolean enableImpedanceControl, YoRegistry parentRegistry, YoGraphicsListRegistry graphicsListRegistry) { @@ -82,6 +83,7 @@ public RigidBodyPositionController(RigidBodyBasics bodyToControl, useBaseFrameForControl, usingWeightFromMessage, enableFunctionGenerators, + enableImpedanceControl, yoTime, registry, graphicsListRegistry); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java index 1e0f1cab415..265dd22921f 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java @@ -5,6 +5,7 @@ import us.ihmc.humanoidRobotics.communication.controllerAPI.command.JointspaceTrajectoryCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.WrenchTrajectoryControllerCommand; import us.ihmc.log.LogTools; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; @@ -76,4 +77,26 @@ public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand comm LogTools.warn("Handling of hybrid command " + command.getClass().getSimpleName() + " not implemented for " + getClass().getSimpleName() + "."); return false; } + + public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand command, JointspaceTrajectoryCommand jointspaceCommand, + WrenchTrajectoryControllerCommand feedForwardCommand, double[] initialJointPositions) + { + LogTools.warn("Handling of hybrid command " + command.getClass().getSimpleName() + " not implemented for " + getClass().getSimpleName() + "."); + return false; + } + + public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand command, JointspaceTrajectoryCommand jointspaceCommand, + SE3PIDGainsTrajectoryControllerCommand gainsCommand, double[] initialJointPositions) + { + LogTools.warn("Handling of hybrid command " + command.getClass().getSimpleName() + " not implemented for " + getClass().getSimpleName() + "."); + return false; + } + + public boolean handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand command, JointspaceTrajectoryCommand jointspaceCommand, + WrenchTrajectoryControllerCommand feedForwardCommand, SE3PIDGainsTrajectoryControllerCommand gainsCommand, + double[] initialJointPositions) + { + LogTools.warn("Handling of hybrid command " + command.getClass().getSimpleName() + " not implemented for " + getClass().getSimpleName() + "."); + return false; + } } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/FeedbackControllerToolbox.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/FeedbackControllerToolbox.java index a03cbcb2394..eb02291cd19 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/FeedbackControllerToolbox.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/FeedbackControllerToolbox.java @@ -773,6 +773,24 @@ public YoTranslationFrame getOrCreatePointFeedbackControlFrame(RigidBodyBasics e isRequiredVariable); } + /** + * Retrieves and returns an impedance point feedback control frame {@code YoTranslationFrame} associated to the given + * end-effector, if it does not exist it is created. + * + * @param endEffector the end-effector to which the control frame is associated. + * @param controllerIndex the index of the feedback controller requesting the data. + * @param isRequiredVariable the {@code YoVariable}s do not get attached to this registry when the + * variable is not a required variable and + * {@link WholeBodyControllerCore#REDUCE_YOVARIABLES} is set to + * {@code true}. + * @return a unique {@code YoTranslationFrame} point feedback control frame associated with the given end-effector. + */ + public YoTranslationFrame getOrCreateImpedancePointFeedbackControlFrame(RigidBodyBasics endEffector, int controllerIndex, boolean isRequiredVariable) + { + return getOrCreateEndEffectorDataPool(endEffector, controllerIndex).getOrCreateImpedancePointFeedbackControlFrame(endEffector.getBodyFixedFrame(), + isRequiredVariable); + } + /** * Retrieves and returns an orientation feedback control frame {@code YoOrientationFrame} associated to the given * end-effector, if it does not exist it is created. @@ -1131,6 +1149,15 @@ public YoTranslationFrame getOrCreatePointFeedbackControlFrame(ReferenceFrame pa return pointFeedbackControlFrame; } + public YoTranslationFrame getOrCreateImpedancePointFeedbackControlFrame(ReferenceFrame parentFrame, boolean isRequiredVariable) + { + if (pointFeedbackControlFrame == null) + pointFeedbackControlFrame = new YoTranslationFrame(namePrefix + "Point" + controlFrameNameSuffix, + getOrCreateControlFrameTranslationOffset(parentFrame, isRequiredVariable), + parentFrame); + return pointFeedbackControlFrame; + } + public YoOrientationFrame getOrCreateOrientationFeedbackControlFrame(ReferenceFrame parentFrame, boolean isRequiredVariable) { if (orientationFeedbackControlFrame == null) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/OrientationFeedbackControlCommand.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/OrientationFeedbackControlCommand.java index a30a9ea3e72..1688be90ec0 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/OrientationFeedbackControlCommand.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/OrientationFeedbackControlCommand.java @@ -78,6 +78,12 @@ public class OrientationFeedbackControlCommand implements FeedbackControlCommand */ private ReferenceFrame controlBaseFrame = null; + /** + * Whether to enable impedance control. If true, the controller will use impedance control + * to compute the desired acceleration. + */ + private boolean isImpedanceEnabled = false; + /** * Creates an empty command. */ @@ -105,6 +111,7 @@ public void set(OrientationFeedbackControlCommand other) angularGainsFrame = other.angularGainsFrame; spatialAccelerationCommand.set(other.spatialAccelerationCommand); controlBaseFrame = other.controlBaseFrame; + isImpedanceEnabled = other.isImpedanceEnabled; } /** @@ -217,6 +224,17 @@ public void setControlMode(WholeBodyControllerCoreMode controlMode) this.controlMode = controlMode; } + /** + * Sets the impedance control mode to be used for this command. + * + * @param isImpedanceEnabled whether to enable impedance control + */ + + public void setImpedanceEnabled(boolean isImpedanceEnabled) + { + this.isImpedanceEnabled = isImpedanceEnabled; + } + /** * Configures this feedback command's inputs for inverse kinematics. *

@@ -548,6 +566,11 @@ public SpatialAccelerationCommand getSpatialAccelerationCommand() return spatialAccelerationCommand; } + public boolean getIsImpedanceEnabled() + { + return isImpedanceEnabled; + } + public PID3DGains getGains() { return gains; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/PointFeedbackControlCommand.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/PointFeedbackControlCommand.java index 2e645ad9452..b0114c68693 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/PointFeedbackControlCommand.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/PointFeedbackControlCommand.java @@ -80,6 +80,12 @@ public class PointFeedbackControlCommand implements FeedbackControlCommand @@ -554,6 +572,11 @@ public SpatialAccelerationCommand getSpatialAccelerationCommand() return spatialAccelerationCommand; } + public boolean getIsImpedanceEnabled() + { + return isImpedanceEnabled; + } + public PID3DGains getGains() { return gains; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/SpatialFeedbackControlCommand.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/SpatialFeedbackControlCommand.java index ddfa46680b9..21080ef8072 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/SpatialFeedbackControlCommand.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/feedbackController/SpatialFeedbackControlCommand.java @@ -108,6 +108,12 @@ public class SpatialFeedbackControlCommand implements FeedbackControlCommand @@ -1239,6 +1256,11 @@ public SpatialAccelerationCommand getSpatialAccelerationCommand() return spatialAccelerationCommand; } + public boolean getIsImpedanceEnabled() + { + return isImpedanceEnabled; + } + public PIDSE3Gains getGains() { return gains; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/HighLevelControlManagerFactory.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/HighLevelControlManagerFactory.java index 7a4f5850733..cce06eb676c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/HighLevelControlManagerFactory.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/HighLevelControlManagerFactory.java @@ -44,6 +44,7 @@ import us.ihmc.yoVariables.parameters.DoubleParameter; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import java.util.Collection; @@ -241,6 +242,8 @@ public RigidBodyControlManager getOrCreateRigidBodyManager(RigidBodyBasics bodyT // Weights Vector3DReadOnly taskspaceAngularWeight = taskspaceAngularWeightMap.get(bodyName); Vector3DReadOnly taskspaceLinearWeight = taskspaceLinearWeightMap.get(bodyName); + PID3DGainsReadOnly taskspaceOrientationImpedanceGains = taskspaceOrientationGainMap.get(bodyName + "_Impedance"); + PID3DGainsReadOnly taskspacePositionImpedanceGains = taskspacePositionGainMap.get(bodyName + "_Impedance"); TObjectDoubleHashMap homeConfiguration = walkingControllerParameters.getOrCreateJointHomeConfiguration(); Pose3D homePose = walkingControllerParameters.getOrCreateBodyHomeConfiguration().get(bodyName); @@ -253,6 +256,9 @@ public RigidBodyControlManager getOrCreateRigidBodyManager(RigidBodyBasics bodyT RigidBodyControlMode defaultControlMode = walkingControllerParameters.getDefaultControlModesForRigidBodies().get(bodyName); boolean enableFunctionGenerators = walkingControllerParameters.enableFunctionGeneratorMode(bodyName); + YoBoolean isImpedanceEnabled = new YoBoolean(bodyName + "-EnableImpedanceControl", registry); + isImpedanceEnabled.set(walkingControllerParameters.enableImpedanceControl(bodyName)); + RigidBodyControlManager manager = new RigidBodyControlManager(bodyToControl, baseBody, elevator, @@ -264,10 +270,13 @@ public RigidBodyControlManager getOrCreateRigidBodyManager(RigidBodyBasics bodyT taskspaceLinearWeight, taskspaceOrientationGains, taskspacePositionGains, + taskspaceOrientationImpedanceGains, + taskspacePositionImpedanceGains, contactableBody, loadBearingParameters, defaultControlMode, enableFunctionGenerators, + isImpedanceEnabled, momentumOptimizationSettings.getRhoWeight(), controllerToolbox.getPostureAdjustmentProvider(), yoTime, diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/ManipulationControllerState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/ManipulationControllerState.java index ce2e39dfe6b..0f9ee811e04 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/ManipulationControllerState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/ManipulationControllerState.java @@ -44,6 +44,7 @@ import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import java.util.ArrayList; @@ -230,6 +231,8 @@ private RigidBodyControlManager createRigidBodyManager(RigidBodyBasics bodyToCon // Gains PID3DGainsReadOnly taskspaceOrientationGains = taskspaceOrientationGainMap.get(bodyName); PID3DGainsReadOnly taskspacePositionGains = taskspacePositionGainMap.get(bodyName); + PID3DGainsReadOnly taskspaceOrientationImpedanceGains = taskspaceOrientationGainMap.get(bodyName + "_Impedance"); + PID3DGainsReadOnly taskspacePositionImpedanceGains = taskspacePositionGainMap.get(bodyName + "_Impedance"); // Weights Vector3DReadOnly taskspaceAngularWeight = taskspaceAngularWeightMap.get(bodyName); @@ -241,6 +244,7 @@ private RigidBodyControlManager createRigidBodyManager(RigidBodyBasics bodyToCon ContactablePlaneBody contactableBody = null; RigidBodyControlMode defaultControlMode = walkingControllerParameters.getDefaultControlModesForRigidBodies().get(bodyName); boolean enableFunctionGenerators = walkingControllerParameters.enableFunctionGeneratorMode(bodyToControl.getName()); + YoBoolean isImpedanceEnabled = new YoBoolean(bodyName + "-EnableImpedanceControl", registry); RigidBodyControlManager manager = new RigidBodyControlManager(bodyToControl, baseBody, @@ -253,10 +257,13 @@ private RigidBodyControlManager createRigidBodyManager(RigidBodyBasics bodyToCon taskspaceLinearWeight, taskspaceOrientationGains, taskspacePositionGains, + taskspaceOrientationImpedanceGains, + taskspacePositionImpedanceGains, contactableBody, null, defaultControlMode, enableFunctionGenerators, + isImpedanceEnabled, momentumOptimizationSettings.getRhoWeight(), WholeBodyPostureAdjustmentProvider.createZeroPostureAdjustmentProvider(), yoTime, @@ -502,9 +509,18 @@ private void consumeManipulationCommands() { SE3TrajectoryControllerCommand taskspaceTrajectoryCommand = command.getTaskspaceTrajectoryCommand(); JointspaceTrajectoryCommand jointspaceTrajectoryCommand = command.getJointspaceTrajectoryCommand(); + WrenchTrajectoryControllerCommand feedForwardCommand = command.getFeedForwardTrajectoryCommand(); taskspaceTrajectoryCommand.setSequenceId(command.getSequenceId()); jointspaceTrajectoryCommand.setSequenceId(command.getSequenceId()); - handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand); + if (feedForwardCommand.getNumberOfTrajectoryPoints() != taskspaceTrajectoryCommand.getNumberOfTrajectoryPoints()) + { + handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand); + } + else + { + feedForwardCommand.setSequenceId(command.getSequenceId()); + handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand, feedForwardCommand); + } } } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java index 602160aded6..cc8dec9eac5 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java @@ -56,6 +56,7 @@ import us.ihmc.humanoidRobotics.communication.directionalControlToolboxAPI.DirectionalControlInputCommand; import us.ihmc.humanoidRobotics.communication.fastWalkingAPI.FastWalkingGaitParametersCommand; import us.ihmc.humanoidRobotics.communication.packets.walking.HumanoidBodyPart; +import us.ihmc.log.LogTools; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.robotics.robotSide.RobotSide; @@ -452,7 +453,36 @@ public void consumeManipulationCommands(WalkingState currentState, boolean allow JointspaceTrajectoryCommand jointspaceTrajectoryCommand = command.getJointspaceTrajectoryCommand(); taskspaceTrajectoryCommand.setSequenceId(command.getSequenceId()); jointspaceTrajectoryCommand.setSequenceId(command.getSequenceId()); - handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand); + WrenchTrajectoryControllerCommand feedForwardCommand = command.getFeedForwardTrajectoryCommand(); + SE3PIDGainsTrajectoryControllerCommand gainsCommand = command.getPIDGainsTrajectoryCommand(); + if (feedForwardCommand.getNumberOfTrajectoryPoints() != taskspaceTrajectoryCommand.getNumberOfTrajectoryPoints()) + { + if (gainsCommand.getNumberOfTrajectoryPoints() != taskspaceTrajectoryCommand.getNumberOfTrajectoryPoints()) + { + handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand); + } + else + { + gainsCommand.setSequenceId(command.getSequenceId()); + handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand, gainsCommand); + } + } + else + { + LogTools.info("Hybrid message with feedForward activated"); + feedForwardCommand.setSequenceId(command.getSequenceId()); + + if (gainsCommand.getNumberOfTrajectoryPoints() != taskspaceTrajectoryCommand.getNumberOfTrajectoryPoints()) + { + handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand, feedForwardCommand); + } + else + { + LogTools.info("Hybrid message with feedForward and gains activated"); + gainsCommand.setSequenceId(command.getSequenceId()); + handManager.handleHybridTrajectoryCommand(taskspaceTrajectoryCommand, jointspaceTrajectoryCommand, feedForwardCommand, gainsCommand); + } + } } } diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/FeedbackControllerInterface.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/FeedbackControllerInterface.java index ce5d71ebc4a..1ff1570844a 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/FeedbackControllerInterface.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/FeedbackControllerInterface.java @@ -10,6 +10,10 @@ public interface FeedbackControllerInterface void setEnabled(boolean isEnabled); + default void setImpedanceEnabled(boolean isImpedanceEnabled){ + // Empty default implementation to avoid breaking existing implementations. + }; + void computeInverseDynamics(); void computeInverseKinematics(); @@ -25,6 +29,10 @@ default void computeAchievedVelocity() boolean isEnabled(); + default boolean isImpedanceEnabled() { + return false; + }; + InverseDynamicsCommand getInverseDynamicsOutput(); InverseKinematicsCommand getInverseKinematicsOutput(); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java index f47516f36bd..5372b202e94 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java @@ -1,5 +1,9 @@ package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.taskspace; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.interfaces.decomposition.SingularValueDecomposition_F64; import us.ihmc.commonWalkingControlModules.controlModules.YoOrientationFrame; import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerException; import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox; @@ -16,20 +20,28 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerInterface; import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings; import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings.FilterVector3D; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointIndexHandler; import us.ihmc.euclid.matrix.Matrix3D; import us.ihmc.euclid.referenceFrame.FrameQuaternion; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.algorithms.CompositeRigidBodyMassMatrixCalculator; +import us.ihmc.mecano.algorithms.GeometricJacobianCalculator; import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider; import us.ihmc.mecano.algorithms.interfaces.RigidBodyTwistProvider; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.tools.MultiBodySystemTools; import us.ihmc.robotics.controllers.pidGains.YoPID3DGains; import us.ihmc.robotics.screwTheory.SelectionMatrix6D; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import java.util.ArrayList; +import java.util.List; + import static us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox.appendIndex; import static us.ihmc.commonWalkingControlModules.controllerCore.data.SpaceData3D.*; import static us.ihmc.commonWalkingControlModules.controllerCore.data.Type.*; @@ -39,6 +51,7 @@ public class OrientationFeedbackController implements FeedbackControllerInterfac private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); private final YoBoolean isEnabled; + private final YoBoolean isImpedanceEnabled; private final FBQuaternion3D yoDesiredOrientation; private final FBQuaternion3D yoCurrentOrientation; @@ -89,6 +102,22 @@ public class OrientationFeedbackController implements FeedbackControllerInterfac private final Twist currentTwist = new Twist(); private final SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); + private final DMatrixRMaj inverseInertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj inverseInertiaTempMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj sqrtInertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj sqrtProportionalGainMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempDerivativeGainMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempSqrtMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj U = new DMatrixRMaj(0, 0); + private final DMatrixRMaj W = new DMatrixRMaj(0, 0); + private final DMatrixRMaj Vt = new DMatrixRMaj(0, 0); + private final SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(true, true, true); + + private final DMatrixRMaj jacobianMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj massInverseMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj subMassInverseMatrix = new DMatrixRMaj(0, 0); + private final SpatialAccelerationCommand inverseDynamicsOutput = new SpatialAccelerationCommand(); private final SpatialVelocityCommand inverseKinematicsOutput = new SpatialVelocityCommand(); private final VirtualTorqueCommand virtualModelControlOutput = new VirtualTorqueCommand(); @@ -97,8 +126,11 @@ public class OrientationFeedbackController implements FeedbackControllerInterfac private final YoPID3DGains gains; private final Matrix3D tempGainMatrix = new Matrix3D(); + private final GeometricJacobianCalculator jacobianCalculator = new GeometricJacobianCalculator(); + private final RigidBodyTwistProvider rigidBodyTwistProvider; private final RigidBodyAccelerationProvider rigidBodyAccelerationProvider; + private final CompositeRigidBodyMassMatrixCalculator massMatrixCalculator; private RigidBodyBasics base; private ReferenceFrame controlBaseFrame; @@ -108,6 +140,11 @@ public class OrientationFeedbackController implements FeedbackControllerInterfac private final RigidBodyBasics endEffector; private final YoOrientationFrame controlFrame; + private final JointIndexHandler jointIndexHandler; + private final List jointPath = new ArrayList<>(); + private final List allJointIndices = new ArrayList<>(); + private int[] jointIndices; + private final double dt; private final boolean isRootBody; private final boolean computeIntegralTerm; @@ -147,9 +184,11 @@ public OrientationFeedbackController(RigidBodyBasics endEffector, isRootBody = false; rootBody = null; } + jointIndexHandler = ccToolbox.getJointIndexHandler(); rigidBodyTwistProvider = ccToolbox.getRigidBodyTwistCalculator(); rigidBodyAccelerationProvider = ccToolbox.getRigidBodyAccelerationProvider(); + massMatrixCalculator = ccToolbox.getMassMatrixCalculator(); String endEffectorName = endEffector.getName(); dt = ccToolbox.getControlDT(); @@ -161,6 +200,9 @@ public OrientationFeedbackController(RigidBodyBasics endEffector, isEnabled = new YoBoolean(appendIndex(endEffectorName, controllerIndex) + "IsOrientationFBControllerEnabled", fbToolbox.getRegistry()); isEnabled.set(false); + isImpedanceEnabled = new YoBoolean(appendIndex(endEffectorName, controllerIndex) + "isOrientationFBControllerImpedanceEnabled", fbToolbox.getRegistry()); + isImpedanceEnabled.set(false); + yoDesiredOrientation = fbToolbox.getOrCreateOrientationData(endEffector, controllerIndex, DESIRED, isEnabled, true); yoCurrentOrientation = fbToolbox.getOrCreateOrientationData(endEffector, controllerIndex, CURRENT, isEnabled, true); yoErrorOrientation = fbToolbox.getOrCreateOrientationData(endEffector, controllerIndex, ERROR, isEnabled, false); @@ -288,6 +330,25 @@ public void submitFeedbackControlCommand(OrientationFeedbackControlCommand comma currentCommandId = command.getCommandId(); base = command.getBase(); controlBaseFrame = command.getControlBaseFrame(); + setImpedanceEnabled(command.getIsImpedanceEnabled()); + + if (isImpedanceEnabled()) + { + MultiBodySystemTools.collectJointPath(base.getChildrenJoints().get(0).getSuccessor(), endEffector, jointPath); + allJointIndices.clear(); + for (JointBasics joint : jointPath) + { + int[] indices = jointIndexHandler.getJointIndices(joint); + for (int index : indices) + { + allJointIndices.add(index); + } + } + jointIndices = new int[allJointIndices.size()]; + for (int i = 0; i < allJointIndices.size(); i++) { + jointIndices[i] = allJointIndices.get(i); + } + } inverseDynamicsOutput.set(command.getSpatialAccelerationCommand()); inverseKinematicsOutput.setProperties(command.getSpatialAccelerationCommand()); @@ -332,6 +393,12 @@ public void setEnabled(boolean isEnabled) this.isEnabled.set(isEnabled); } + @Override + public void setImpedanceEnabled(boolean isImpedanceEnabled) + { + this.isImpedanceEnabled.set(isImpedanceEnabled); + } + @Override public void initialize() { // TODO See SpatialFeedbackController.initialize() @@ -350,6 +417,7 @@ public void initialize() private final FrameVector3D proportionalFeedback = new FrameVector3D(); private final FrameVector3D derivativeFeedback = new FrameVector3D(); private final FrameVector3D integralFeedback = new FrameVector3D(); + private final Matrix3D inverseInertiaMatrix3D = new Matrix3D(); @Override public void computeInverseDynamics() @@ -358,6 +426,10 @@ public void computeInverseDynamics() return; ReferenceFrame trajectoryFrame = yoDesiredOrientation.getReferenceFrame(); + if (isImpedanceEnabled()) + { + computeInverseInertiaMatrix(); + } computeProportionalTerm(proportionalFeedback); computeDerivativeTerm(derivativeFeedback); @@ -365,6 +437,13 @@ public void computeInverseDynamics() feedForwardAngularAcceleration.setIncludingFrame(yoFeedForwardAngularAcceleration); feedForwardAngularAcceleration.changeFrame(controlFrame); + if (isImpedanceEnabled()){ + inverseInertiaMatrix3D.set(inverseInertiaTempMatrix); + inverseInertiaMatrix3D.transform(proportionalFeedback); + inverseInertiaMatrix3D.transform(derivativeFeedback); + inverseInertiaMatrix3D.transform(integralFeedback); + } + desiredAngularAcceleration.setIncludingFrame(proportionalFeedback); desiredAngularAcceleration.add(derivativeFeedback); desiredAngularAcceleration.add(integralFeedback); @@ -393,6 +472,11 @@ public void computeInverseKinematics() if (!isEnabled()) return; + if (isImpedanceEnabled()) + { + throw new FeedbackControllerException("Impedance control is not implemented in computeInverseKinematics."); + } + inverseKinematicsOutput.setProperties(inverseDynamicsOutput); ReferenceFrame trajectoryFrame = yoDesiredOrientation.getReferenceFrame(); @@ -427,6 +511,11 @@ public void computeVirtualModelControl() if (!isEnabled()) return; + if (isImpedanceEnabled()) + { + throw new FeedbackControllerException("Impedance control is not implemented in computeVirtualModelControl."); + } + computeFeedbackTorque(); if (isRootBody) @@ -584,6 +673,24 @@ public void computeDerivativeTerm(FrameVector3D feedbackTermToPack) feedbackTermToPack.changeFrame(angularGainsFrame != null ? angularGainsFrame : controlFrame); gains.getDerivativeGainMatrix(tempGainMatrix); + if (isImpedanceEnabled()){ + gains.getFullProportionalGainMatrix(tempMatrix, 3); + + sqrtProportionalGainMatrix.reshape(6,6); + sqrtInertiaMatrix.reshape(6,6); + + MatrixMissingTools.sqrt(tempMatrix, sqrtProportionalGainMatrix, tempSqrtMatrix, U, W, Vt, svd); + tempMatrix.set(inverseInertiaMatrix); + CommonOps_DDRM.invert(tempMatrix); + MatrixMissingTools.sqrt(tempMatrix, sqrtInertiaMatrix, tempSqrtMatrix, U, W, Vt, svd); + + CommonOps_DDRM.mult(sqrtInertiaMatrix, sqrtProportionalGainMatrix, tempDerivativeGainMatrix); + CommonOps_DDRM.multAdd(sqrtProportionalGainMatrix, sqrtInertiaMatrix, tempDerivativeGainMatrix); + + tempMatrix.reshape(3, 3); + CommonOps_DDRM.extract(tempDerivativeGainMatrix, 0, 3, 0, 3, tempMatrix, 0, 0); + tempGainMatrix.set(tempMatrix); + } tempGainMatrix.transform(feedbackTermToPack); feedbackTermToPack.changeFrame(controlFrame); @@ -664,12 +771,45 @@ public void computeIntegralTerm(FrameVector3D feedbackTermToPack) feedbackTermToPack.changeFrame(controlFrame); } + private void computeInverseInertiaMatrix() + { + jacobianCalculator.clear(); + jacobianCalculator.setKinematicChain(base.getChildrenJoints().get(0).getSuccessor(), endEffector); + jacobianCalculator.setJacobianFrame(controlFrame); + jacobianCalculator.reset(); + + // Jacobian is an M x N matrix, M is called the task size and + // * N is the overall number of degrees of freedom (DoFs) to be controlled. + jacobianMatrix.set(jacobianCalculator.getJacobianMatrix()); + jacobianMatrix.reshape(jacobianMatrix.getNumRows(), jacobianMatrix.getNumCols()); + + massInverseMatrix.set(massMatrixCalculator.getMassMatrix()); + massMatrixCalculator.reset(); + massInverseMatrix.reshape(massInverseMatrix.getNumRows(), massInverseMatrix.getNumCols()); + CommonOps_DDRM.invert(massInverseMatrix); + subMassInverseMatrix.set(new DMatrixRMaj(jointIndices.length, jointIndices.length)); + CommonOps_DDRM.extract(massInverseMatrix, jointIndices, jointIndices.length, jointIndices, jointIndices.length, subMassInverseMatrix); + + inverseInertiaTempMatrix.reshape(jointIndices.length, jointIndices.length); + CommonOps_DDRM.mult(jacobianMatrix, subMassInverseMatrix, inverseInertiaTempMatrix); + CommonOps_DDRM.multTransB(inverseInertiaTempMatrix, jacobianMatrix, inverseInertiaMatrix); + inverseInertiaTempMatrix.reshape(3,3); + // Point so extract the 3x3 matrix from the 6x6 matrix (Lower right 3x3 matrix) + CommonOps_DDRM.extract(inverseInertiaMatrix, 0, 3, 0, 3, inverseInertiaTempMatrix, 0, 0); + } + @Override public boolean isEnabled() { return isEnabled.getBooleanValue(); } + @Override + public boolean isImpedanceEnabled() + { + return isImpedanceEnabled.getBooleanValue(); + } + @Override public SpatialAccelerationCommand getInverseDynamicsOutput() { diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java index 0b3a3f684e6..2612732258b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java @@ -1,5 +1,11 @@ package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.taskspace; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.decomposition.lu.LUDecompositionAlt_DDRM; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.dense.row.linsol.lu.LinearSolverLu_DDRM; +import org.ejml.interfaces.decomposition.SingularValueDecomposition_F64; import us.ihmc.commonWalkingControlModules.controlModules.YoTranslationFrame; import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerException; import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox; @@ -17,21 +23,29 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerInterface; import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings; import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings.FilterVector3D; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointIndexHandler; import us.ihmc.euclid.matrix.Matrix3D; import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.algorithms.CompositeRigidBodyMassMatrixCalculator; +import us.ihmc.mecano.algorithms.GeometricJacobianCalculator; import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider; import us.ihmc.mecano.algorithms.interfaces.RigidBodyTwistProvider; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.tools.MultiBodySystemTools; import us.ihmc.robotics.controllers.pidGains.YoPID3DGains; import us.ihmc.robotics.screwTheory.SelectionMatrix6D; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import java.util.ArrayList; +import java.util.List; + import static us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox.appendIndex; import static us.ihmc.commonWalkingControlModules.controllerCore.data.SpaceData3D.*; import static us.ihmc.commonWalkingControlModules.controllerCore.data.Type.*; @@ -41,6 +55,7 @@ public class PointFeedbackController implements FeedbackControllerInterface private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); private final YoBoolean isEnabled; + private final YoBoolean isImpedanceEnabled; private final FBPoint3D yoDesiredPosition; private final FBPoint3D yoCurrentPosition; @@ -85,18 +100,39 @@ public class PointFeedbackController implements FeedbackControllerInterface private final Twist currentTwist = new Twist(); + private final DMatrixRMaj inverseInertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj inverseInertiaTempMatrix = new DMatrixRMaj(0, 0); + private final SpatialAccelerationCommand inverseDynamicsOutput = new SpatialAccelerationCommand(); private final SpatialVelocityCommand inverseKinematicsOutput = new SpatialVelocityCommand(); private final VirtualForceCommand virtualModelControlOutput = new VirtualForceCommand(); private final MomentumRateCommand virtualModelControlRootOutput = new MomentumRateCommand(); private final SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); + private final Matrix3D inverseInertiaMatrix3D = new Matrix3D(); + private final DMatrixRMaj tempMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj feedbacktermsMatrix = new DMatrixRMaj(6, 1); + private final DMatrixRMaj sqrtInertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj sqrtProportionalGainMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempDerivativeGainMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempSqrtMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj U = new DMatrixRMaj(0, 0); + private final DMatrixRMaj W = new DMatrixRMaj(0, 0); + private final DMatrixRMaj Vt = new DMatrixRMaj(0, 0); + private final SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(true, true, true); + private final DMatrixRMaj jacobianMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj massInverseMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj subMassInverseMatrix = new DMatrixRMaj(0, 0); + private final YoPID3DGains gains; private final Matrix3D tempGainMatrix = new Matrix3D(); private final YoTranslationFrame controlFrame; + private final GeometricJacobianCalculator jacobianCalculator = new GeometricJacobianCalculator(); private final RigidBodyTwistProvider rigidBodyTwistProvider; private final RigidBodyAccelerationProvider rigidBodyAccelerationProvider; + private final CompositeRigidBodyMassMatrixCalculator massMatrixCalculator; + private final LinearSolverLu_DDRM inverseSolver = new LinearSolverLu_DDRM(new LUDecompositionAlt_DDRM()); private RigidBodyBasics base; private ReferenceFrame controlBaseFrame; @@ -105,6 +141,11 @@ public class PointFeedbackController implements FeedbackControllerInterface private final RigidBodyBasics rootBody; private final RigidBodyBasics endEffector; + private final JointIndexHandler jointIndexHandler; + private final List jointPath = new ArrayList<>(); + private final List allJointIndices = new ArrayList<>(); + private int[] jointIndices; + private final double dt; private final boolean isRootBody; private final boolean computeIntegralTerm; @@ -144,9 +185,11 @@ public PointFeedbackController(RigidBodyBasics endEffector, isRootBody = false; rootBody = null; } + jointIndexHandler = ccToolbox.getJointIndexHandler(); rigidBodyTwistProvider = ccToolbox.getRigidBodyTwistCalculator(); rigidBodyAccelerationProvider = ccToolbox.getRigidBodyAccelerationProvider(); + massMatrixCalculator = ccToolbox.getMassMatrixCalculator(); String endEffectorName = endEffector.getName(); dt = ccToolbox.getControlDT(); @@ -158,6 +201,9 @@ public PointFeedbackController(RigidBodyBasics endEffector, isEnabled = new YoBoolean(appendIndex(endEffectorName, controllerIndex) + "isPointFBControllerEnabled", fbToolbox.getRegistry()); isEnabled.set(false); + isImpedanceEnabled = new YoBoolean(appendIndex(endEffectorName, controllerIndex) + "isPointFBControllerImpedanceEnabled", fbToolbox.getRegistry()); + isImpedanceEnabled.set(false); + yoDesiredPosition = fbToolbox.getOrCreatePositionData(endEffector, controllerIndex, DESIRED, isEnabled, true); yoCurrentPosition = fbToolbox.getOrCreatePositionData(endEffector, controllerIndex, CURRENT, isEnabled, true); yoErrorPosition = fbToolbox.getOrCreateVectorData3D(endEffector, controllerIndex, ERROR, POSITION, isEnabled, false); @@ -280,6 +326,25 @@ public void submitFeedbackControlCommand(PointFeedbackControlCommand command) currentCommandId = command.getCommandId(); base = command.getBase(); controlBaseFrame = command.getControlBaseFrame(); + setImpedanceEnabled(command.getIsImpedanceEnabled()); + + if (isImpedanceEnabled()) + { + MultiBodySystemTools.collectJointPath(base.getChildrenJoints().get(0).getSuccessor(), endEffector, jointPath); + allJointIndices.clear(); + for (JointBasics joint : jointPath) + { + int[] indices = jointIndexHandler.getJointIndices(joint); + for (int index : indices) + { + allJointIndices.add(index); + } + } + jointIndices = new int[allJointIndices.size()]; + for (int i = 0; i < allJointIndices.size(); i++) { + jointIndices[i] = allJointIndices.get(i); + } + } inverseDynamicsOutput.set(command.getSpatialAccelerationCommand()); @@ -321,6 +386,12 @@ public void setEnabled(boolean isEnabled) this.isEnabled.set(isEnabled); } + @Override + public void setImpedanceEnabled(boolean isEnabled) + { + this.isImpedanceEnabled.set(isEnabled); + } + @Override public void initialize() { // TODO: See SpatialFeedbackController.initialize() @@ -346,12 +417,23 @@ public void computeInverseDynamics() ReferenceFrame trajectoryFrame = yoDesiredPosition.getReferenceFrame(); + if (isImpedanceEnabled()) + { + computeInverseInertiaMatrix(); + } + computeProportionalTerm(proportionalFeedback); computeDerivativeTerm(derivativeFeedback); computeIntegralTerm(integralFeedback); feedForwardLinearAcceleration.setIncludingFrame(yoFeedForwardLinearAcceleration); feedForwardLinearAcceleration.changeFrame(controlFrame); + if (isImpedanceEnabled()){ + inverseInertiaMatrix3D.set(inverseInertiaTempMatrix); + inverseInertiaMatrix3D.transform(proportionalFeedback); + inverseInertiaMatrix3D.transform(derivativeFeedback); + } + desiredLinearAcceleration.setIncludingFrame(proportionalFeedback); desiredLinearAcceleration.add(derivativeFeedback); desiredLinearAcceleration.add(integralFeedback); @@ -382,6 +464,11 @@ public void computeInverseKinematics() if (!isEnabled()) return; + if (isImpedanceEnabled()) + { + throw new FeedbackControllerException("Impedance control is not implemented in computeInverseKinematics."); + } + inverseKinematicsOutput.setProperties(inverseDynamicsOutput); ReferenceFrame trajectoryFrame = yoDesiredPosition.getReferenceFrame(); @@ -421,6 +508,11 @@ public void computeVirtualModelControl() if (!isEnabled()) return; + if (isImpedanceEnabled()) + { + throw new FeedbackControllerException("Impedance control is not implemented in computeVirtualModelControl."); + } + computeFeedbackForce(); if (isRootBody) @@ -577,6 +669,36 @@ private void computeDerivativeTerm(FrameVector3D feedbackTermToPack) feedbackTermToPack.changeFrame(linearGainsFrame != null ? linearGainsFrame : controlFrame); gains.getDerivativeGainMatrix(tempGainMatrix); + if (isImpedanceEnabled()){ + gains.getFullProportionalGainMatrix(tempMatrix, 3); + + sqrtProportionalGainMatrix.reshape(6,6); + sqrtInertiaMatrix.reshape(6,6); + + MatrixMissingTools.sqrt(tempMatrix, sqrtProportionalGainMatrix, tempSqrtMatrix, U, W, Vt, svd); + tempMatrix.set(inverseInertiaMatrix); + MatrixMissingTools.invert(tempMatrix, inverseSolver); + MatrixMissingTools.sqrt(tempMatrix, sqrtInertiaMatrix, tempSqrtMatrix, U, W, Vt, svd); + + CommonOps_DDRM.mult(sqrtInertiaMatrix, sqrtProportionalGainMatrix, tempDerivativeGainMatrix); + CommonOps_DDRM.multAdd(sqrtProportionalGainMatrix, sqrtInertiaMatrix, tempDerivativeGainMatrix); + + feedbacktermsMatrix.set(0, 0, 1); + feedbacktermsMatrix.set(1, 0, 1); + feedbacktermsMatrix.set(2, 0, 1); + feedbacktermsMatrix.set(3, 0, feedbackTermToPack.getX()); + feedbacktermsMatrix.set(4, 0, feedbackTermToPack.getY()); + feedbacktermsMatrix.set(5, 0, feedbackTermToPack.getZ()); + + tempMatrix.reshape(6,1); + CommonOps_DDRM.mult(tempDerivativeGainMatrix, feedbacktermsMatrix, tempMatrix); + + feedbackTermToPack.setX(tempMatrix.get(3, 0)); + feedbackTermToPack.setY(tempMatrix.get(4, 0)); + feedbackTermToPack.setZ(tempMatrix.get(5, 0)); + + feedbackTermToPack.scale(gains.getDampingRatios()[0], gains.getDampingRatios()[1], gains.getDampingRatios()[2]); + } tempGainMatrix.transform(feedbackTermToPack); feedbackTermToPack.changeFrame(controlFrame); @@ -702,12 +824,45 @@ private void subtractCoriolisAcceleration(FrameVector3D linearAccelerationToModi linearAccelerationToModify.changeFrame(worldFrame); } + private void computeInverseInertiaMatrix() + { + jacobianCalculator.clear(); + jacobianCalculator.setKinematicChain(base.getChildrenJoints().get(0).getSuccessor(), endEffector); + jacobianCalculator.setJacobianFrame(controlFrame); + jacobianCalculator.reset(); + + // Jacobian is an M x N matrix, M is called the task size and + // * N is the overall number of degrees of freedom (DoFs) to be controlled. + jacobianMatrix.set(jacobianCalculator.getJacobianMatrix()); + jacobianMatrix.reshape(jacobianMatrix.getNumRows(), jacobianMatrix.getNumCols()); + + massInverseMatrix.set(massMatrixCalculator.getMassMatrix()); + massMatrixCalculator.reset(); + massInverseMatrix.reshape(massInverseMatrix.getNumRows(), massInverseMatrix.getNumCols()); + subMassInverseMatrix.set(new DMatrixRMaj(jointIndices.length, jointIndices.length)); + CommonOps_DDRM.extract(massInverseMatrix, jointIndices, jointIndices.length, jointIndices, jointIndices.length, subMassInverseMatrix); + MatrixMissingTools.invert(subMassInverseMatrix, inverseSolver); + + inverseInertiaTempMatrix.reshape(jointIndices.length, jointIndices.length); + CommonOps_DDRM.mult(jacobianMatrix, subMassInverseMatrix, inverseInertiaTempMatrix); + CommonOps_DDRM.multTransB(inverseInertiaTempMatrix, jacobianMatrix, inverseInertiaMatrix); + inverseInertiaTempMatrix.reshape(3,3); + // Point so extract the 3x3 matrix from the 6x6 matrix (Lower right 3x3 matrix) + CommonOps_DDRM.extract(inverseInertiaMatrix, 3, 6, 3, 6, inverseInertiaTempMatrix, 0, 0); + } + @Override public boolean isEnabled() { return isEnabled.getBooleanValue(); } + @Override + public boolean isImpedanceEnabled() + { + return isImpedanceEnabled.getBooleanValue(); + } + @Override public SpatialAccelerationCommand getInverseDynamicsOutput() { diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java index 3ae9ed96995..f0fe5bdc35e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java @@ -1,5 +1,12 @@ package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.taskspace; +import gnu.trove.list.array.TIntArrayList; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.decomposition.lu.LUDecompositionAlt_DDRM; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.dense.row.linsol.lu.LinearSolverLu_DDRM; +import org.ejml.interfaces.decomposition.SingularValueDecomposition_F64; import us.ihmc.commonWalkingControlModules.controlModules.YoSE3OffsetFrame; import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerException; import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox; @@ -14,21 +21,31 @@ import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerInterface; import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings; import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings.FilterVector3D; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointIndexHandler; import us.ihmc.euclid.matrix.Matrix3D; import us.ihmc.euclid.referenceFrame.*; +import us.ihmc.mecano.algorithms.CompositeRigidBodyMassMatrixCalculator; +import us.ihmc.mecano.algorithms.GeometricJacobianCalculator; import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider; import us.ihmc.mecano.algorithms.interfaces.RigidBodyTwistProvider; import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.tools.MultiBodySystemTools; import us.ihmc.robotics.controllers.pidGains.YoPID3DGains; import us.ihmc.robotics.controllers.pidGains.YoPIDSE3Gains; import us.ihmc.robotics.screwTheory.SelectionMatrix6D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import java.util.ArrayList; +import java.util.List; + import static us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox.appendIndex; import static us.ihmc.commonWalkingControlModules.controllerCore.data.SpaceData3D.POSITION; import static us.ihmc.commonWalkingControlModules.controllerCore.data.SpaceData3D.ROTATION_VECTOR; @@ -40,6 +57,7 @@ public class SpatialFeedbackController implements FeedbackControllerInterface protected static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); protected final YoBoolean isEnabled; + protected boolean isImpedanceEnabled; protected final FBPose3D yoDesiredPose; protected final FBPose3D yoCurrentPose; @@ -78,6 +96,13 @@ public class SpatialFeedbackController implements FeedbackControllerInterface protected final FBVector3D yoDesiredRotationVector; protected final FBVector3D yoCurrentRotationVector; + protected final YoFrameVector3D yoPositionFeedback; + protected final YoFrameVector3D yoOrientationFeedback; + + protected final YoMatrix yoDerivativeTermMatrix; + protected final YoMatrix yoCoriolisMatrix; + protected final YoMatrix yoTestMatrix; + protected final FramePoint3D controlFramePosition = new FramePoint3D(); protected final FrameQuaternion controlFrameOrientation = new FrameQuaternion(); protected final FramePose3D desiredPose = new FramePose3D(); @@ -118,9 +143,40 @@ public class SpatialFeedbackController implements FeedbackControllerInterface protected final YoPID3DGains positionGains; protected final YoPID3DGains orientationGains; protected final Matrix3D tempGainMatrix = new Matrix3D(); + protected final DMatrixRMaj dampingRatioMatrix = new DMatrixRMaj(6, 6); + + protected final GeometricJacobianCalculator jacobianCalculator = new GeometricJacobianCalculator(); + protected final DMatrixRMaj inverseInertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj inverseInertiaTempMatrix = new DMatrixRMaj(0, 0); + protected final DMatrixRMaj coriolisMatrix = new DMatrixRMaj(0,0); + protected final TIntArrayList activeAxis = new TIntArrayList(); + private final DMatrixRMaj tempMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempFeedbackMatrix = new DMatrixRMaj(6, 1); + + private final DMatrixRMaj tempLinearMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempAngularMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj feedbacktermsMatrix = new DMatrixRMaj(6, 1); + private final DMatrixRMaj sqrtInertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj sqrtProportionalGainMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj tempDerivativeGainMatrix = new DMatrixRMaj(0, 0); + private final Matrix3D tempMatrix3D = new Matrix3D(); + private final DMatrixRMaj tempSqrtMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj U = new DMatrixRMaj(0, 0); + private final DMatrixRMaj W = new DMatrixRMaj(0, 0); + private final DMatrixRMaj Vt = new DMatrixRMaj(0, 0); + private final SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(true, true, true); + + protected final DMatrixRMaj jacobianMatrix = new DMatrixRMaj(0, 0); + protected final DMatrixRMaj subMassMatrix = new DMatrixRMaj(0, 0); + protected final DMatrixRMaj subMassInverseMatrix = new DMatrixRMaj(0, 0); + protected final DMatrixRMaj identityMatrix = CommonOps_DDRM.identity(6, 6); + private final DMatrixRMaj inertiaMatrix = new DMatrixRMaj(0, 0); + private final DMatrixRMaj savedInertiaMatrix = new DMatrixRMaj(0, 0); protected final RigidBodyTwistProvider rigidBodyTwistProvider; protected final RigidBodyAccelerationProvider rigidBodyAccelerationProvider; + private final CompositeRigidBodyMassMatrixCalculator massMatrixCalculator; + protected final LinearSolverLu_DDRM inverseSolver = new LinearSolverLu_DDRM(new LUDecompositionAlt_DDRM()); protected final RigidBodyBasics rootBody; protected RigidBodyBasics base; @@ -130,6 +186,9 @@ public class SpatialFeedbackController implements FeedbackControllerInterface protected final RigidBodyBasics endEffector; protected final YoSE3OffsetFrame controlFrame; + protected final JointIndexHandler jointIndexHandler; + protected final TIntArrayList jointIndices = new TIntArrayList(); + protected final List jointPath = new ArrayList<>(); protected final double dt; protected final boolean isRootBody; @@ -170,9 +229,11 @@ public SpatialFeedbackController(RigidBodyBasics endEffector, isRootBody = false; rootBody = null; } + jointIndexHandler = ccToolbox.getJointIndexHandler(); rigidBodyTwistProvider = ccToolbox.getRigidBodyTwistCalculator(); rigidBodyAccelerationProvider = ccToolbox.getRigidBodyAccelerationProvider(); + massMatrixCalculator = ccToolbox.getMassMatrixCalculator(); String endEffectorName = endEffector.getName(); dt = ccToolbox.getControlDT(); @@ -186,12 +247,20 @@ public SpatialFeedbackController(RigidBodyBasics endEffector, isEnabled = new YoBoolean(appendIndex(endEffectorName, controllerIndex) + "isSpatialFBControllerEnabled", fbToolbox.getRegistry()); isEnabled.set(false); + isImpedanceEnabled = false; + + yoPositionFeedback = new YoFrameVector3D(endEffector.getName() + "positionFeedback", worldFrame, parentRegistry); + yoOrientationFeedback = new YoFrameVector3D(endEffector.getName() + "orientationFeedback", worldFrame, parentRegistry); yoDesiredPose = fbToolbox.getOrCreatePoseData(endEffector, controllerIndex, DESIRED, isEnabled, true); yoCurrentPose = fbToolbox.getOrCreatePoseData(endEffector, controllerIndex, CURRENT, isEnabled, true); yoErrorVector = fbToolbox.getOrCreateVectorData6D(endEffector, controllerIndex, ERROR, POSE, isEnabled, false); yoErrorOrientation = fbToolbox.getOrCreateOrientationData(endEffector, controllerIndex, ERROR, isEnabled, false); + yoDerivativeTermMatrix = new YoMatrix(appendIndex(endEffectorName, controllerIndex) + "DerivativeTermMatrix", 6, 6, fbToolbox.getRegistry()); + yoCoriolisMatrix = new YoMatrix(appendIndex(endEffectorName, controllerIndex) + "CoriolisMatrix", 6, 6, fbToolbox.getRegistry()); + yoTestMatrix = new YoMatrix(appendIndex(endEffectorName, controllerIndex) + "TestMatrix", 6, 6, fbToolbox.getRegistry()); + if (computeIntegralTerm) { yoErrorPositionIntegrated = fbToolbox.getOrCreateVectorData3D(endEffector, controllerIndex, ERROR_INTEGRATED, POSITION, isEnabled, false); @@ -328,15 +397,37 @@ public void submitFeedbackControlCommand(SpatialFeedbackControlCommand command) currentCommandId = command.getCommandId(); base = command.getBase(); controlBaseFrame = command.getControlBaseFrame(); + setImpedanceEnabled(command.getIsImpedanceEnabled()); inverseDynamicsOutput.set(command.getSpatialAccelerationCommand()); inverseKinematicsOutput.setProperties(command.getSpatialAccelerationCommand()); virtualModelControlOutput.setProperties(command.getSpatialAccelerationCommand()); gains.set(command.getGains()); + dampingRatioMatrix.set(0,0, gains.getOrientationGains().getDampingRatios()[0]); + dampingRatioMatrix.set(1,1, gains.getOrientationGains().getDampingRatios()[1]); + dampingRatioMatrix.set(2,2, gains.getOrientationGains().getDampingRatios()[2]); + dampingRatioMatrix.set(3,3, gains.getPositionGains().getDampingRatios()[0]); + dampingRatioMatrix.set(4,4, gains.getPositionGains().getDampingRatios()[1]); + dampingRatioMatrix.set(5,5, gains.getPositionGains().getDampingRatios()[2]); command.getSpatialAccelerationCommand().getSelectionMatrix(selectionMatrix); + selectionMatrix.getAngularPart().setAxisSelection(false, false, true); angularGainsFrame = command.getAngularGainsFrame(); linearGainsFrame = command.getLinearGainsFrame(); + if (isImpedanceEnabled()) + { + MultiBodySystemTools.collectJointPath(base.getChildrenJoints().get(0).getSuccessor(), endEffector, jointPath); + jointIndices.clear(); + + for (int i = 0; i < jointPath.size(); i++) + { + jointIndices.add(jointIndexHandler.getJointIndices(jointPath.get(i))); + } + + activeAxis.reset(); + selectionMatrix.getActiveAxes(activeAxis); + } + command.getControlFramePoseIncludingFrame(controlFramePosition, controlFrameOrientation); controlFrame.setOffsetToParent(controlFramePosition, controlFrameOrientation); @@ -368,8 +459,11 @@ public void submitFeedbackControlCommand(SpatialFeedbackControlCommand command) @Override public void setEnabled(boolean isEnabled) + + @Override + public void setImpedanceEnabled(boolean isImpedanceEnabled) { - this.isEnabled.set(isEnabled); + this.isImpedanceEnabled = isImpedanceEnabled; } @Override @@ -425,10 +519,50 @@ public void computeInverseDynamics() ReferenceFrame trajectoryFrame = yoDesiredPose.getReferenceFrame(); + if (isImpedanceEnabled()) + { + computeInverseInertiaMatrix(); + } + computeProportionalTerm(linearProportionalFeedback, angularProportionalFeedback); computeDerivativeTerm(linearDerivativeFeedback, angularDerivativeFeedback); if (computeIntegralTerm) computeIntegralTerm(linearIntegralFeedback, angularIntegralFeedback); + + updateFeedForward(); + + if (isImpedanceEnabled()) + { + tempMatrix.reshape(6, 1); + + tempFeedbackMatrix.set(0, 0, angularProportionalFeedback.getX()); + tempFeedbackMatrix.set(1, 0, angularProportionalFeedback.getY()); + tempFeedbackMatrix.set(2, 0, angularProportionalFeedback.getZ()); + tempFeedbackMatrix.set(3, 0, linearProportionalFeedback.getX()); + tempFeedbackMatrix.set(4, 0, linearProportionalFeedback.getY()); + tempFeedbackMatrix.set(5, 0, linearProportionalFeedback.getZ()); + CommonOps_DDRM.mult(inverseInertiaMatrix, tempFeedbackMatrix, tempMatrix); + angularProportionalFeedback.setX(tempMatrix.get(0, 0)); + angularProportionalFeedback.setY(tempMatrix.get(1, 0)); + angularProportionalFeedback.setZ(tempMatrix.get(2, 0)); + linearProportionalFeedback.setX(tempMatrix.get(3, 0)); + linearProportionalFeedback.setY(tempMatrix.get(4, 0)); + linearProportionalFeedback.setZ(tempMatrix.get(5, 0)); + + tempFeedbackMatrix.set(0, 0, angularDerivativeFeedback.getX()); + tempFeedbackMatrix.set(1, 0, angularDerivativeFeedback.getY()); + tempFeedbackMatrix.set(2, 0, angularDerivativeFeedback.getZ()); + tempFeedbackMatrix.set(3, 0, linearDerivativeFeedback.getX()); + tempFeedbackMatrix.set(4, 0, linearDerivativeFeedback.getY()); + tempFeedbackMatrix.set(5, 0, linearDerivativeFeedback.getZ()); + CommonOps_DDRM.mult(inverseInertiaMatrix, tempFeedbackMatrix, tempMatrix); + angularDerivativeFeedback.setX(tempMatrix.get(0, 0)); + angularDerivativeFeedback.setY(tempMatrix.get(1, 0)); + angularDerivativeFeedback.setZ(tempMatrix.get(2, 0)); + linearDerivativeFeedback.setX(tempMatrix.get(3, 0)); + linearDerivativeFeedback.setY(tempMatrix.get(4, 0)); + linearDerivativeFeedback.setZ(tempMatrix.get(5, 0)); + } feedForwardLinearAction.setIncludingFrame(yoFeedForwardAcceleration.getLinearPart()); feedForwardAngularAction.setIncludingFrame(yoFeedForwardAcceleration.getAngularPart()); feedForwardLinearAction.changeFrame(controlFrame); @@ -477,7 +611,10 @@ public void computeInverseDynamics() yoDesiredAcceleration.changeFrame(trajectoryFrame); yoDesiredAcceleration.setCommandId(currentCommandId); - addCoriolisAcceleration(desiredLinearAcceleration); + if (!isImpedanceEnabled()) + { + addCoriolisAcceleration(desiredLinearAcceleration); + } inverseDynamicsOutput.setSpatialAcceleration(controlFrame, desiredAngularAcceleration, desiredLinearAcceleration); } @@ -488,6 +625,11 @@ public void computeInverseKinematics() if (!isEnabled()) return; + if (isImpedanceEnabled()) + { + throw new FeedbackControllerException("Impedance control is not implemented in computeInverseKinematics."); + } + inverseKinematicsOutput.setProperties(inverseDynamicsOutput); ReferenceFrame trajectoryFrame = yoDesiredPose.getReferenceFrame(); @@ -531,6 +673,11 @@ public void computeVirtualModelControl() if (!isEnabled()) return; + if (isImpedanceEnabled()) + { + throw new FeedbackControllerException("Impedance control is not implemented in computeVirtualModelControl."); + } + computeFeedbackWrench(); if (isRootBody) @@ -746,10 +893,76 @@ protected void computeDerivativeTerm(FrameVector3D linearFeedbackTermToPack, Fra angularFeedbackTermToPack.changeFrame(angularGainsFrame != null ? angularGainsFrame : controlFrame); positionGains.getDerivativeGainMatrix(tempGainMatrix); - tempGainMatrix.transform(linearFeedbackTermToPack); + orientationGains.getDerivativeGainMatrix(tempMatrix3D); - orientationGains.getDerivativeGainMatrix(tempGainMatrix); - tempGainMatrix.transform(angularFeedbackTermToPack); + if (isImpedanceEnabled()) + { + positionGains.getFullProportionalGainMatrix(tempLinearMatrix, 3); + orientationGains.getFullProportionalGainMatrix(tempAngularMatrix, 0); + + CommonOps_DDRM.mult(tempAngularMatrix, tempLinearMatrix, tempMatrix); + + sqrtProportionalGainMatrix.reshape(6,6); + sqrtInertiaMatrix.reshape(6,6); + + MatrixMissingTools.sqrt(tempMatrix, sqrtProportionalGainMatrix, tempSqrtMatrix, U, W, Vt, svd); + // LogTools.info("K_p^{1/2} = " + sqrtProportionalGainMatrix); + + tempMatrix.set(inverseInertiaMatrix); + + MatrixMissingTools.invert(tempMatrix, inverseSolver); + MatrixMissingTools.sqrt(tempMatrix, sqrtInertiaMatrix, tempSqrtMatrix, U, W, Vt, svd); + + // for (int row = 0; row < tempMatrix.getNumRows(); row++) + // { + // for (int col = 0; col < tempMatrix.getNumCols(); col++) + // { + // yoTestMatrix.set(row, col, tempMatrix.get(row, col)); + // } + // } + // LogTools.info("End effector: " + endEffector.getName() + " - Inertia matrix: " + tempMatrix); + + tempMatrix.reshape(6,6); + CommonOps_DDRM.mult(sqrtInertiaMatrix, dampingRatioMatrix, tempMatrix); + CommonOps_DDRM.mult(tempMatrix, sqrtProportionalGainMatrix, tempDerivativeGainMatrix); + CommonOps_DDRM.mult(sqrtProportionalGainMatrix, dampingRatioMatrix, tempMatrix); + CommonOps_DDRM.multAdd(tempMatrix, sqrtInertiaMatrix, tempDerivativeGainMatrix); + + computeCoriolisMatrix(); + CommonOps_DDRM.add(tempDerivativeGainMatrix,-1, coriolisMatrix, tempDerivativeGainMatrix); + + feedbacktermsMatrix.set(0, 0, angularFeedbackTermToPack.getX()); + feedbacktermsMatrix.set(1, 0, angularFeedbackTermToPack.getY()); + feedbacktermsMatrix.set(2, 0, angularFeedbackTermToPack.getZ()); + feedbacktermsMatrix.set(3, 0, linearFeedbackTermToPack.getX()); + feedbacktermsMatrix.set(4, 0, linearFeedbackTermToPack.getY()); + feedbacktermsMatrix.set(5, 0, linearFeedbackTermToPack.getZ()); + + tempMatrix.reshape(6,1); + CommonOps_DDRM.mult(tempDerivativeGainMatrix, feedbacktermsMatrix, tempMatrix); + + linearFeedbackTermToPack.setX(tempMatrix.get(3, 0)); + linearFeedbackTermToPack.setY(tempMatrix.get(4, 0)); + linearFeedbackTermToPack.setZ(tempMatrix.get(5, 0)); + + angularFeedbackTermToPack.setX(tempMatrix.get(0, 0)); + angularFeedbackTermToPack.setY(tempMatrix.get(1, 0)); + angularFeedbackTermToPack.setZ(tempMatrix.get(2, 0)); + + // for (int row = 0; row < 6; row++) + // { + // for (int col = 0; col < 6; col++) + // { + // yoDerivativeTermMatrix.set(row, col, tempDerivativeGainMatrix.get(row, col)); + // yoCoriolisMatrix.set(row, col, coriolisMatrix.get(row, col)); + // } + // } + } + else + { + tempGainMatrix.transform(linearFeedbackTermToPack); + tempMatrix3D.transform(angularFeedbackTermToPack); + } linearFeedbackTermToPack.changeFrame(controlFrame); angularFeedbackTermToPack.changeFrame(controlFrame); @@ -952,12 +1165,89 @@ protected void proccessInverseDynamicsDesiredAcceleration(MovingReferenceFrame c { } + private void computeInverseInertiaMatrix() + { + jacobianCalculator.clear(); + jacobianCalculator.setKinematicChain(bodyBase, endEffector); + jacobianCalculator.setJacobianFrame(controlFrame); + jacobianCalculator.reset(); + + // Jacobian is an M x N matrix, M is called the task size and + // * N is the overall number of degrees of freedom (DoFs) to be controlled. + jacobianMatrix.set(jacobianCalculator.getJacobianMatrix()); + jacobianMatrix.reshape(jacobianMatrix.getNumRows(), jacobianMatrix.getNumCols()); + + tempMatrix.reshape(activeAxis.size(), jacobianMatrix.getNumCols()); + MatrixMissingTools.extractRows(jacobianMatrix, activeAxis, tempMatrix); + jacobianMatrix.set(tempMatrix); + + subMassMatrix.set(massMatrixCalculator.getMassMatrix()); + massMatrixCalculator.reset(); + subMassMatrix.reshape(subMassMatrix.getNumRows(), subMassMatrix.getNumCols()); + + subMassInverseMatrix.reshape(jointIndices.size(), jointIndices.size()); + MatrixMissingTools.extract(subMassMatrix, jointIndices, jointIndices, subMassInverseMatrix); + + MatrixMissingTools.invert(subMassInverseMatrix, inverseSolver); + + inverseInertiaTempMatrix.reshape(jointIndices.size(), jointIndices.size()); + CommonOps_DDRM.mult(jacobianMatrix, subMassInverseMatrix, inverseInertiaTempMatrix); + CommonOps_DDRM.multTransB(inverseInertiaTempMatrix, jacobianMatrix, tempMatrix); + + inverseInertiaMatrix.set(identityMatrix); + MatrixMissingTools.insert(tempMatrix, inverseInertiaMatrix, activeAxis, activeAxis); + } + + // This is \bar{C} for the Derivative term + private void computeCoriolisMatrix() + { + inertiaMatrix.reshape(inverseInertiaMatrix.getNumRows(), inverseInertiaMatrix.getNumCols()); + inertiaMatrix.set(inverseInertiaMatrix); + MatrixMissingTools.invert(inertiaMatrix, inverseSolver); + + if (savedInertiaMatrix.getNumCols()==0) + { + savedInertiaMatrix.set(inertiaMatrix); + savedInertiaMatrix.reshape(savedInertiaMatrix.getNumRows(), savedInertiaMatrix.getNumCols()); + coriolisMatrix.reshape(savedInertiaMatrix.getNumRows(), savedInertiaMatrix.getNumCols()); + coriolisMatrix.zero(); + return; + } + + coriolisMatrix.reshape(inertiaMatrix.getNumRows(), inertiaMatrix.getNumCols()); + CommonOps_DDRM.add(inertiaMatrix, -1, savedInertiaMatrix, coriolisMatrix); + CommonOps_DDRM.scale(0.5/dt, coriolisMatrix); + savedInertiaMatrix.set(inertiaMatrix); + } + + private void updateFeedForward() + { + linearProportionalFeedback.changeFrame(worldFrame); + angularProportionalFeedback.changeFrame(worldFrame); + linearDerivativeFeedback.changeFrame(worldFrame); + angularDerivativeFeedback.changeFrame(worldFrame); + yoPositionFeedback.set(linearProportionalFeedback); + yoPositionFeedback.add(linearDerivativeFeedback); + yoOrientationFeedback.set(angularProportionalFeedback); + yoOrientationFeedback.add(angularDerivativeFeedback); + linearProportionalFeedback.changeFrame(controlFrame); + angularProportionalFeedback.changeFrame(controlFrame); + linearDerivativeFeedback.changeFrame(controlFrame); + angularDerivativeFeedback.changeFrame(controlFrame); + } + @Override public boolean isEnabled() { return isEnabled.getBooleanValue(); } + @Override + public boolean isImpedanceEnabled() + { + return isImpedanceEnabled; + } + @Override public SpatialAccelerationCommand getInverseDynamicsOutput() { diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManagerTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManagerTest.java index 1679438e353..eb9e631fc78 100644 --- a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManagerTest.java +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManagerTest.java @@ -47,6 +47,7 @@ import us.ihmc.yoVariables.parameters.DefaultParameterReader; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import java.util.ArrayList; @@ -506,6 +507,8 @@ private RigidBodyControlManager createManager() Vector3D taskspaceAngularWeight = new Vector3D(1.0, 1.0, 1.0); Vector3D taskspaceLinearWeight = new Vector3D(1.0, 1.0, 1.0); double nominalRhoWeight = 1.0; + YoBoolean isImpedanceEnabled = new YoBoolean("isImpedanceEnabled", testRegistry); + isImpedanceEnabled.set(false); RigidBodyControlManager manager = new RigidBodyControlManager(bodyToControl, baseBody, @@ -518,10 +521,13 @@ private RigidBodyControlManager createManager() taskspaceLinearWeight, taskspaceOrientationGains, taskspacePositionGains, + null, + null, contactableBody, null, null, false, + isImpedanceEnabled, nominalRhoWeight, WholeBodyPostureAdjustmentProvider.createZeroPostureAdjustmentProvider(), yoTime, diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceOrientationFeedbackControllerTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceOrientationFeedbackControllerTest.java new file mode 100644 index 00000000000..0453163d8c3 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceOrientationFeedbackControllerTest.java @@ -0,0 +1,140 @@ +package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.taskspace; + +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.MatrixFeatures_DDRM; +import org.ejml.dense.row.NormOps_DDRM; +import org.junit.jupiter.api.Test; +import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox; +import us.ihmc.commonWalkingControlModules.controllerCore.WholeBodyControlCoreToolbox; +import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.OrientationFeedbackControlCommand; +import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.SpatialFeedbackControlCommand; +import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.SpatialAccelerationCommand; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.MotionQPInputCalculator; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.NativeQPInputTypeA; +import us.ihmc.commons.RandomNumbers; +import us.ihmc.euclid.referenceFrame.FrameQuaternion; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.matrixlib.NativeMatrix; +import us.ihmc.mecano.frames.CenterOfMassReferenceFrame; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools.RandomFloatingRevoluteJointChain; +import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPID3DGains; +import us.ihmc.yoVariables.registry.YoRegistry; + +import java.util.List; +import java.util.Random; + +import static us.ihmc.robotics.Assert.assertTrue; + +public class ImpedanceOrientationFeedbackControllerTest +{ + private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + + // Test already didn't work. Seems to be an issue with SpatialController and not the Impedance controller + @Test + public void testCompareAgainstSpatialController() throws Exception + { + Random random = new Random(5641654L); + + YoRegistry registry = new YoRegistry("Dummy"); + int numberOfRevoluteJoints = 10; + RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, numberOfRevoluteJoints); + List joints = randomFloatingChain.getRevoluteJoints(); + RigidBodyBasics elevator = randomFloatingChain.getElevator(); + RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); + + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + double controlDT = 0.004; + + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, jointsToOptimizeFor, centerOfMassFrame, null, null, + registry); + toolbox.setupForInverseDynamicsSolver(null); + // Making the controllers to run with different instances of the toolbox so they don't share variables. + OrientationFeedbackController orientationFeedbackController = new OrientationFeedbackController(endEffector, toolbox, new FeedbackControllerToolbox(new YoRegistry("Dummy")), registry); + SpatialFeedbackController spatialFeedbackController = new SpatialFeedbackController(endEffector, toolbox, new FeedbackControllerToolbox(new YoRegistry("Dummy")), registry); + orientationFeedbackController.setEnabled(true); + spatialFeedbackController.setEnabled(true); + + OrientationFeedbackControlCommand orientationFeedbackControlCommand = new OrientationFeedbackControlCommand(); + orientationFeedbackControlCommand.set(elevator, endEffector); + PID3DGains orientationGains = new DefaultPID3DGains(); + + SpatialFeedbackControlCommand spatialFeedbackControlCommand = new SpatialFeedbackControlCommand(); + spatialFeedbackControlCommand.set(elevator, endEffector); + spatialFeedbackControlCommand.getSpatialAccelerationCommand().setSelectionMatrixForAngularControl(); + + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + NativeQPInputTypeA orientationMotionQPInput = new NativeQPInputTypeA(toolbox.getJointIndexHandler().getNumberOfDoFs()); + NativeQPInputTypeA spatialMotionQPInput = new NativeQPInputTypeA(toolbox.getJointIndexHandler().getNumberOfDoFs()); + + SpatialAccelerationCommand orientationControllerOutput = orientationFeedbackController.getInverseDynamicsOutput(); + SpatialAccelerationCommand spatialControllerOutput = spatialFeedbackController.getInverseDynamicsOutput(); + + for (int i = 0; i < 300; i++) + { + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + centerOfMassFrame.update(); + + double proportionalGain = RandomNumbers.nextDouble(random, 10.0, 200.0); + double derivativeGain = RandomNumbers.nextDouble(random, 0.0, 100.0); + double integralGain = RandomNumbers.nextDouble(random, 0.0, 100.0); + double maxIntegralError = RandomNumbers.nextDouble(random, 0.0, 10.0); + orientationGains.setGains(proportionalGain, derivativeGain, integralGain, maxIntegralError); + orientationGains.setMaxProportionalError(RandomNumbers.nextDouble(random, 0.0, 10.0)); + orientationGains.setMaxDerivativeError(RandomNumbers.nextDouble(random, 0.0, 10.0)); + orientationGains.setMaxFeedbackAndFeedbackRate(RandomNumbers.nextDouble(random, 0.1, 10.0), RandomNumbers.nextDouble(random, 0.1, 10.0)); + orientationGains.setDampingRatios(1.0); + orientationFeedbackControlCommand.setGains(orientationGains); + spatialFeedbackControlCommand.setOrientationGains(orientationGains); + + FrameQuaternion desiredOrientation = new FrameQuaternion(worldFrame, EuclidCoreRandomTools.nextQuaternion(random)); + FrameVector3D desiredAngularVelocity = new FrameVector3D(worldFrame, EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0)); + FrameVector3D feedForwardAngularAcceleration = new FrameVector3D(worldFrame, EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0)); + + orientationFeedbackControlCommand.setInverseDynamics(desiredOrientation, desiredAngularVelocity, feedForwardAngularAcceleration); + spatialFeedbackControlCommand.setInverseDynamics(desiredOrientation, desiredAngularVelocity, feedForwardAngularAcceleration); + + spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand); + orientationFeedbackController.submitFeedbackControlCommand(orientationFeedbackControlCommand); + + spatialFeedbackController.computeInverseDynamics(); + orientationFeedbackController.computeInverseDynamics(); + + motionQPInputCalculator.convertSpatialAccelerationCommand(orientationControllerOutput, orientationMotionQPInput); + motionQPInputCalculator.convertSpatialAccelerationCommand(spatialControllerOutput, spatialMotionQPInput); + + assertEquals(spatialMotionQPInput.taskJacobian, orientationMotionQPInput.taskJacobian, 1.0e-12); + assertEquals(spatialMotionQPInput.taskObjective, orientationMotionQPInput.taskObjective, 1.0e-12); + assertEquals(spatialMotionQPInput.taskWeightMatrix, orientationMotionQPInput.taskWeightMatrix, 1.0e-12); + } + } + + private static void assertEquals(NativeMatrix expected, NativeMatrix actual, double epsilon) + { + assertEquals(new DMatrixRMaj(expected), new DMatrixRMaj(actual), epsilon); + } + + private static void assertEquals(DMatrixRMaj expected, DMatrixRMaj actual, double epsilon) + { + assertTrue(assertErrorMessage(expected, actual), MatrixFeatures_DDRM.isEquals(expected, actual, epsilon)); + } + + private static String assertErrorMessage(DMatrixRMaj expected, DMatrixRMaj actual) + { + DMatrixRMaj diff = new DMatrixRMaj(expected.getNumRows(), expected.getNumCols()); + CommonOps_DDRM.subtract(expected, actual, diff); + return "Expected:\n" + expected + "\nActual:\n" + actual + ", difference: " + NormOps_DDRM.normP2(diff); + } +} \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedancePointFeedbackControllerTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedancePointFeedbackControllerTest.java new file mode 100644 index 00000000000..5e9365377dd --- /dev/null +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedancePointFeedbackControllerTest.java @@ -0,0 +1,446 @@ +package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.taskspace; + +import java.util.List; +import java.util.Random; + +import org.ejml.EjmlUnitTests; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; +import org.ejml.interfaces.linsol.LinearSolverDense; +import org.junit.jupiter.api.Test; + +import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox; +import us.ihmc.commonWalkingControlModules.controllerCore.WholeBodyControlCoreToolbox; +import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.PointFeedbackControlCommand; +import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.SpatialFeedbackControlCommand; +import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.SpatialAccelerationCommand; +import us.ihmc.commonWalkingControlModules.inverseKinematics.RobotJointVelocityAccelerationIntegrator; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.MotionQPInputCalculator; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.NativeQPInputTypeA; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.QPInputTypeA; +import us.ihmc.commons.RandomNumbers; +import us.ihmc.convexOptimization.quadraticProgram.OASESConstrainedQPSolver; +import us.ihmc.convexOptimization.quadraticProgram.SimpleEfficientActiveSetQPSolver; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameRandomTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.matrixlib.MatrixTools; +import us.ihmc.matrixlib.NativeCommonOps; +import us.ihmc.matrixlib.NativeMatrix; +import us.ihmc.mecano.frames.CenterOfMassReferenceFrame; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.RigidBody; +import us.ihmc.mecano.multiBodySystem.SixDoFJoint; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools.RandomFloatingRevoluteJointChain; +import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPID3DGains; +import us.ihmc.yoVariables.registry.YoRegistry; + +import static org.junit.jupiter.api.Assertions.*; + +public final class ImpedancePointFeedbackControllerTest +{ + private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + + @Test + public void testBaseFrame() + { + double controlDT = 0.004; + double simulationTime = 4.0; + int integrationSteps = 100; + Random random = new Random(562968L); + YoRegistry registry = new YoRegistry("TestRegistry"); + + // Create two floating joints. This test attempts to control the end effector body with respect to the moving base body. + RigidBody elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame()); + SixDoFJoint joint1 = MultiBodySystemRandomTools.nextSixDoFJoint(random, "joint1", elevator); + RigidBody baseBody = MultiBodySystemRandomTools.nextRigidBody(random, "baseBody", joint1); + SixDoFJoint joint2 = MultiBodySystemRandomTools.nextSixDoFJoint(random, "joint2", baseBody); + RigidBody endEffector = MultiBodySystemRandomTools.nextRigidBody(random, "endEffector", joint2); + + // Create the feedback controller for the end effector. + JointBasics[] joints = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, joints, centerOfMassFrame, null, null, registry); + toolbox.setupForInverseDynamicsSolver(null); + FeedbackControllerToolbox feedbackControllerToolbox = new FeedbackControllerToolbox(registry); + PointFeedbackController pointFeedbackController = new PointFeedbackController(endEffector, toolbox, feedbackControllerToolbox, registry); + + // Scramble the joint states. + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + elevator.updateFramesRecursively(); + + // Create a command with baseBody as base and desired values in the baseBody frame. + FramePoint3D desiredPosition = EuclidFrameRandomTools.nextFramePoint3D(random, baseBody.getBodyFixedFrame()); + FrameVector3D zero = new FrameVector3D(desiredPosition.getReferenceFrame()); + PID3DGains gains = new DefaultPID3DGains(); + gains.setProportionalGains(2.0); + gains.setDerivativeGains(Double.NaN); + gains.setDampingRatios(1.0); + PointFeedbackControlCommand pointFeedbackControlCommand = new PointFeedbackControlCommand(); + pointFeedbackControlCommand.set(elevator, endEffector); + pointFeedbackControlCommand.setGains(gains); + pointFeedbackControlCommand.setInverseDynamics(desiredPosition, zero, zero); + pointFeedbackController.submitFeedbackControlCommand(pointFeedbackControlCommand); + pointFeedbackController.setEnabled(true); + pointFeedbackController.setImpedanceEnabled(true); + + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(MultiBodySystemTools.computeDegreesOfFreedom(joints)); + DMatrixRMaj jointAccelerations = new DMatrixRMaj(0, 0); + double damping = 0.001; + RobotJointVelocityAccelerationIntegrator integrator = new RobotJointVelocityAccelerationIntegrator(controlDT / integrationSteps); + + for (int i = 0; i < simulationTime / controlDT; i++) + { + pointFeedbackController.computeInverseDynamics(); + SpatialAccelerationCommand spatialAccelerationCommand = pointFeedbackController.getInverseDynamicsOutput(); + assertTrue(motionQPInputCalculator.convertSpatialAccelerationCommand(spatialAccelerationCommand, motionQPInput)); + NativeCommonOps.solveDamped(new DMatrixRMaj(motionQPInput.getTaskJacobian()), new DMatrixRMaj(motionQPInput.getTaskObjective()), damping, jointAccelerations); + + // Need to do a fine grain integration since the point we care about will be the center of rotation of the body and we need + // to maintain that situation. + for (int j = 0; j < integrationSteps; j++) + { + integrator.integrateJointAccelerations(joints, jointAccelerations); + integrator.integrateJointVelocities(joints, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(joints, JointStateType.VELOCITY, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(joints, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); + elevator.updateFramesRecursively(); + } + } + + // Assert position is close to desired + FramePoint3D position = new FramePoint3D(endEffector.getBodyFixedFrame()); + position.changeFrame(desiredPosition.getReferenceFrame()); + EuclidCoreTestTools.assertEquals(desiredPosition, position, 1.0E-5); + } + + @Test + public void testConvergence() throws Exception + { + Random random = new Random(5641654L); + + int numberOfJoints = 10; + Vector3D[] jointAxes = new Vector3D[numberOfJoints]; + for (int i = 0; i < numberOfJoints; i++) + jointAxes[i] = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); + + YoRegistry registry = new YoRegistry("Dummy"); + RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); + List joints = randomFloatingChain.getRevoluteJoints(); + RigidBodyBasics elevator = randomFloatingChain.getElevator(); + RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); + FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0); + + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + FramePoint3D desiredPosition = new FramePoint3D(); + desiredPosition.setIncludingFrame(bodyFixedPointToControl); + desiredPosition.changeFrame(worldFrame); + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + double controlDT = 0.004; + + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, jointsToOptimizeFor, centerOfMassFrame, null, null, + registry); + toolbox.setupForInverseDynamicsSolver(null); + FeedbackControllerToolbox feedbackControllerToolbox = new FeedbackControllerToolbox(registry); + PointFeedbackController pointFeedbackController = new PointFeedbackController(endEffector, toolbox, feedbackControllerToolbox, registry); + + PointFeedbackControlCommand pointFeedbackControlCommand = new PointFeedbackControlCommand(); + pointFeedbackControlCommand.set(elevator, endEffector); + PID3DGains gains = new DefaultPID3DGains(); + gains.setProportionalGains(5.0); + gains.setDerivativeGains(Double.NaN); + gains.setDampingRatios(1.0); + pointFeedbackControlCommand.setGains(gains); + pointFeedbackControlCommand.setBodyFixedPointToControl(bodyFixedPointToControl); + pointFeedbackControlCommand.setInverseDynamics(desiredPosition, new FrameVector3D(worldFrame), new FrameVector3D(worldFrame)); + pointFeedbackController.submitFeedbackControlCommand(pointFeedbackControlCommand); + pointFeedbackController.setEnabled(true); + pointFeedbackController.setImpedanceEnabled(true); + + int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor); + NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(numberOfDoFs); + LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); + DMatrixRMaj jInverse = new DMatrixRMaj(numberOfDoFs, 6); + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + DMatrixRMaj jointAccelerations = new DMatrixRMaj(numberOfDoFs, 1); + RobotJointVelocityAccelerationIntegrator integrator = new RobotJointVelocityAccelerationIntegrator(controlDT); + + FramePoint3D currentPosition = new FramePoint3D(); + FrameVector3D errorVector = new FrameVector3D(); + + double previousErrorMagnitude = Double.POSITIVE_INFINITY; + double errorMagnitude = previousErrorMagnitude; + + for (int i = 0; i < 100; i++) + { + pointFeedbackController.computeInverseDynamics(); + SpatialAccelerationCommand output = pointFeedbackController.getInverseDynamicsOutput(); + + motionQPInputCalculator.convertSpatialAccelerationCommand(output, motionQPInput); + pseudoInverseSolver.setA(new DMatrixRMaj(motionQPInput.taskJacobian)); + pseudoInverseSolver.invert(jInverse); + CommonOps_DDRM.mult(jInverse, new DMatrixRMaj(motionQPInput.taskObjective), jointAccelerations); + + integrator.integrateJointAccelerations(jointsToOptimizeFor, jointAccelerations); + integrator.integrateJointVelocities(jointsToOptimizeFor, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); + elevator.updateFramesRecursively(); + + currentPosition.setIncludingFrame(bodyFixedPointToControl); + currentPosition.changeFrame(worldFrame); + errorVector.sub(desiredPosition, currentPosition); + errorMagnitude = errorVector.norm(); + boolean isErrorReducing = errorMagnitude < previousErrorMagnitude; + assertTrue(isErrorReducing); + previousErrorMagnitude = errorMagnitude; + } + } + + @Test + public void testConvergenceWithJerryQP() throws Exception + { + Random random = new Random(5641654L); + + int numberOfJoints = 10; + Vector3D[] jointAxes = new Vector3D[numberOfJoints]; + for (int i = 0; i < numberOfJoints; i++) + jointAxes[i] = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); + + YoRegistry registry = new YoRegistry("Dummy"); + RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); + List joints = randomFloatingChain.getRevoluteJoints(); + RigidBodyBasics elevator = randomFloatingChain.getElevator(); + RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); + FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0); + + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, 0.0, 1.0, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + FramePoint3D desiredPosition = new FramePoint3D(); + desiredPosition.setIncludingFrame(bodyFixedPointToControl); + desiredPosition.changeFrame(worldFrame); + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, 0.0, 1.0, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + double controlDT = 0.004; + + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, jointsToOptimizeFor, centerOfMassFrame, null, null, + registry); + toolbox.setupForInverseDynamicsSolver(null); + FeedbackControllerToolbox feedbackControllerToolbox = new FeedbackControllerToolbox(registry); + PointFeedbackController pointFeedbackController = new PointFeedbackController(endEffector, toolbox, feedbackControllerToolbox, registry); + + PointFeedbackControlCommand pointFeedbackControlCommand = new PointFeedbackControlCommand(); + pointFeedbackControlCommand.set(elevator, endEffector); + PID3DGains gains = new DefaultPID3DGains(); + gains.setProportionalGains(5.0); + gains.setDerivativeGains(Double.NaN); + gains.setDampingRatios(1.0); + pointFeedbackControlCommand.setGains(gains); + pointFeedbackControlCommand.setBodyFixedPointToControl(bodyFixedPointToControl); + pointFeedbackControlCommand.setInverseDynamics(desiredPosition, new FrameVector3D(worldFrame), new FrameVector3D(worldFrame)); + pointFeedbackController.submitFeedbackControlCommand(pointFeedbackControlCommand); + pointFeedbackController.setEnabled(true); + pointFeedbackController.setImpedanceEnabled(true); + + int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor); + NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(numberOfDoFs); + LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); + DMatrixRMaj jInverse = new DMatrixRMaj(numberOfDoFs, 6); + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + DMatrixRMaj jointAccelerations = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj jointAccelerationsFromJerryQP = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj jointAccelerationsFromQPOASES = new DMatrixRMaj(numberOfDoFs, 1); + RobotJointVelocityAccelerationIntegrator integrator = new RobotJointVelocityAccelerationIntegrator(controlDT); + + SimpleEfficientActiveSetQPSolver jerryQPSolver = new SimpleEfficientActiveSetQPSolver(); + OASESConstrainedQPSolver oasesQPSolver = new OASESConstrainedQPSolver(); + + DMatrixRMaj solverInput_H = new DMatrixRMaj(numberOfDoFs, numberOfDoFs); + DMatrixRMaj solverInput_f = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj solverInput_Aeq = new DMatrixRMaj(0, numberOfDoFs); + DMatrixRMaj solverInput_beq = new DMatrixRMaj(0, 1); + DMatrixRMaj solverInput_Ain = new DMatrixRMaj(0, numberOfDoFs); + DMatrixRMaj solverInput_bin = new DMatrixRMaj(0, 1); + DMatrixRMaj solverInput_lb = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj solverInput_ub = new DMatrixRMaj(numberOfDoFs, 1); + CommonOps_DDRM.fill(solverInput_lb, Double.NEGATIVE_INFINITY); + CommonOps_DDRM.fill(solverInput_ub, Double.POSITIVE_INFINITY); + + DMatrixRMaj tempJtW = new DMatrixRMaj(numberOfDoFs, 3); + + FramePoint3D currentPosition = new FramePoint3D(); + FrameVector3D errorVector = new FrameVector3D(); + + double previousErrorMagnitude = Double.POSITIVE_INFINITY; + double errorMagnitude = previousErrorMagnitude; + + for (int i = 0; i < 100; i++) + { + pointFeedbackController.computeInverseDynamics(); + SpatialAccelerationCommand output = pointFeedbackController.getInverseDynamicsOutput(); + motionQPInputCalculator.convertSpatialAccelerationCommand(output, motionQPInput); + + MatrixTools.scaleTranspose(1.0, new DMatrixRMaj(motionQPInput.taskJacobian), tempJtW); // J^T W + CommonOps_DDRM.mult(tempJtW, new DMatrixRMaj(motionQPInput.taskJacobian), solverInput_H); // H = J^T W J + CommonOps_DDRM.mult(tempJtW, new DMatrixRMaj(motionQPInput.taskObjective), solverInput_f); // f = - J^T W xDDot + CommonOps_DDRM.scale(-1.0, solverInput_f); + + for (int diag = 0; diag < numberOfDoFs; diag++) + solverInput_H.add(diag, diag, 1e-5); + + jerryQPSolver.clear(); + jerryQPSolver.setQuadraticCostFunction(solverInput_H, solverInput_f, 0.0); + jerryQPSolver.solve(jointAccelerationsFromJerryQP); + oasesQPSolver.solve(solverInput_H, solverInput_f, solverInput_Aeq, solverInput_beq, solverInput_Ain, solverInput_bin, solverInput_lb, solverInput_ub, + jointAccelerationsFromQPOASES, true); + + pseudoInverseSolver.setA(new DMatrixRMaj(motionQPInput.taskJacobian)); + pseudoInverseSolver.invert(jInverse); + CommonOps_DDRM.mult(jInverse, new DMatrixRMaj(motionQPInput.taskObjective), jointAccelerations); + + integrator.integrateJointAccelerations(jointsToOptimizeFor, jointAccelerationsFromJerryQP); + integrator.integrateJointVelocities(jointsToOptimizeFor, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); + elevator.updateFramesRecursively(); + + assertArrayEquals(jointAccelerations.data, jointAccelerationsFromJerryQP.data, 1.0e-4); + assertArrayEquals(jointAccelerations.data, jointAccelerationsFromQPOASES.data, 2.0e-1); + + currentPosition.setIncludingFrame(bodyFixedPointToControl); + currentPosition.changeFrame(worldFrame); + errorVector.sub(desiredPosition, currentPosition); + errorMagnitude = errorVector.norm(); + boolean isErrorReducing = errorMagnitude < previousErrorMagnitude; + assertTrue(isErrorReducing); + previousErrorMagnitude = errorMagnitude; + } + } + + @Test + public void testCompareAgainstSpatialController() throws Exception + { + Random random = new Random(5641654L); + + YoRegistry registry = new YoRegistry("Dummy"); + int numberOfRevoluteJoints = 10; + RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, numberOfRevoluteJoints); + List joints = randomFloatingChain.getRevoluteJoints(); + RigidBodyBasics elevator = randomFloatingChain.getElevator(); + RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); + + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + double controlDT = 0.004; + + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, jointsToOptimizeFor, centerOfMassFrame, null, null, + registry); + toolbox.setupForInverseDynamicsSolver(null); + // Making the controllers to run with different instances of the toolbox so they don't share variables. + PointFeedbackController pointFeedbackController = new PointFeedbackController(endEffector, toolbox, new FeedbackControllerToolbox(new YoRegistry("Dummy")), registry); + SpatialFeedbackController spatialFeedbackController = new SpatialFeedbackController(endEffector, toolbox, new FeedbackControllerToolbox(new YoRegistry("Dummy")), registry); + pointFeedbackController.setEnabled(true); + spatialFeedbackController.setEnabled(true); + + PointFeedbackControlCommand pointFeedbackControlCommand = new PointFeedbackControlCommand(); + pointFeedbackControlCommand.set(elevator, endEffector); + PID3DGains positionGains = new DefaultPID3DGains(); + + SpatialFeedbackControlCommand spatialFeedbackControlCommand = new SpatialFeedbackControlCommand(); + spatialFeedbackControlCommand.set(elevator, endEffector); + spatialFeedbackControlCommand.getSpatialAccelerationCommand().setSelectionMatrixForLinearControl(); + + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + NativeQPInputTypeA pointMotionQPInput = new NativeQPInputTypeA(toolbox.getJointIndexHandler().getNumberOfDoFs()); + NativeQPInputTypeA spatialMotionQPInput = new NativeQPInputTypeA(toolbox.getJointIndexHandler().getNumberOfDoFs()); + + SpatialAccelerationCommand pointControllerOutput = pointFeedbackController.getInverseDynamicsOutput(); + SpatialAccelerationCommand spatialControllerOutput = spatialFeedbackController.getInverseDynamicsOutput(); + + for (int i = 0; i < 300; i++) + { + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + centerOfMassFrame.update(); + + double proportionalGain = RandomNumbers.nextDouble(random, 10.0, 200.0); + double derivativeGain = RandomNumbers.nextDouble(random, 0.0, 100.0); + double integralGain = RandomNumbers.nextDouble(random, 0.0, 100.0); + double maxIntegralError = RandomNumbers.nextDouble(random, 0.0, 10.0); + positionGains.setGains(proportionalGain, derivativeGain, integralGain, maxIntegralError); + positionGains.setMaxProportionalError(RandomNumbers.nextDouble(random, 0.0, 10.0)); + positionGains.setMaxDerivativeError(RandomNumbers.nextDouble(random, 0.0, 10.0)); + positionGains.setMaxFeedbackAndFeedbackRate(RandomNumbers.nextDouble(random, 0.1, 10.0), RandomNumbers.nextDouble(random, 0.1, 10.0)); + positionGains.setDampingRatios(1.0); + pointFeedbackControlCommand.setGains(positionGains); + spatialFeedbackControlCommand.setPositionGains(positionGains); + + FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0); + FramePoint3D desiredPosition = new FramePoint3D(worldFrame, EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0)); + FrameVector3D desiredLinearVelocity = new FrameVector3D(worldFrame, EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0)); + FrameVector3D feedForwardLinearAcceleration = new FrameVector3D(worldFrame, EuclidCoreRandomTools.nextVector3D(random, -10.0, 10.0)); + + pointFeedbackControlCommand.setBodyFixedPointToControl(bodyFixedPointToControl); + spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(bodyFixedPointToControl); + + pointFeedbackControlCommand.setInverseDynamics(desiredPosition, desiredLinearVelocity, feedForwardLinearAcceleration); + spatialFeedbackControlCommand.setInverseDynamics(desiredPosition, desiredLinearVelocity, feedForwardLinearAcceleration); + + spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand); + pointFeedbackController.submitFeedbackControlCommand(pointFeedbackControlCommand); + + spatialFeedbackController.computeInverseDynamics(); + pointFeedbackController.computeInverseDynamics(); + + motionQPInputCalculator.convertSpatialAccelerationCommand(pointControllerOutput, pointMotionQPInput); + motionQPInputCalculator.convertSpatialAccelerationCommand(spatialControllerOutput, spatialMotionQPInput); + + DMatrixRMaj pointDesiredAcceleration = new DMatrixRMaj(3, 1); + DMatrixRMaj spatialDesiredAcceleration = new DMatrixRMaj(3, 1); + pointControllerOutput.getDesiredSpatialAcceleration(pointDesiredAcceleration); + spatialControllerOutput.getDesiredSpatialAcceleration(spatialDesiredAcceleration); + + assertEquals(spatialDesiredAcceleration, pointDesiredAcceleration, 1.0e-12); + + assertEquals(spatialMotionQPInput.taskJacobian, pointMotionQPInput.taskJacobian, 1.0e-12); + assertEquals(spatialMotionQPInput.taskObjective, pointMotionQPInput.taskObjective, 1.0e-12); + assertEquals(spatialMotionQPInput.taskWeightMatrix, pointMotionQPInput.taskWeightMatrix, 1.0e-12); + } + } + + private static void assertEquals(NativeMatrix expected, NativeMatrix actual, double epsilon) + { + assertEquals(new DMatrixRMaj(expected), new DMatrixRMaj(actual), epsilon); + } + + private static void assertEquals(DMatrixRMaj expected, DMatrixRMaj actual, double epsilon) + { + EjmlUnitTests.assertEquals(actual, expected, epsilon); + } +} \ No newline at end of file diff --git a/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceSpatialFeedbackControllerTest.java b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceSpatialFeedbackControllerTest.java new file mode 100644 index 00000000000..3abe7e5ca88 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/test/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/ImpedanceSpatialFeedbackControllerTest.java @@ -0,0 +1,375 @@ +package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.taskspace; + +import java.util.List; +import java.util.Random; + +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; +import org.ejml.interfaces.linsol.LinearSolverDense; +import org.junit.jupiter.api.Test; + +import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox; +import us.ihmc.commonWalkingControlModules.controllerCore.WholeBodyControlCoreToolbox; +import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.SpatialFeedbackControlCommand; +import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.SpatialAccelerationCommand; +import us.ihmc.commonWalkingControlModules.inverseKinematics.RobotJointVelocityAccelerationIntegrator; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.MotionQPInputCalculator; +import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.NativeQPInputTypeA; +import us.ihmc.convexOptimization.quadraticProgram.OASESConstrainedQPSolver; +import us.ihmc.convexOptimization.quadraticProgram.SimpleEfficientActiveSetQPSolver; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.FrameQuaternion; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.matrixlib.MatrixTools; +import us.ihmc.matrixlib.NativeCommonOps; +import us.ihmc.mecano.frames.CenterOfMassReferenceFrame; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.RigidBody; +import us.ihmc.mecano.multiBodySystem.SixDoFJoint; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.SpatialVector; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools.RandomFloatingRevoluteJointChain; +import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.Assert; +import us.ihmc.robotics.controllers.pidGains.PIDSE3Gains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPIDSE3Gains; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.yoVariables.registry.YoRegistry; + +import static org.junit.jupiter.api.Assertions.*; + +public final class ImpedanceSpatialFeedbackControllerTest +{ + private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + + @Test + public void testBaseFrame() + { + double controlDT = 0.004; + double simulationTime = 30.0; + Random random = new Random(562968L); + YoRegistry registry = new YoRegistry("TestRegistry"); + + // Create two floating joints. This test attempts to control the end effector body with respect to the moving base body. + RigidBody elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame()); + SixDoFJoint joint1 = MultiBodySystemRandomTools.nextSixDoFJoint(random, "joint1", elevator); + RigidBody baseBody = MultiBodySystemRandomTools.nextRigidBody(random, "baseBody", joint1); + SixDoFJoint joint2 = MultiBodySystemRandomTools.nextSixDoFJoint(random, "joint2", baseBody); + RigidBody endEffector = MultiBodySystemRandomTools.nextRigidBody(random, "endEffector", joint2); + + // Create the feedback controller for the end effector. + JointBasics[] joints = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, joints, centerOfMassFrame, null, null, registry); + toolbox.setupForInverseDynamicsSolver(null); + FeedbackControllerToolbox feedbackControllerToolbox = new FeedbackControllerToolbox(registry); + SpatialFeedbackController spatialFeedbackController = new SpatialFeedbackController(endEffector, toolbox, feedbackControllerToolbox, registry); + + // Scramble the joint states. + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + elevator.updateFramesRecursively(); + + // Create a command with baseBody as base and desired values in the baseBody frame. + FramePose3D desiredPose = EuclidFrameRandomTools.nextFramePose3D(random, baseBody.getBodyFixedFrame()); + SpatialVector zero = new SpatialVector(desiredPose.getReferenceFrame()); + PIDSE3Gains gains = new DefaultPIDSE3Gains(); + gains.setPositionProportionalGains(2.0); + gains.setPositionDerivativeGains(Double.NaN); + gains.setOrientationProportionalGains(2.0); + gains.setOrientationDerivativeGains(Double.NaN); + gains.getPositionGains().setDampingRatios(1.0); + gains.getOrientationGains().setDampingRatios(1.0); + SpatialFeedbackControlCommand spatialFeedbackControlCommand = new SpatialFeedbackControlCommand(); + spatialFeedbackControlCommand.set(baseBody, endEffector); + spatialFeedbackControlCommand.setGains(gains); + spatialFeedbackControlCommand.setInverseDynamics(desiredPose, zero, zero); + spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand); + spatialFeedbackController.setEnabled(true); + spatialFeedbackController.setImpedanceEnabled(true); + + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(MultiBodySystemTools.computeDegreesOfFreedom(joints)); + DMatrixRMaj jointAccelerations = new DMatrixRMaj(0, 0); + double damping = 0.001; + RobotJointVelocityAccelerationIntegrator integrator = new RobotJointVelocityAccelerationIntegrator(controlDT); + + for (int i = 0; i < simulationTime / controlDT; i++) + { + spatialFeedbackController.computeInverseDynamics(); + SpatialAccelerationCommand spatialAccelerationCommand = spatialFeedbackController.getInverseDynamicsOutput(); + Assert.assertTrue(motionQPInputCalculator.convertSpatialAccelerationCommand(spatialAccelerationCommand, motionQPInput)); + NativeCommonOps.solveDamped(new DMatrixRMaj(motionQPInput.getTaskJacobian()), new DMatrixRMaj(motionQPInput.getTaskObjective()), damping, jointAccelerations); + integrator.integrateJointAccelerations(joints, jointAccelerations); + integrator.integrateJointVelocities(joints, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(joints, JointStateType.VELOCITY, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(joints, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); + elevator.updateFramesRecursively(); + } + + // Assert pose is close to desired + FramePose3D pose = new FramePose3D(endEffector.getBodyFixedFrame()); + pose.changeFrame(desiredPose.getReferenceFrame()); + EuclidCoreTestTools.assertGeometricallyEquals(desiredPose, pose, 1.0E-10); + } + + // Warning: Unfair test for the controller. The controller is not designed to handle this case. + @Test + public void testConvergence() throws Exception + { + Random random = new Random(5641654L); + + int numberOfJoints = 10; + Vector3D[] jointAxes = new Vector3D[numberOfJoints]; + for (int i = 0; i < numberOfJoints; i++) + jointAxes[i] = EuclidCoreRandomTools.nextVector3D(random, 1.0); + + YoRegistry registry = new YoRegistry("Dummy"); + RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); + List joints = randomFloatingChain.getRevoluteJoints(); + RigidBodyBasics elevator = randomFloatingChain.getElevator(); + RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); + FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0); + + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, 0.0, 1.0, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + FramePoint3D desiredPosition = new FramePoint3D(); + desiredPosition.setIncludingFrame(bodyFixedPointToControl); + desiredPosition.changeFrame(worldFrame); + FrameQuaternion desiredOrientation = new FrameQuaternion(); + desiredOrientation.setToZero(bodyFixedPointToControl.getReferenceFrame()); + desiredOrientation.changeFrame(worldFrame); + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, 0.0, 1.0, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + double controlDT = 0.004; + + + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, jointsToOptimizeFor, centerOfMassFrame, null, + null, registry); + toolbox.setupForInverseDynamicsSolver(null); + FeedbackControllerToolbox feedbackControllerToolbox = new FeedbackControllerToolbox(registry); + SpatialFeedbackController spatialFeedbackController = new SpatialFeedbackController(endEffector, toolbox, feedbackControllerToolbox, registry); + + SpatialFeedbackControlCommand spatialFeedbackControlCommand = new SpatialFeedbackControlCommand(); + spatialFeedbackControlCommand.set(elevator, endEffector); + DefaultPIDSE3Gains gains = new DefaultPIDSE3Gains(); + gains.getPositionGains().setProportialAndDerivativeGains(1.5, Double.NaN); + gains.getOrientationGains().setProportialAndDerivativeGains(1.5, Double.NaN); + gains.getPositionGains().setDampingRatios(1); + gains.getOrientationGains().setDampingRatios(1); + spatialFeedbackControlCommand.setGains(gains); + spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(bodyFixedPointToControl); + spatialFeedbackControlCommand.setInverseDynamics(desiredOrientation, desiredPosition, new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame)); + spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand); + spatialFeedbackController.setEnabled(true); + spatialFeedbackController.setImpedanceEnabled(true); + + int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor); + NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(numberOfDoFs); + LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); + DMatrixRMaj jInverse = new DMatrixRMaj(numberOfDoFs, 6); + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + DMatrixRMaj jointAccelerations = new DMatrixRMaj(numberOfDoFs, 1); + RobotJointVelocityAccelerationIntegrator integrator = new RobotJointVelocityAccelerationIntegrator(controlDT); + + FramePoint3D currentPosition = new FramePoint3D(); + FrameQuaternion currentOrientation = new FrameQuaternion(); + FrameQuaternion differenceOrientation = new FrameQuaternion(); + + FrameVector3D positionError = new FrameVector3D(); + FrameVector3D rotationError = new FrameVector3D(); + + double previousErrorMagnitude = Double.POSITIVE_INFINITY; + double errorMagnitude = previousErrorMagnitude; + + for (int i = 0; i < 100; i++) + { + spatialFeedbackController.computeInverseDynamics(); + SpatialAccelerationCommand output = spatialFeedbackController.getInverseDynamicsOutput(); + + motionQPInputCalculator.convertSpatialAccelerationCommand(output, motionQPInput); + pseudoInverseSolver.setA(new DMatrixRMaj(motionQPInput.taskJacobian)); + pseudoInverseSolver.invert(jInverse); + CommonOps_DDRM.mult(jInverse, new DMatrixRMaj(motionQPInput.taskObjective), jointAccelerations); + + integrator.integrateJointAccelerations(jointsToOptimizeFor, jointAccelerations); + integrator.integrateJointVelocities(jointsToOptimizeFor, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); + elevator.updateFramesRecursively(); + + currentPosition.setIncludingFrame(bodyFixedPointToControl); + currentPosition.changeFrame(worldFrame); + positionError.sub(desiredPosition, currentPosition); + + currentOrientation.setToZero(bodyFixedPointToControl.getReferenceFrame()); + currentOrientation.changeFrame(worldFrame); + + differenceOrientation.difference(desiredOrientation, currentOrientation); + differenceOrientation.normalizeAndLimitToPi(); + differenceOrientation.getRotationVector(rotationError); + + errorMagnitude = Math.sqrt(positionError.lengthSquared() + rotationError.lengthSquared()); + boolean isErrorReducing = errorMagnitude < previousErrorMagnitude; + assertTrue(isErrorReducing); + previousErrorMagnitude = errorMagnitude; + } + } + + + // Warning: Unfair test for the controller. The controller is not designed to handle this case. + @Test + public void testConvergenceWithJerryQP() throws Exception + { + Random random = new Random(54654L); + + int numberOfJoints = 10; + Vector3D[] jointAxes = new Vector3D[numberOfJoints]; + for (int i = 0; i < numberOfJoints; i++) + jointAxes[i] = EuclidCoreRandomTools.nextVector3D(random, 1.0); + + YoRegistry registry = new YoRegistry("Dummy"); + RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); + List joints = randomFloatingChain.getRevoluteJoints(); + RigidBodyBasics elevator = randomFloatingChain.getElevator(); + RigidBodyBasics endEffector = joints.get(joints.size() - 1).getSuccessor(); + FramePoint3D bodyFixedPointToControl = EuclidFrameRandomTools.nextFramePoint3D(random, endEffector.getBodyFixedFrame(), 1.0, 1.0, 1.0); + + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, 0.0, 1.0, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + FramePoint3D desiredPosition = new FramePoint3D(); + desiredPosition.setIncludingFrame(bodyFixedPointToControl); + desiredPosition.changeFrame(worldFrame); + FrameQuaternion desiredOrientation = new FrameQuaternion(); + desiredOrientation.setToZero(bodyFixedPointToControl.getReferenceFrame()); + desiredOrientation.changeFrame(worldFrame); + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, 0.0, 1.0, joints); + joints.get(0).getPredecessor().updateFramesRecursively(); + + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", worldFrame, elevator); + JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); + double controlDT = 0.004; + + WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, 0.0, null, jointsToOptimizeFor, centerOfMassFrame, null, + null, registry); + toolbox.setupForInverseDynamicsSolver(null); + FeedbackControllerToolbox feedbackControllerToolbox = new FeedbackControllerToolbox(registry); + SpatialFeedbackController spatialFeedbackController = new SpatialFeedbackController(endEffector, toolbox, feedbackControllerToolbox, registry); + + SpatialFeedbackControlCommand spatialFeedbackControlCommand = new SpatialFeedbackControlCommand(); + spatialFeedbackControlCommand.set(elevator, endEffector); + DefaultPIDSE3Gains gains = new DefaultPIDSE3Gains(); + gains.getPositionGains().setProportialAndDerivativeGains(1.5, Double.NaN); + gains.getOrientationGains().setProportialAndDerivativeGains(1.5, Double.NaN); + gains.getPositionGains().setDampingRatios(1); + gains.getOrientationGains().setDampingRatios(1); + spatialFeedbackControlCommand.setGains(gains); + spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(bodyFixedPointToControl); + spatialFeedbackControlCommand.setInverseDynamics(desiredOrientation, desiredPosition, new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame), new FrameVector3D(worldFrame)); + spatialFeedbackController.submitFeedbackControlCommand(spatialFeedbackControlCommand); + spatialFeedbackController.setEnabled(true); + spatialFeedbackController.setImpedanceEnabled(true); + + int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor); + NativeQPInputTypeA motionQPInput = new NativeQPInputTypeA(numberOfDoFs); + LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); + DMatrixRMaj jInverse = new DMatrixRMaj(numberOfDoFs, 6); + MotionQPInputCalculator motionQPInputCalculator = toolbox.getMotionQPInputCalculator(); + DMatrixRMaj jointAccelerations = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj jointAccelerationsFromJerryQP = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj jointAccelerationsFromQPOASES = new DMatrixRMaj(numberOfDoFs, 1); + RobotJointVelocityAccelerationIntegrator integrator = new RobotJointVelocityAccelerationIntegrator(controlDT); + + SimpleEfficientActiveSetQPSolver jerryQPSolver = new SimpleEfficientActiveSetQPSolver(); + OASESConstrainedQPSolver oasesQPSolver = new OASESConstrainedQPSolver(); + + DMatrixRMaj solverInput_H = new DMatrixRMaj(numberOfDoFs, numberOfDoFs); + DMatrixRMaj solverInput_f = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj solverInput_Aeq = new DMatrixRMaj(0, numberOfDoFs); + DMatrixRMaj solverInput_beq = new DMatrixRMaj(0, 1); + DMatrixRMaj solverInput_Ain = new DMatrixRMaj(0, numberOfDoFs); + DMatrixRMaj solverInput_bin = new DMatrixRMaj(0, 1); + DMatrixRMaj solverInput_lb = new DMatrixRMaj(numberOfDoFs, 1); + DMatrixRMaj solverInput_ub = new DMatrixRMaj(numberOfDoFs, 1); + CommonOps_DDRM.fill(solverInput_lb, Double.NEGATIVE_INFINITY); + CommonOps_DDRM.fill(solverInput_ub, Double.POSITIVE_INFINITY); + + DMatrixRMaj tempJtW = new DMatrixRMaj(numberOfDoFs, 6); + + FramePoint3D currentPosition = new FramePoint3D(); + FrameQuaternion currentOrientation = new FrameQuaternion(); + FrameQuaternion differenceOrientation = new FrameQuaternion(); + + FrameVector3D positionError = new FrameVector3D(); + FrameVector3D rotationError = new FrameVector3D(); + + double previousErrorMagnitude = Double.POSITIVE_INFINITY; + double errorMagnitude = previousErrorMagnitude; + + for (int i = 0; i < 100; i++) + { + spatialFeedbackController.computeInverseDynamics(); + SpatialAccelerationCommand output = spatialFeedbackController.getInverseDynamicsOutput(); + motionQPInputCalculator.convertSpatialAccelerationCommand(output, motionQPInput); + + MatrixTools.scaleTranspose(1.0, new DMatrixRMaj(motionQPInput.taskJacobian), tempJtW); // J^T W + CommonOps_DDRM.mult(tempJtW, new DMatrixRMaj(motionQPInput.taskJacobian), solverInput_H); // H = J^T W J + CommonOps_DDRM.mult(tempJtW, new DMatrixRMaj(motionQPInput.taskObjective), solverInput_f); // f = - J^T W xDDot + CommonOps_DDRM.scale(-1.0, solverInput_f); + + for (int diag = 0; diag < numberOfDoFs; diag++) + solverInput_H.add(diag, diag, 1e-8); + + jerryQPSolver.clear(); + jerryQPSolver.setQuadraticCostFunction(solverInput_H, solverInput_f, 0.0); + jerryQPSolver.solve(jointAccelerationsFromJerryQP); + oasesQPSolver.solve(solverInput_H, solverInput_f, solverInput_Aeq, solverInput_beq, solverInput_Ain, solverInput_bin, solverInput_lb, solverInput_ub, + jointAccelerationsFromQPOASES, true); + + pseudoInverseSolver.setA(new DMatrixRMaj(motionQPInput.taskJacobian)); + pseudoInverseSolver.invert(jInverse); + CommonOps_DDRM.mult(jInverse, new DMatrixRMaj(motionQPInput.taskObjective), jointAccelerations); + + integrator.integrateJointAccelerations(jointsToOptimizeFor, jointAccelerationsFromJerryQP); + integrator.integrateJointVelocities(jointsToOptimizeFor, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); + MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); + elevator.updateFramesRecursively(); + + assertArrayEquals(jointAccelerations.data, jointAccelerationsFromJerryQP.data, 1.0e-4); + assertArrayEquals(jointAccelerations.data, jointAccelerationsFromQPOASES.data, 3.0e-1); + + currentPosition.setIncludingFrame(bodyFixedPointToControl); + currentPosition.changeFrame(worldFrame); + positionError.sub(desiredPosition, currentPosition); + + currentOrientation.setToZero(bodyFixedPointToControl.getReferenceFrame()); + currentOrientation.changeFrame(worldFrame); + + differenceOrientation.difference(desiredOrientation, currentOrientation); + differenceOrientation.normalizeAndLimitToPi(); + differenceOrientation.getRotationVector(rotationError); + + errorMagnitude = Math.sqrt(positionError.lengthSquared() + rotationError.lengthSquared()); + boolean isErrorReducing = errorMagnitude < previousErrorMagnitude; + assertTrue(isErrorReducing); + previousErrorMagnitude = errorMagnitude; + } + } +} \ No newline at end of file diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java index ae412565a5e..1d0d9954464 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java @@ -17,6 +17,8 @@ public class HandHybridJointspaceTaskspaceTrajectoryCommand private boolean forceExecution = false; private final JointspaceTrajectoryCommand jointspaceTrajectoryCommand = new JointspaceTrajectoryCommand(); private final SE3TrajectoryControllerCommand taskspaceTrajectoryCommand = new SE3TrajectoryControllerCommand(); + private final WrenchTrajectoryControllerCommand feedForwardTrajectoryCommand = new WrenchTrajectoryControllerCommand(); + private final SE3PIDGainsTrajectoryControllerCommand pidGainsTrajectoryCommand = new SE3PIDGainsTrajectoryControllerCommand(); public HandHybridJointspaceTaskspaceTrajectoryCommand() { @@ -31,6 +33,41 @@ public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boole this.taskspaceTrajectoryCommand.set(taskspaceTrajectoryCommand); } + public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand, + JointspaceTrajectoryCommand jointspaceTrajectoryCommand, + SE3PIDGainsTrajectoryControllerCommand pidGainsTrajectoryCommand) + { + this.robotSide = robotSide; + this.setForceExecution(forceExecution); + this.jointspaceTrajectoryCommand.set(jointspaceTrajectoryCommand); + this.taskspaceTrajectoryCommand.set(taskspaceTrajectoryCommand); + this.pidGainsTrajectoryCommand.set(pidGainsTrajectoryCommand); + } + + public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand, + JointspaceTrajectoryCommand jointspaceTrajectoryCommand, + WrenchTrajectoryControllerCommand feedForwardTrajectoryCommand) + { + this.robotSide = robotSide; + this.setForceExecution(forceExecution); + this.jointspaceTrajectoryCommand.set(jointspaceTrajectoryCommand); + this.taskspaceTrajectoryCommand.set(taskspaceTrajectoryCommand); + this.feedForwardTrajectoryCommand.set(feedForwardTrajectoryCommand); + } + + public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand, + JointspaceTrajectoryCommand jointspaceTrajectoryCommand, + WrenchTrajectoryControllerCommand feedForwardTrajectoryCommand, + SE3PIDGainsTrajectoryControllerCommand pidGainsTrajectoryCommand) + { + this.robotSide = robotSide; + this.setForceExecution(forceExecution); + this.jointspaceTrajectoryCommand.set(jointspaceTrajectoryCommand); + this.taskspaceTrajectoryCommand.set(taskspaceTrajectoryCommand); + this.feedForwardTrajectoryCommand.set(feedForwardTrajectoryCommand); + this.pidGainsTrajectoryCommand.set(pidGainsTrajectoryCommand); + } + public HandHybridJointspaceTaskspaceTrajectoryCommand(Random random) { this(RobotSide.generateRandomRobotSide(random), random.nextBoolean(), new SE3TrajectoryControllerCommand(random), @@ -50,7 +87,8 @@ public void clear() robotSide = null; setForceExecution(false); jointspaceTrajectoryCommand.clear(); - taskspaceTrajectoryCommand.clear(); + feedForwardTrajectoryCommand.clear(); + pidGainsTrajectoryCommand.clear(); } @Override @@ -67,12 +105,15 @@ public void set(ReferenceFrameHashCodeResolver resolver, HandHybridJointspaceTas setForceExecution(message.getForceExecution()); jointspaceTrajectoryCommand.setFromMessage(message.getJointspaceTrajectoryMessage()); taskspaceTrajectoryCommand.set(resolver, message.getTaskspaceTrajectoryMessage()); + feedForwardTrajectoryCommand.set(resolver, message.getFeedforwardTaskspaceTrajectoryMessage()); + pidGainsTrajectoryCommand.setFromMessage(message.getTaskspacePidGainsTrajectoryMessage()); } @Override public boolean isCommandValid() { - return robotSide != null && jointspaceTrajectoryCommand.isCommandValid() && taskspaceTrajectoryCommand.isCommandValid(); + return robotSide != null && jointspaceTrajectoryCommand.isCommandValid() && taskspaceTrajectoryCommand.isCommandValid() + && feedForwardTrajectoryCommand.isCommandValid() && pidGainsTrajectoryCommand.isCommandValid(); } @Override @@ -83,6 +124,8 @@ public void set(HandHybridJointspaceTaskspaceTrajectoryCommand other) setForceExecution(other.getForceExecution()); taskspaceTrajectoryCommand.set(other.getTaskspaceTrajectoryCommand()); jointspaceTrajectoryCommand.set(other.getJointspaceTrajectoryCommand()); + feedForwardTrajectoryCommand.set(other.getFeedForwardTrajectoryCommand()); + pidGainsTrajectoryCommand.set(other.getPIDGainsTrajectoryCommand()); } public RobotSide getRobotSide() @@ -110,6 +153,16 @@ public SE3TrajectoryControllerCommand getTaskspaceTrajectoryCommand() return taskspaceTrajectoryCommand; } + public WrenchTrajectoryControllerCommand getFeedForwardTrajectoryCommand() + { + return feedForwardTrajectoryCommand; + } + + public SE3PIDGainsTrajectoryControllerCommand getPIDGainsTrajectoryCommand() + { + return pidGainsTrajectoryCommand; + } + @Override public boolean isDelayedExecutionSupported() { diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/SE3PIDGainsTrajectoryControllerCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/SE3PIDGainsTrajectoryControllerCommand.java new file mode 100644 index 00000000000..6b0df5e3223 --- /dev/null +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/SE3PIDGainsTrajectoryControllerCommand.java @@ -0,0 +1,160 @@ +package us.ihmc.humanoidRobotics.communication.controllerAPI.command; + +import controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage; +import controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage; +import gnu.trove.list.array.TDoubleArrayList; +import us.ihmc.commons.lists.RecyclingArrayList; +import us.ihmc.communication.controllerAPI.command.QueueableCommand; +import controller_msgs.msg.dds.PID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPID3DGains; +import us.ihmc.robotics.math.trajectories.trajectorypoints.OneDoFTrajectoryPoint; +import us.ihmc.robotics.math.trajectories.trajectorypoints.SE3PIDGainsTrajectoryPoint; +import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.SE3PIDGainsTrajectoryPointBasics; +import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.TrajectoryPointListBasics; +import us.ihmc.robotics.math.trajectories.trajectorypoints.lists.SE3PIDGainsTrajectoryPointList; + +import java.util.List; + +public class SE3PIDGainsTrajectoryControllerCommand extends QueueableCommand + implements TrajectoryPointListBasics +{ + private long sequenceId; + private final RecyclingArrayList trajectoryPoints = new RecyclingArrayList<>(200, SE3PIDGainsTrajectoryPoint.class); + + public SE3PIDGainsTrajectoryControllerCommand() { + } + + @Override + public void addTrajectoryPoint(SE3PIDGainsTrajectoryPoint trajectoryPoint) + { + trajectoryPoints.add().set(trajectoryPoint); + } + + public void addTrajectoryPoint(SE3PIDGainsTrajectoryPointBasics trajectoryPoint) + { + trajectoryPoints.add().set(trajectoryPoint); + } + + public void addTrajectoryPoint(double time, PID3DGains angularGains, PID3DGains linearGains) + { + us.ihmc.robotics.controllers.pidGains.PID3DGains newAngularGains = getNewPID3DGains(angularGains); + us.ihmc.robotics.controllers.pidGains.PID3DGains newLinearGains = getNewPID3DGains(linearGains); + + trajectoryPoints.add().set(time, newAngularGains, newLinearGains); + } + + @Override + public void clear() + { + sequenceId = 0; + trajectoryPoints.clear(); + clearQueuableCommandVariables(); + } + + @Override + public void set(SE3PIDGainsTrajectoryControllerCommand other) + { + trajectoryPoints.clear(); + for (int i = 0; i < other.getNumberOfTrajectoryPoints(); i++) + { + trajectoryPoints.add().set(other.getTrajectoryPoint(i)); + } + setPropertiesOnly(other); + } + + @Override + public void setFromMessage(SE3PIDGainsTrajectoryMessage message) + { + clear(); + + sequenceId = message.getSequenceId(); + setQueueableCommandVariables(message.getQueueingProperties()); + SE3PIDGainsTrajectoryPointMessage pointMessage = new SE3PIDGainsTrajectoryPointMessage(); + + for (int i = 0; i < message.getPidGainsTrajectoryPoints().size(); i++) + { + pointMessage = message.getPidGainsTrajectoryPoints().get(i); + addTrajectoryPoint(pointMessage.getTime(), pointMessage.getAngularGains(), pointMessage.getLinearGains()); + } + } + + /** + * Same as {@link #set(SE3PIDGainsTrajectoryControllerCommand)} but does not change the trajectory + * points. + * + * @param other + */ + public void setPropertiesOnly(SE3PIDGainsTrajectoryControllerCommand other) + { + sequenceId = other.sequenceId; + setQueueableCommandVariables(other); + } + + public List getTrajectoryPoints() + { + return trajectoryPoints; + } + + @Override + public boolean isCommandValid() + { + return executionModeValid() && !trajectoryPoints.isEmpty(); + } + + /** {@inheritDoc}} */ + @Override + public void addTimeOffset(double timeOffsetToAdd) + { + for (int i = 0; i < getNumberOfTrajectoryPoints(); i++) + { + getTrajectoryPoint(i).setTime(getTrajectoryPoint(i).getTime() + timeOffsetToAdd); + } + } + + public int getNumberOfTrajectoryPoints() + { + return trajectoryPoints.size(); + } + + public SE3PIDGainsTrajectoryPoint getTrajectoryPoint(int trajectoryPointIndex) + { + return trajectoryPoints.get(trajectoryPointIndex); + } + + public SE3PIDGainsTrajectoryPoint getLastTrajectoryPoint() + { + return trajectoryPoints.getLast(); + } + + @Override + public Class getMessageClass() + { + return SE3PIDGainsTrajectoryMessage.class; + } + + public void setSequenceId(long sequenceId) + { + this.sequenceId = sequenceId; + } + + @Override + public long getSequenceId() + { + return sequenceId; + } + + public us.ihmc.robotics.controllers.pidGains.PID3DGains getNewPID3DGains(PID3DGains gains) + { + us.ihmc.robotics.controllers.pidGains.PID3DGains newGains = new DefaultPID3DGains(); + newGains.setProportionalGains(gains.getGainsX().getKp(), gains.getGainsY().getKp(), gains.getGainsZ().getKp()); + newGains.setDerivativeGains(gains.getGainsX().getKd(), gains.getGainsY().getKd(), gains.getGainsZ().getKd()); + newGains.setIntegralGains(gains.getGainsX().getKi(), gains.getGainsY().getKi(), gains.getGainsZ().getKi(), gains.getMaximumIntegralError()); + newGains.getDampingRatios()[0] = gains.getGainsX().getZeta(); + newGains.getDampingRatios()[1] = gains.getGainsY().getZeta(); + newGains.getDampingRatios()[2] = gains.getGainsZ().getZeta(); + newGains.setMaxFeedbackAndFeedbackRate(gains.getMaximumFeedback(), gains.getMaximumFeedbackRate()); + newGains.setMaxDerivativeError(gains.getMaximumDerivativeError()); + newGains.setMaxProportionalError(gains.getMaximumProportionalError()); + return newGains; + } +} \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg index 42077518178..97e6342dceb 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg @@ -18,4 +18,8 @@ byte robot_side 255 ihmc_common_msgs/SE3TrajectoryMessage taskspace_trajectory_message # The jointspace trajectory information. # The indexing for the joints goes increasingly from the first shoulder joint to the last arm joint. -JointspaceTrajectoryMessage jointspace_trajectory_message \ No newline at end of file +JointspaceTrajectoryMessage jointspace_trajectory_message +# The feedforward taskspace trajectory information. +WrenchTrajectoryMessage feedforward_taskspace_trajectory_message +# The PIDGains for the taskspace controller. +SE3PIDGainsTrajectoryMessage taskspace_pid_gains_trajectory_message \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PID3DGains.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PID3DGains.msg new file mode 100644 index 00000000000..edacb04bd23 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PID3DGains.msg @@ -0,0 +1,11 @@ +# this represents the 3d gains for a PID controller + +PIDGains gains_x +PIDGains gains_y +PIDGains gains_z + +float64 maximum_feedback +float64 maximum_feedback_rate +float64 maximum_integral_error +float64 maximum_derivative_error +float64 maximum_proportional_error \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PIDGains.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PIDGains.msg new file mode 100644 index 00000000000..e35e630ff66 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/PIDGains.msg @@ -0,0 +1,6 @@ +# this represents the gains for a PID controller + +float64 kp +float64 kd +float64 ki +float64 zeta \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg new file mode 100644 index 00000000000..2f4e6292719 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg @@ -0,0 +1,10 @@ +# This message is part of the IHMC whole-body controller API. +# This class is used to change the PID gains over time. + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + +# List of trajectory points (in taskpsace) to go through while executing the trajectory. +SE3PIDGainsTrajectoryPointMessage[<=200] pid_gains_trajectory_points +# Properties for queueing trajectories. +ihmc_common_msgs/QueueableMessage queueing_properties \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg new file mode 100644 index 00000000000..1298702cf9e --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg @@ -0,0 +1,13 @@ +# This message is part of the IHMC whole-body controller API. +# This class is used to change the PID gains over time. +# It holds the necessary information for one trajectory point. + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + +# Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts. +float64 time +# Define the desired linear PID gains for the trajectory point. +PID3DGains linear_gains +# Define the desired angular PID gains for the trajectory point. +PID3DGains angular_gains \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MatrixMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MatrixMissingTools.java index 3c88cabb897..20952d818eb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MatrixMissingTools.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MatrixMissingTools.java @@ -1,11 +1,15 @@ package us.ihmc.robotics; +import gnu.trove.list.array.TIntArrayList; import org.ejml.MatrixDimensionException; import org.ejml.data.DMatrix; import org.ejml.data.DMatrix1Row; import org.ejml.data.DMatrix3x3; import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.linsol.lu.LinearSolverLu_DDRM; +import org.ejml.dense.row.misc.UnrolledInverseFromMinor_DDRM; +import org.ejml.interfaces.decomposition.SingularValueDecomposition_F64; import org.ejml.simple.SimpleMatrix; import us.ihmc.euclid.matrix.interfaces.Matrix3DBasics; import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly; @@ -102,6 +106,23 @@ public static void setDiagonal(DMatrix3x3 mat, double diagonalValue) } } + public static void diagonal(DMatrixRMaj matrix, DMatrixRMaj diagMatrixToPack) { + if (matrix.getNumRows() != matrix.getNumCols()) { + throw new IllegalArgumentException("The input matrix must be square."); + } + if (diagMatrixToPack.getNumRows() != matrix.getNumRows() || diagMatrixToPack.getNumCols() != matrix.getNumCols()) { + throw new IllegalArgumentException("The diagonal matrix must have the same dimensions as the input matrix."); + } + + // Set all elements to zero + CommonOps_DDRM.fill(diagMatrixToPack, 0.0); + + // Set the diagonal elements + for (int i = 0; i < matrix.getNumRows(); i++) { + diagMatrixToPack.set(i, i, matrix.get(i, i)); + } + } + public static void setMatrixBlock(DMatrix dest, int destStartRow, int destStartColumn, DMatrix3x3 src, double scale) { setMatrixBlock(dest, destStartRow, destStartColumn, src, 0, 0, 3, 3, scale); @@ -236,6 +257,31 @@ public static void setSelectedMatrixDiagonals(int[] indices, double value, DMatr } } + public static void extractRows(DMatrixRMaj src, TIntArrayList rows, DMatrixRMaj dst) + { + if (rows.isEmpty()) + return; + + if (src.getNumCols() != dst.getNumCols()) + throw new IllegalArgumentException( + "src and dst must have the same number of columns, was: [src cols: " + src.getNumCols() + ", dst cols: " + dst.getNumCols() + "]"); + if (src.getNumRows() < rows.get(rows.size() - 1) + 1) + throw new IllegalArgumentException( + "src is too small, min size: [rows: " + (rows.get(rows.size() - 1) + 1) + "], was: [rows: " + src.getNumRows() + "]"); + if (dst.getNumRows() < rows.size()) + throw new IllegalArgumentException( + "dst is too small, min size: [rows: " + rows.size() + "], was: [rows: " + dst.getNumRows() + "]"); + + for (int i = 0; i < rows.size(); i++) + { + int srcRow = rows.get(i); + for (int j = 0; j < src.getNumCols(); j++) + { + dst.unsafe_set(i, j, src.unsafe_get(srcRow, j)); + } + } + } + public static DMatrixRMaj createVector(int size, double fillValue) { DMatrixRMaj vector = new DMatrixRMaj(size, 1); @@ -507,6 +553,48 @@ public static void power(DMatrixRMaj input, int power, DMatrixRMaj resultToPack) } } + /** + * Computes the square root of a positive definite matrix using the Eigenvalue decomposition. + * The result is stored in the provided resultToPack matrix. + * + * Form: resultToPack*resultToPack = input (NO TRANSPOSES: Cholesky is not used) + * + * @param input the matrix to compute the square root of. Not modified. + * @param resultToPack the matrix to store the result in. Modified. + * @throws IllegalArgumentException if the input and resultToPack matrices have incompatible sizes. + * @throws RuntimeException if the input matrix is not positive definite. + */ + public static void sqrt(DMatrixRMaj input, DMatrixRMaj resultToPack, DMatrixRMaj temp, DMatrixRMaj U, DMatrixRMaj W, DMatrixRMaj Vt, SingularValueDecomposition_F64 svd) + { + if (input.numCols != resultToPack.numCols) + throw new IllegalArgumentException("The matrices have incompatible column sizes."); + if (input.numRows != resultToPack.numRows) + throw new IllegalArgumentException("The matrices have incompatible row sizes."); + + if (!svd.decompose(input)) + { + throw new RuntimeException("Singular Value Decomposition failed."); + } + + U.reshape(input.numRows, input.numCols); + W.reshape(input.numCols, input.numCols); + Vt.reshape(input.numCols, input.numCols); + + svd.getU(U, false); + svd.getW(W); + svd.getV(Vt, true); + + // Compute the square root of the sinDoes the sqrt of the singular values + for (int i = 0; i < W.numRows; i++) + { + W.set(i, i, Math.sqrt(W.get(i, i))); + } + + temp.reshape(input.numCols, input.numCols); + CommonOps_DDRM.mult(U, W, temp); + CommonOps_DDRM.mult(temp, Vt, resultToPack); + } + /** * Comparing elements of two matrices. * This method results true when all elements in a is less than b.
@@ -535,4 +623,114 @@ public static boolean elementWiseLessThan(DMatrixRMaj a, DMatrixRMaj b) } return true; } + + /** + * Extract function of CommonOps_DDRM.extract with TIntArrayList + * + * @param src source matrix + * @param rows row indices to extract + * @param cols column indices to extract + * @param dst destination matrix + */ + public static void extract(DMatrixRMaj src, TIntArrayList rows, TIntArrayList cols, DMatrixRMaj dst) + { + if (rows.size() == 0 || cols.size() == 0) + return; + + if (src.getNumRows() < rows.get(rows.size() - 1) + 1) + throw new IllegalArgumentException( + "src is too small, min size: [rows: " + (rows.get(rows.size() - 1) + 1) + "], was: [rows: " + src.getNumRows() + "]"); + if (src.getNumCols() < cols.get(cols.size() - 1) + 1) + throw new IllegalArgumentException( + "src is too small, min size: [cols: " + (cols.get(cols.size() - 1) + 1) + "], was: [cols: " + src.getNumCols() + "]"); + + if (dst.getNumRows() < rows.size() || dst.getNumCols() < cols.size()) + throw new IllegalArgumentException( + "dst is too small, min size: [rows: " + rows.size() + ", cols: " + cols.size() + "], was: [rows: " + dst.getNumRows() + ", cols: " + + dst.getNumCols() + "]"); + + for (int i = 0; i < rows.size(); i++) + { + for (int j = 0; j < cols.size(); j++) + { + dst.unsafe_set(i, j, src.unsafe_get(rows.get(i), cols.get(j))); + } + } + } + + /** + * insert function of CommonOps_DDRM.insert with TIntArrayList. + * + * @param src source matrix + * @param rows row indices to insert + * @param cols column indices to insert + * @param dst destination matrix + */ + public static void insert( DMatrixRMaj src , + DMatrixRMaj dst , + TIntArrayList rows , + TIntArrayList cols ) { + if ( rows.size() == 0 || cols.size() == 0 ) return; + + if (dst.getNumRows() < rows.get(rows.size() - 1) + 1) + throw new IllegalArgumentException( + "dst is too small, min size: [rows: " + (rows.get(rows.size() - 1) + 1) + "], was: [rows: " + dst.getNumRows() + "]"); + if (dst.getNumCols() < cols.get(cols.size() - 1) + 1) + throw new IllegalArgumentException( + "dst is too small, min size: [cols: " + (cols.get(cols.size() - 1) + 1) + "], was: [cols: " + dst.getNumCols() + "]"); + + if (src.getNumRows() < rows.size() || src.getNumCols() < cols.size()) + throw new IllegalArgumentException( + "src is too small, min size: [rows: " + rows.size() + ", cols: " + cols.size() + "], was: [rows: " + src.getNumRows() + ", cols: " + + src.getNumCols() + "]"); + + for (int i = 0; i < rows.size(); i++) + { + for (int j = 0; j < cols.size(); j++) + { + dst.unsafe_set(rows.get(i), cols.get(j), src.unsafe_get(i, j)); + } + } + } + + /** + * This is the invert function of CommonOps_DDRM.invert but garbage free by providing the solver. + * + *

+ * Performs a matrix inversion operation on the specified matrix and stores the results + * in the same matrix.
+ *
+ * a = a-1 + *

+ * + *

+ * If the algorithm could not invert the matrix then false is returned. If it returns true + * that just means the algorithm finished. The results could still be bad + * because the matrix is singular or nearly singular. + *

+ * + * @param mat The matrix that is to be inverted. Results are stored here. Modified. + * @param solver Invertion algorithm that is used. Should be preconfigured. + * @return true if it could invert the matrix false if it could not. + */ + public static boolean invert( DMatrixRMaj mat, LinearSolverLu_DDRM solver) { + if(mat.numCols <= UnrolledInverseFromMinor_DDRM.MAX ) { + if( mat.numCols != mat.numRows ) { + throw new MatrixDimensionException("Must be a square matrix."); + } + + if( mat.numCols >= 2 ) { + UnrolledInverseFromMinor_DDRM.inv(mat,mat); + } else { + mat.set(0, 1.0/mat.get(0)); + } + } else { + if( solver.setA(mat) ) { + solver.invert(mat); + } else { + return false; + } + } + return true; + } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGains.java index 7f097cd8e5d..e5e3fd77c4a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGains.java @@ -135,6 +135,23 @@ public default void setDerivativeGains(Tuple3DReadOnly derivativeGains) setDerivativeGains(derivativeGains.getX(), derivativeGains.getY(), derivativeGains.getZ()); } + /** + * Sets the damping ratio for all three dimensions. + *

+ * Note: Depending on the implementation the damping ratio might be used to determine the derivative + * gains from the current proportional gains and the current damping ratio. Those implementations will + * typically update the damping ratio from the current proportional gain and the provided derivative + * gain if the derivative gain is set. In that case the order in which the proportional and derivative + * gains are set might influence the outcome. Typically, it is safe to first set the proportional gain + * and then set the derivative gain or damping ratio. For an example of this see {@link DefaultPID3DGains}. + *

+ * + * @param dampingRatioX the new damping ratio for the x direction. + * @param dampingRatioY the new damping ratio for the y direction. + * @param dampingRatioZ the new damping ratio for the z direction. + */ + public abstract void setDampingRatios(double dampingRatioX, double dampingRatioY, double dampingRatioZ); + /** * Sets the derivative PID gains for all three dimensions from a double array. The provided array * must be of length three. Also sets the maximum integral error allowed by the PID controller. @@ -233,6 +250,29 @@ public default void setGains(double proportionalGain, double derivativeGain, dou setIntegralGains(integralGain, maxIntegralError); } + /** + * Sets the damping ratio for all three dimensions. + * + * @param dampingRatio the new damping ratio for all three dimensions. + * @see PID3DGains#setDampingRatios(double, double, double) + */ + public default void setDampingRatios(double dampingRatio) + { + setDampingRatios(dampingRatio, dampingRatio, dampingRatio); + } + + /** + * Sets the damping ratio for all three dimensions. + * + * @param dampingRatios the new damping ratios for the three directions. + * @see PID3DGains#setDampingRatios(double, double, double) + */ + public default void setDampingRatios(double[] dampingRatios) + { + PID3DGainsReadOnly.checkArrayLength(dampingRatios); + setDampingRatios(dampingRatios[0], dampingRatios[1], dampingRatios[2]); + } + /** * Copies the gains and parameters from the provided {@link PID3DGainsReadOnly} parameters into * this. @@ -256,5 +296,6 @@ public default void set(PID3DGainsReadOnly other) setMaxFeedbackAndFeedbackRate(other.getMaximumFeedback(), other.getMaximumFeedbackRate()); setMaxDerivativeError(other.getMaximumDerivativeError()); setMaxProportionalError(other.getMaximumProportionalError()); + setDampingRatios(other.getDampingRatios()); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGainsReadOnly.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGainsReadOnly.java index 8db4a6da047..db843f46763 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGainsReadOnly.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PID3DGainsReadOnly.java @@ -1,7 +1,7 @@ package us.ihmc.robotics.controllers.pidGains; import java.util.Arrays; - +import org.ejml.data.DMatrixRMaj; import us.ihmc.euclid.matrix.Matrix3D; /** @@ -33,6 +33,14 @@ public interface PID3DGainsReadOnly */ public abstract double[] getIntegralGains(); + /** + * Returns the derivative damping ratio for all three dimensions. The returned + * array is of length three. + * + * @return the derivative damping ratio as double array. + */ + public abstract double[] getDampingRatios(); + /** * Returns the maximum integral error allowed by the PID controller * @@ -92,6 +100,31 @@ public default void getDerivativeGainMatrix(Matrix3D derivativeGainMatrixToPack) setMatrixDiagonal(getDerivativeGains(), derivativeGainMatrixToPack); } + /** + * Will pack the proportional stiffness matrix (6x6). The matrix will be a diagonal + * matrix with the diagonal elements set to the proportional stiffnesss. + * + * @param proportionalGainMatrixToPack the matrix in which the stiffnesss are stored. Modified. + */ + public default void getFullProportionalGainMatrix(DMatrixRMaj proportionalGainMatrixToPack, int startIndex) + { + double[] proportionalGains = getProportionalGains(); + proportionalGainMatrixToPack.reshape(6, 6); + proportionalGainMatrixToPack.zero(); + + for (int i = 0; i < 6; i++) + { + if (i >= startIndex && i < startIndex + proportionalGains.length) + { + proportionalGainMatrixToPack.set(i, i, proportionalGains[i - startIndex]); + } + else + { + proportionalGainMatrixToPack.set(i, i, 1.0); + } + } + } + /** * Will pack the integral gain matrix. The matrix will be a diagonal * matrix with the diagonal elements set to the integral gains. diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultPID3DGains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultPID3DGains.java index 9fa3f1b5d93..2effce68d54 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultPID3DGains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultPID3DGains.java @@ -64,6 +64,12 @@ public double[] getIntegralGains() return integralGains; } + @Override + public double[] getDampingRatios() + { + return dampingRatios; + } + @Override public double getMaximumIntegralError() { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPID3DGains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPID3DGains.java index 945cb87ef87..74fa8a5ed6f 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPID3DGains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPID3DGains.java @@ -197,6 +197,13 @@ public double[] getIntegralGains() return tempIntegralGains; } + @Override + public double[] getDampingRatios() + { + fillFromMap(zetaMap, tempIntegralGains); + return tempIntegralGains; + } + static void fillFromMap(Map map, double[] arrayToFill) { arrayToFill[0] = map.get(Axis3D.X).getDoubleValue(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPIDSE3Gains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPIDSE3Gains.java index 9e250d36160..eaf9cb23f8e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPIDSE3Gains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/DefaultYoPIDSE3Gains.java @@ -104,4 +104,10 @@ public YoPID3DGains getOrientationGains() { return orientationGains; } + + @Override + public String toString() + { + return "Position: " + getPositionGains().toString() + "; Orientation: " + getOrientationGains().toString(); + } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ParameterizedPID3DGains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ParameterizedPID3DGains.java index dfa5af5575d..5aa1d70f9ec 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ParameterizedPID3DGains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ParameterizedPID3DGains.java @@ -263,6 +263,13 @@ public double[] getIntegralGains() return tempIntegralGains; } + @Override + public double[] getDampingRatios() + { + fillFromMap(zetaMap, tempProportionalGains); + return tempProportionalGains; + } + private static void fillFromMap(Map map, double[] arrayToFill) { arrayToFill[0] = map.get(Axis3D.X).getValue(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/SymmetricPID3DGains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/SymmetricPID3DGains.java index aefddc30a92..f2f2764cb2d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/SymmetricPID3DGains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/SymmetricPID3DGains.java @@ -34,6 +34,12 @@ public double[] getIntegralGains() return fill(tempIntegralGains, gains.getKi()); } + @Override + public double[] getDampingRatios() + { + return new double[] {1.0, 1.0, 1.0}; + } + @Override public double getMaximumIntegralError() { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ZeroablePID3DGains.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ZeroablePID3DGains.java index 04b647abb57..5d481330f08 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ZeroablePID3DGains.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/ZeroablePID3DGains.java @@ -15,6 +15,7 @@ public class ZeroablePID3DGains implements PID3DGains, Settable waypoints; + private final FixedFrameQuaternionBasics currentOrientation; + private final FixedFrameVector3DBasics currentVelocity; + private final FixedFrameVector3DBasics currentAcceleration; private final HermiteCurveBasedOrientationTrajectoryGenerator subTrajectory; public MultipleWaypointsOrientationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, YoRegistry parentRegistry) @@ -73,6 +82,10 @@ public MultipleWaypointsOrientationTrajectoryGenerator(String namePrefix, currentTrajectoryTime = new YoDouble(namePrefix + "CurrentTrajectoryTime", registry); currentWaypointIndex = new YoInteger(namePrefix + "CurrentWaypointIndex", registry); + currentOrientation = EuclidFrameFactories.newLinkedFixedFrameQuaternionBasics(this, new YoQuaternion(namePrefix + "CurrentOrientation", registry)); + currentVelocity = EuclidFrameFactories.newLinkedFixedFrameVector3DBasics(this, new YoVector3D(namePrefix + "CurrentVelocity", registry)); + currentAcceleration = EuclidFrameFactories.newLinkedFixedFrameVector3DBasics(this, new YoVector3D(namePrefix + "CurrentAcceleration", registry)); + subTrajectory = new HermiteCurveBasedOrientationTrajectoryGenerator(namePrefix + "SubTrajectory", referenceFrame, registry); for (int i = 0; i < maximumNumberOfWaypoints; i++) @@ -257,6 +270,42 @@ public void compute(double time) changedSubTrajectory = true; } + int secondWaypointIndex = Math.min(currentWaypointIndex.getValue() + 1, numberOfWaypoints.getValue() - 1); + + FixedFrameSO3TrajectoryPointBasics start = waypoints.get(currentWaypointIndex.getValue()); + FixedFrameSO3TrajectoryPointBasics end = waypoints.get(secondWaypointIndex); + + if (time < start.getTime()) + { + currentOrientation.set(start.getOrientation()); + currentVelocity.setToZero(); + currentAcceleration.setToZero(); + return; + } + if (time > end.getTime()) + { + currentOrientation.set(end.getOrientation()); + currentVelocity.setToZero(); + currentAcceleration.setToZero(); + return; + } + + if (Precision.equals(start.getTime(), end.getTime())) + { + currentOrientation.set(start.getOrientation()); + currentVelocity.set(start.getAngularVelocity()); + currentAcceleration.setToZero(); + return; + } + else if (Precision.equals(start.getTime(), end.getTime(), 0.05)) + { + double alpha = (time - start.getTime()) / (end.getTime() - start.getTime()); + currentOrientation.interpolate(start.getOrientation(), end.getOrientation(), alpha); + currentVelocity.interpolate(start.getAngularVelocity(), end.getAngularVelocity(), alpha); + currentAcceleration.setToZero(); + return; + } + if (changedSubTrajectory) { initializeSubTrajectory(currentWaypointIndex.getIntegerValue()); @@ -264,6 +313,9 @@ public void compute(double time) double subTrajectoryTime = time - waypoints.get(currentWaypointIndex.getIntegerValue()).getTime(); subTrajectory.compute(subTrajectoryTime); + currentOrientation.set(subTrajectory.getOrientation()); + currentVelocity.set(subTrajectory.getAngularVelocity()); + currentAcceleration.set(subTrajectory.getAngularAcceleration()); } @Override @@ -292,19 +344,19 @@ public int getCurrentWaypointIndex() @Override public FrameQuaternionReadOnly getOrientation() { - return subTrajectory.getOrientation(); + return currentOrientation; } @Override public FrameVector3DReadOnly getAngularVelocity() { - return subTrajectory.getAngularVelocity(); + return currentVelocity; } @Override public FrameVector3DReadOnly getAngularAcceleration() { - return subTrajectory.getAngularAcceleration(); + return currentAcceleration; } public int getCurrentNumberOfWaypoints() diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleWaypointsPositionTrajectoryGenerator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleWaypointsPositionTrajectoryGenerator.java index d6ae27d6e20..b8ede57c81a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleWaypointsPositionTrajectoryGenerator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/MultipleWaypointsPositionTrajectoryGenerator.java @@ -278,6 +278,7 @@ public void compute(double time) return; } + double subTrajectoryTime = MathTools.clamp(time - start.getTime(), 0.0, end.getTime() - start.getTime()); if (Precision.equals(start.getTime(), end.getTime())) { currentPosition.set(start.getPosition()); @@ -285,11 +286,18 @@ public void compute(double time) currentAcceleration.setToZero(); return; } + else if (Precision.equals(start.getTime(), end.getTime(), 0.05)) + { + double alpha = (time - start.getTime()) / (end.getTime() - start.getTime()); + currentPosition.interpolate(start.getPosition(), end.getPosition(), alpha); + currentVelocity.interpolate(start.getLinearVelocity(), end.getLinearVelocity(), alpha); + currentAcceleration.setToZero(); + return; + } // Initialize the segment trajectory, in case the index or waypoints have changed subTrajectory.setCubicDirectly(end.getTime() - start.getTime(), start.getPosition(), start.getLinearVelocity(), end.getPosition(), end.getLinearVelocity()); - double subTrajectoryTime = MathTools.clamp(time - start.getTime(), 0.0, end.getTime() - start.getTime()); subTrajectory.compute(subTrajectoryTime); currentPosition.set(subTrajectory.getPosition()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/SE3PIDGainsTrajectoryPoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/SE3PIDGainsTrajectoryPoint.java new file mode 100644 index 00000000000..f87b454b0cb --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/SE3PIDGainsTrajectoryPoint.java @@ -0,0 +1,67 @@ +package us.ihmc.robotics.math.trajectories.trajectorypoints; + +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.SE3PIDGainsTrajectoryPointBasics; +import us.ihmc.robotics.math.trajectories.waypoints.SE3PIDGainsWaypoint; + +import java.text.DecimalFormat; +import java.text.NumberFormat; + +public class SE3PIDGainsTrajectoryPoint implements SE3PIDGainsTrajectoryPointBasics +{ + private final SE3PIDGainsWaypoint gainsWaypoint = new SE3PIDGainsWaypoint(); + private double time; + + public SE3PIDGainsTrajectoryPoint() + { + } + + public SE3PIDGainsTrajectoryPoint(double time, PID3DGains angularGains, PID3DGains linearGains) + { + set(time, angularGains, linearGains); + } + + @Override + public void setAngular(PID3DGains angular) + { + gainsWaypoint.setAngular(angular); + } + + @Override + public void setLinear(PID3DGains linear) + { + gainsWaypoint.setLinear(linear); + } + + @Override + public PID3DGains getAngular() + { + return gainsWaypoint.getAngular(); + } + + @Override + public PID3DGains getLinear() + { + return gainsWaypoint.getLinear(); + } + + @Override + public void setTime(double time) + { + this.time = time; + } + + @Override + public double getTime() + { + return time; + } + + @Override + public String toString() + { + NumberFormat doubleFormat = new DecimalFormat(" 0.00;-0.00"); + String timeString = "time = " + doubleFormat.format(getTime()); + return "PIDGainsTrajectoryPoint: (" + timeString + ", " + gainsWaypoint.toString() + ")"; + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointBasics.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointBasics.java new file mode 100644 index 00000000000..e6ebd81532d --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointBasics.java @@ -0,0 +1,46 @@ +package us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces; + +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.math.trajectories.waypoints.interfaces.SE3PIDGainsWaypointBasics; +import us.ihmc.robotics.math.trajectories.waypoints.interfaces.SE3PIDGainsWaypointReadOnly; + +public interface SE3PIDGainsTrajectoryPointBasics extends SE3PIDGainsTrajectoryPointReadOnly, TrajectoryPointBasics, SE3PIDGainsWaypointBasics +{ + default void set(double time, PID3DGains angular, PID3DGains linear) + { + setTime(time); + set(angular, linear); + } + + default void set(SE3PIDGainsTrajectoryPointBasics other) + { + setTime(other.getTime()); + SE3PIDGainsWaypointBasics.super.set(other); + } + + default void set(double time, SE3PIDGainsWaypointReadOnly waypoint) + { + setTime(time); + set(waypoint); + } + + @Override + default void setToNaN() + { + setTimeToNaN(); + SE3PIDGainsWaypointBasics.super.setToNaN(); + } + + @Override + default void setToZero() + { + setTimeToZero(); + SE3PIDGainsWaypointBasics.super.setToZero(); + } + + @Override + default boolean containsNaN() + { + return SE3PIDGainsTrajectoryPointReadOnly.super.containsNaN(); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointReadOnly.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointReadOnly.java new file mode 100644 index 00000000000..c6c7532ebce --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/interfaces/SE3PIDGainsTrajectoryPointReadOnly.java @@ -0,0 +1,24 @@ +package us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces; + +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.robotics.math.trajectories.waypoints.interfaces.SE3PIDGainsWaypointReadOnly; + +public interface SE3PIDGainsTrajectoryPointReadOnly extends TrajectoryPointReadOnly, SE3PIDGainsWaypointReadOnly +{ + @Override + default boolean containsNaN() + { + return isTimeNaN() || SE3PIDGainsWaypointReadOnly.super.containsNaN(); + } + + + default boolean equals(SE3PIDGainsTrajectoryPointReadOnly other) + { + return EuclidCoreTools.equals(getTime(), other.getTime()) && SE3PIDGainsWaypointReadOnly.super.equals(other); + } + + default boolean epsilonEquals(SE3PIDGainsTrajectoryPointReadOnly other, double epsilon) + { + return EuclidCoreTools.epsilonEquals(getTime(), other.getTime(), epsilon) && SE3PIDGainsWaypointReadOnly.super.epsilonEquals(other, epsilon); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/lists/SE3PIDGainsTrajectoryPointList.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/lists/SE3PIDGainsTrajectoryPointList.java new file mode 100644 index 00000000000..43f1f831e76 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/trajectorypoints/lists/SE3PIDGainsTrajectoryPointList.java @@ -0,0 +1,68 @@ +package us.ihmc.robotics.math.trajectories.trajectorypoints.lists; + +import us.ihmc.commons.lists.RecyclingArrayList; +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.math.trajectories.trajectorypoints.SE3PIDGainsTrajectoryPoint; +import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.SE3PIDGainsTrajectoryPointBasics; +import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.TrajectoryPointListBasics; + +public class SE3PIDGainsTrajectoryPointList implements TrajectoryPointListBasics +{ + private final RecyclingArrayList trajectoryPoints = new RecyclingArrayList<>(SE3PIDGainsTrajectoryPoint.class); + + @Override + public void addTrajectoryPoint(SE3PIDGainsTrajectoryPoint trajectoryPoint) + { + trajectoryPoints.add().set(trajectoryPoint); + } + + public void addTrajectoryPoint(SE3PIDGainsTrajectoryPointBasics trajectoryPoint) + { + trajectoryPoints.add().set(trajectoryPoint); + } + + public void addTrajectoryPoint(double time, PID3DGains angularGains, PID3DGains linearGains) + { + trajectoryPoints.add().set(time, angularGains, linearGains); + } + + @Override + public void clear() + { + trajectoryPoints.clear(); + } + + @Override + public SE3PIDGainsTrajectoryPoint getTrajectoryPoint(int trajectoryPointIndex) + { + return trajectoryPoints.get(trajectoryPointIndex); + } + + @Override + public int getNumberOfTrajectoryPoints() + { + return trajectoryPoints.size(); + } + + public boolean epsilonEquals(SE3PIDGainsTrajectoryPointList other, double epsilon) + { + if (getNumberOfTrajectoryPoints() != other.getNumberOfTrajectoryPoints()) + { + return false; + } + for (int i = 0; i < getNumberOfTrajectoryPoints(); i++) + { + if (!getTrajectoryPoint(i).epsilonEquals(other.getTrajectoryPoint(i), epsilon)) + { + return false; + } + } + return true; + } + + @Override + public String toString() + { + return trajectoryPoints.toString(); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/SE3PIDGainsWaypoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/SE3PIDGainsWaypoint.java new file mode 100644 index 00000000000..ce8cf00a875 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/SE3PIDGainsWaypoint.java @@ -0,0 +1,42 @@ +package us.ihmc.robotics.math.trajectories.waypoints; + +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.math.trajectories.waypoints.interfaces.SE3PIDGainsWaypointBasics; + +public class SE3PIDGainsWaypoint implements SE3PIDGainsWaypointBasics +{ + private PID3DGains linearGains; + private PID3DGains angularGains; + + @Override + public void setAngular(PID3DGains angular) + { + this.angularGains = angular; + } + + @Override + public void setLinear(PID3DGains linear) + { + this.linearGains = linear; + } + + @Override + public PID3DGains getAngular() + { + return angularGains; + } + + @Override + public PID3DGains getLinear() + { + return linearGains; + } + + @Override + public String toString() + { + String linearString = "linear = " + linearGains.toString(); + String angularString = "angular = " + angularGains.toString(); + return "PIDGainsWaypoint: (" + linearString + ", " + angularString + ")"; + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointBasics.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointBasics.java new file mode 100644 index 00000000000..92c62e75873 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointBasics.java @@ -0,0 +1,51 @@ +package us.ihmc.robotics.math.trajectories.waypoints.interfaces; + +import us.ihmc.euclid.interfaces.Clearable; +import us.ihmc.robotics.controllers.pidGains.PID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.ZeroablePID3DGains; + +public interface SE3PIDGainsWaypointBasics extends SE3PIDGainsWaypointReadOnly, Clearable +{ + void setAngular(PID3DGains angular); + + void setLinear(PID3DGains linear); + + default void set(PID3DGains angular, PID3DGains linear) + { + setAngular(angular); + setLinear(linear); + } + + default void set(SE3PIDGainsWaypointReadOnly other) + { + setAngular(other.getAngular()); + setLinear(other.getLinear()); + } + + @Override + default void setToNaN() + { + setAngular(new DefaultPID3DGains()); + setLinear(new DefaultPID3DGains()); + + getAngular().setProportionalGains(Double.NaN); + getLinear().setProportionalGains(Double.NaN); + getAngular().setDerivativeGains(Double.NaN); + getLinear().setDerivativeGains(Double.NaN); + + } + + @Override + default void setToZero() + { + setAngular(new ZeroablePID3DGains()); + setLinear(new ZeroablePID3DGains()); + } + + @Override + default boolean containsNaN() + { + return SE3PIDGainsWaypointReadOnly.super.containsNaN(); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointReadOnly.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointReadOnly.java new file mode 100644 index 00000000000..1407d8e552c --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/waypoints/interfaces/SE3PIDGainsWaypointReadOnly.java @@ -0,0 +1,141 @@ +package us.ihmc.robotics.math.trajectories.waypoints.interfaces; + +import us.ihmc.euclid.interfaces.EuclidGeometry; +import us.ihmc.robotics.controllers.pidGains.PID3DGains; + +import java.util.Arrays; + +public interface SE3PIDGainsWaypointReadOnly +{ + PID3DGains getAngular(); + + PID3DGains getLinear(); + + default double[] getAngularProportionalGains() + { + return getAngular().getProportionalGains(); + } + + default double[] getAngularIntegralGains() + { + return getAngular().getIntegralGains(); + } + + default double[] getAngularDerivativeGains() + { + return getAngular().getDerivativeGains(); + } + + default double[] getAngularDampingRatios() + { + return getAngular().getDampingRatios(); + } + + default double getAngularMaximumIntegralError() + { + return getAngular().getMaximumIntegralError(); + } + + default double getAngularMaximumDerivativeError() + { + return getAngular().getMaximumDerivativeError(); + } + + default double getAngularMaximumProportionalError() + { + return getAngular().getMaximumProportionalError(); + } + + default double getAngularMaximumFeedback() + { + return getAngular().getMaximumFeedback(); + } + + default double getAngularMaximumFeedbackRate() + { + return getAngular().getMaximumFeedbackRate(); + } + + default double[] getLinearProportionalGains() + { + return getLinear().getProportionalGains(); + } + + default double[] getLinearIntegralGains() + { + return getLinear().getIntegralGains(); + } + + default double[] getLinearDerivativeGains() + { + return getLinear().getDerivativeGains(); + } + + default double[] getLinearDampingRatios() + { + return getLinear().getDampingRatios(); + } + + default double getLinearMaximumIntegralError() + { + return getLinear().getMaximumIntegralError(); + } + + default double getLinearMaximumDerivativeError() + { + return getLinear().getMaximumDerivativeError(); + } + + default double getLinearMaximumProportionalError() + { + return getLinear().getMaximumProportionalError(); + } + + default double getLinearMaximumFeedback() + { + return getLinear().getMaximumFeedback(); + } + + default double getLinearMaximumFeedbackRate() + { + return getLinear().getMaximumFeedbackRate(); + } + + default boolean equals(EuclidGeometry geometry) + { + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof SE3PIDGainsWaypointReadOnly)) + return false; + + SE3PIDGainsWaypointReadOnly other = (SE3PIDGainsWaypointReadOnly) geometry; + + if (!getAngular().equals(other.getAngular())) + return false; + if (!getLinear().equals(other.getLinear())) + return false; + return true; + } + + default boolean epsilonEquals(SE3PIDGainsWaypointReadOnly other, double epsilon) + { + // PIDGains does not have an epsilonEquals method + if (!getAngular().equals(other.getAngular())) + return false; + if (!getLinear().equals(other.getLinear())) + return false; + return true; + } + + default boolean containsNaN() + { + return Arrays.stream(getAngularProportionalGains()).anyMatch(Double::isNaN); + } + + default String toString(String format) + { + return String.format("Angular: %s, Linear: %s", getAngular().toString(), getLinear().toString()); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/SelectionMatrix6D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/SelectionMatrix6D.java index 0fd5031ebb0..231543e147d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/SelectionMatrix6D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/SelectionMatrix6D.java @@ -1,11 +1,14 @@ package us.ihmc.robotics.screwTheory; +import gnu.trove.list.array.TIntArrayList; import org.ejml.MatrixDimensionException; import org.ejml.data.DMatrixRMaj; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import java.util.List; + /** * The {@code SelectionMatrix6D} provides a simple way to define for a given application what are * the axes of interest for spatial data consisting of an angular part and a linear part. @@ -679,6 +682,69 @@ public int getNumberOfSelectedAxes() return linearPart.getNumberOfSelectedAxes() + angularPart.getNumberOfSelectedAxes(); } + /** + * Returns a list of indices for the active axes + * @param activeAxesToPack list to store the active axes + */ + public void getActiveAxes(List activeAxesToPack) + { + activeAxesToPack.clear(); + if (isAngularXSelected()) + activeAxesToPack.add(0); + if (isAngularYSelected()) + activeAxesToPack.add(1); + if (isAngularZSelected()) + activeAxesToPack.add(2); + if (isLinearXSelected()) + activeAxesToPack.add(3); + if (isLinearYSelected()) + activeAxesToPack.add(4); + if (isLinearZSelected()) + activeAxesToPack.add(5); + } + + /** + * Returns a list of indices for the active axes + * @param activeAxesToPack list to store the active axes + */ + public void getActiveAxes(int[] activeAxesToPack) + { + int index = 0; + if (isAngularXSelected()) + activeAxesToPack[index++] = 0; + if (isAngularYSelected()) + activeAxesToPack[index++] = 1; + if (isAngularZSelected()) + activeAxesToPack[index++] = 2; + if (isLinearXSelected()) + activeAxesToPack[index++] = 3; + if (isLinearYSelected()) + activeAxesToPack[index++] = 4; + if (isLinearZSelected()) + activeAxesToPack[index++] = 5; + } + + /** + * Returns a list of indices for the active axes + * @param activeAxesToPack list to store the active axes + */ + public void getActiveAxes(TIntArrayList activeAxesToPack) + { + activeAxesToPack.reset(); + if (isAngularXSelected()) + activeAxesToPack.add(0); + if (isAngularYSelected()) + activeAxesToPack.add(1); + if (isAngularZSelected()) + activeAxesToPack.add(2); + if (isLinearXSelected()) + activeAxesToPack.add(3); + if (isLinearYSelected()) + activeAxesToPack.add(4); + if (isLinearZSelected()) + activeAxesToPack.add(5); + } + @Override public String toString() { diff --git a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/MatrixMissingToolsTest.java b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/MatrixMissingToolsTest.java index 723b4d1ee7b..2ece344d684 100644 --- a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/MatrixMissingToolsTest.java +++ b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/MatrixMissingToolsTest.java @@ -4,6 +4,8 @@ import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; import org.ejml.dense.row.RandomMatrices_DDRM; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.interfaces.decomposition.SingularValueDecomposition_F64; import org.junit.jupiter.api.Test; import us.ihmc.commons.RandomNumbers; import us.ihmc.euclid.tools.EuclidCoreRandomTools; @@ -463,4 +465,32 @@ public void testElementWiseLessThan() assertThrows(IllegalArgumentException.class, () -> MatrixMissingTools.elementWiseLessThan(new DMatrixRMaj(1, 2), new DMatrixRMaj(2, 2))); } + + @Test + public void testSqrt() + { + Random random = new Random(41584L); + int iters = 1000; + DMatrixRMaj U = new DMatrixRMaj(0, 0); + DMatrixRMaj W = new DMatrixRMaj(0, 0); + DMatrixRMaj Vt = new DMatrixRMaj(0, 0); + DMatrixRMaj temp = new DMatrixRMaj(0, 0); + SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(true, true, true); + + for (int matrixSize = 2; matrixSize < 10; matrixSize++) + { + for (int i = 0; i < iters; i++) + { + DMatrixRMaj A = RandomMatrices_DDRM.symmetricPosDef(matrixSize, random); + DMatrixRMaj A_sqrt = new DMatrixRMaj(matrixSize, matrixSize); + + MatrixMissingTools.sqrt(A, A_sqrt, temp, U, W, Vt, svd); + + DMatrixRMaj Asqrt_times_Asqrt = new DMatrixRMaj(matrixSize, matrixSize); + CommonOps_DDRM.mult(A_sqrt, A_sqrt, Asqrt_times_Asqrt); + + MatrixTestTools.assertMatrixEquals(A, Asqrt_times_Asqrt, 1.0e-7); + } + } + } } \ No newline at end of file From 19e405b1190be7bc87f54805872d85b2854080ca Mon Sep 17 00:00:00 2001 From: lpenco Date: Thu, 12 Dec 2024 11:56:59 -0600 Subject: [PATCH 2/5] messages for impedance --- ...dJointspaceTaskspaceTrajectoryMessage_.idl | 10 + .../controller_msgs/msg/PID3DGains_.idl | 31 +++ .../controller_msgs/msg/PIDGains_.idl | 26 +++ .../msg/SE3PIDGainsTrajectoryMessage_.idl | 37 +++ .../SE3PIDGainsTrajectoryPointMessage_.idl | 41 ++++ ...dJointspaceTaskspaceTrajectoryMessage.java | 40 +++- ...eTaskspaceTrajectoryMessagePubSubType.java | 22 +- .../controller_msgs/msg/dds/PID3DGains.java | 202 ++++++++++++++++ .../msg/dds/PID3DGainsPubSubType.java | 218 ++++++++++++++++++ .../controller_msgs/msg/dds/PIDGains.java | 145 ++++++++++++ .../msg/dds/PIDGainsPubSubType.java | 177 ++++++++++++++ .../msg/dds/SE3PIDGainsTrajectoryMessage.java | 146 ++++++++++++ ...E3PIDGainsTrajectoryMessagePubSubType.java | 169 ++++++++++++++ .../SE3PIDGainsTrajectoryPointMessage.java | 167 ++++++++++++++ ...GainsTrajectoryPointMessagePubSubType.java | 175 ++++++++++++++ ...idJointspaceTaskspaceTrajectoryMessage.msg | 6 + .../ros1/controller_msgs/msg/PID3DGains.msg | 19 ++ .../ros1/controller_msgs/msg/PIDGains.msg | 11 + .../msg/SE3PIDGainsTrajectoryMessage.msg | 13 ++ .../msg/SE3PIDGainsTrajectoryPointMessage.msg | 17 ++ 20 files changed, 1670 insertions(+), 2 deletions(-) create mode 100644 ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PID3DGains_.idl create mode 100644 ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PIDGains_.idl create mode 100644 ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGains.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGainsPubSubType.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGains.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGainsPubSubType.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PID3DGains.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PIDGains.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl index fd0c6070368..f223909b5cd 100644 --- a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl @@ -2,6 +2,8 @@ #define __controller_msgs__msg__HandHybridJointspaceTaskspaceTrajectoryMessage__idl__ #include "controller_msgs/msg/./JointspaceTrajectoryMessage_.idl" +#include "controller_msgs/msg/./SE3PIDGainsTrajectoryMessage_.idl" +#include "controller_msgs/msg/./WrenchTrajectoryMessage_.idl" #include "ihmc_common_msgs/msg/./SE3TrajectoryMessage_.idl" module controller_msgs { @@ -47,6 +49,14 @@ module controller_msgs * The indexing for the joints goes increasingly from the first shoulder joint to the last arm joint. */ controller_msgs::msg::dds::JointspaceTrajectoryMessage jointspace_trajectory_message; + /** + * The feedforward taskspace trajectory information. + */ + controller_msgs::msg::dds::WrenchTrajectoryMessage feedforward_taskspace_trajectory_message; + /** + * The PIDGains for the taskspace controller. + */ + controller_msgs::msg::dds::SE3PIDGainsTrajectoryMessage taskspace_pid_gains_trajectory_message; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PID3DGains_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PID3DGains_.idl new file mode 100644 index 00000000000..62c60b1bc69 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PID3DGains_.idl @@ -0,0 +1,31 @@ +#ifndef __controller_msgs__msg__PID3DGains__idl__ +#define __controller_msgs__msg__PID3DGains__idl__ + +#include "controller_msgs/msg/./PIDGains_.idl" +module controller_msgs +{ + module msg + { + module dds + { + + /** + * this represents the 3d gains for a PID controller + */ + @TypeCode(type="controller_msgs::msg::dds_::PID3DGains_") + struct PID3DGains + { + controller_msgs::msg::dds::PIDGains gains_x; + controller_msgs::msg::dds::PIDGains gains_y; + controller_msgs::msg::dds::PIDGains gains_z; + double maximum_feedback; + double maximum_feedback_rate; + double maximum_integral_error; + double maximum_derivative_error; + double maximum_proportional_error; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PIDGains_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PIDGains_.idl new file mode 100644 index 00000000000..a474c106557 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/PIDGains_.idl @@ -0,0 +1,26 @@ +#ifndef __controller_msgs__msg__PIDGains__idl__ +#define __controller_msgs__msg__PIDGains__idl__ + +module controller_msgs +{ + module msg + { + module dds + { + + /** + * this represents the gains for a PID controller + */ + @TypeCode(type="controller_msgs::msg::dds_::PIDGains_") + struct PIDGains + { + double kp; + double kd; + double ki; + double zeta; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryMessage_.idl new file mode 100644 index 00000000000..47085912135 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryMessage_.idl @@ -0,0 +1,37 @@ +#ifndef __controller_msgs__msg__SE3PIDGainsTrajectoryMessage__idl__ +#define __controller_msgs__msg__SE3PIDGainsTrajectoryMessage__idl__ + +#include "controller_msgs/msg/./SE3PIDGainsTrajectoryPointMessage_.idl" +#include "ihmc_common_msgs/msg/./QueueableMessage_.idl" +module controller_msgs +{ + module msg + { + module dds + { + + /** + * This message is part of the IHMC whole-body controller API. + * This class is used to change the PID gains over time. + */ + @TypeCode(type="controller_msgs::msg::dds_::SE3PIDGainsTrajectoryMessage_") + struct SE3PIDGainsTrajectoryMessage + { + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + unsigned long sequence_id; + /** + * List of trajectory points (in taskpsace) to go through while executing the trajectory. + */ + sequence pid_gains_trajectory_points; + /** + * Properties for queueing trajectories. + */ + ihmc_common_msgs::msg::dds::QueueableMessage queueing_properties; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage_.idl new file mode 100644 index 00000000000..f91933038e1 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage_.idl @@ -0,0 +1,41 @@ +#ifndef __controller_msgs__msg__SE3PIDGainsTrajectoryPointMessage__idl__ +#define __controller_msgs__msg__SE3PIDGainsTrajectoryPointMessage__idl__ + +#include "controller_msgs/msg/./PID3DGains_.idl" +module controller_msgs +{ + module msg + { + module dds + { + + /** + * This message is part of the IHMC whole-body controller API. + * This class is used to change the PID gains over time. + * It holds the necessary information for one trajectory point. + */ + @TypeCode(type="controller_msgs::msg::dds_::SE3PIDGainsTrajectoryPointMessage_") + struct SE3PIDGainsTrajectoryPointMessage + { + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + unsigned long sequence_id; + /** + * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts. + */ + double time; + /** + * Define the desired linear PID gains for the trajectory point. + */ + controller_msgs::msg::dds::PID3DGains linear_gains; + /** + * Define the desired angular PID gains for the trajectory point. + */ + controller_msgs::msg::dds::PID3DGains angular_gains; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java index d499e0585d6..9081d582c40 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java @@ -37,11 +37,21 @@ public class HandHybridJointspaceTaskspaceTrajectoryMessage extends Packet getPubSubType() { return HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType::new; @@ -154,6 +184,8 @@ public boolean epsilonEquals(HandHybridJointspaceTaskspaceTrajectoryMessage othe if (!this.taskspace_trajectory_message_.epsilonEquals(other.taskspace_trajectory_message_, epsilon)) return false; if (!this.jointspace_trajectory_message_.epsilonEquals(other.jointspace_trajectory_message_, epsilon)) return false; + if (!this.feedforward_taskspace_trajectory_message_.epsilonEquals(other.feedforward_taskspace_trajectory_message_, epsilon)) return false; + if (!this.taskspace_pid_gains_trajectory_message_.epsilonEquals(other.taskspace_pid_gains_trajectory_message_, epsilon)) return false; return true; } @@ -175,6 +207,8 @@ public boolean equals(Object other) if (!this.taskspace_trajectory_message_.equals(otherMyClass.taskspace_trajectory_message_)) return false; if (!this.jointspace_trajectory_message_.equals(otherMyClass.jointspace_trajectory_message_)) return false; + if (!this.feedforward_taskspace_trajectory_message_.equals(otherMyClass.feedforward_taskspace_trajectory_message_)) return false; + if (!this.taskspace_pid_gains_trajectory_message_.equals(otherMyClass.taskspace_pid_gains_trajectory_message_)) return false; return true; } @@ -194,7 +228,11 @@ public java.lang.String toString() builder.append("taskspace_trajectory_message="); builder.append(this.taskspace_trajectory_message_); builder.append(", "); builder.append("jointspace_trajectory_message="); - builder.append(this.jointspace_trajectory_message_); + builder.append(this.jointspace_trajectory_message_); builder.append(", "); + builder.append("feedforward_taskspace_trajectory_message="); + builder.append(this.feedforward_taskspace_trajectory_message_); builder.append(", "); + builder.append("taskspace_pid_gains_trajectory_message="); + builder.append(this.taskspace_pid_gains_trajectory_message_); builder.append("}"); return builder.toString(); } diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType.java index b0b4eab3d45..e0056fe5e1f 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType.java @@ -15,7 +15,7 @@ public class HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType implements @Override public final java.lang.String getDefinitionChecksum() { - return "c01e1a08af9f36b626b12612e79eed35ede251d3a0c3778bc602e7c8b907260b"; + return "57f0562a4836d1e7fb01985aa69986ae3d8fe5c608c96cbac28f49436ec2445d"; } @Override @@ -62,6 +62,10 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + current_alignment += controller_msgs.msg.dds.WrenchTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + return current_alignment - initial_alignment; } @@ -88,6 +92,10 @@ public final static int getCdrSerializedSize(controller_msgs.msg.dds.HandHybridJ current_alignment += controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.getCdrSerializedSize(data.getJointspaceTrajectoryMessage(), current_alignment); + current_alignment += controller_msgs.msg.dds.WrenchTrajectoryMessagePubSubType.getCdrSerializedSize(data.getFeedforwardTaskspaceTrajectoryMessage(), current_alignment); + + current_alignment += controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessagePubSubType.getCdrSerializedSize(data.getTaskspacePidGainsTrajectoryMessage(), current_alignment); + return current_alignment - initial_alignment; } @@ -102,6 +110,8 @@ public static void write(controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTr ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.write(data.getTaskspaceTrajectoryMessage(), cdr); controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.write(data.getJointspaceTrajectoryMessage(), cdr); + controller_msgs.msg.dds.WrenchTrajectoryMessagePubSubType.write(data.getFeedforwardTaskspaceTrajectoryMessage(), cdr); + controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessagePubSubType.write(data.getTaskspacePidGainsTrajectoryMessage(), cdr); } public static void read(controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTrajectoryMessage data, us.ihmc.idl.CDR cdr) @@ -114,6 +124,8 @@ public static void read(controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTra ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.read(data.getTaskspaceTrajectoryMessage(), cdr); controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.read(data.getJointspaceTrajectoryMessage(), cdr); + controller_msgs.msg.dds.WrenchTrajectoryMessagePubSubType.read(data.getFeedforwardTaskspaceTrajectoryMessage(), cdr); + controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessagePubSubType.read(data.getTaskspacePidGainsTrajectoryMessage(), cdr); } @@ -127,6 +139,10 @@ public final void serialize(controller_msgs.msg.dds.HandHybridJointspaceTaskspac ser.write_type_a("jointspace_trajectory_message", new controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType(), data.getJointspaceTrajectoryMessage()); + ser.write_type_a("feedforward_taskspace_trajectory_message", new controller_msgs.msg.dds.WrenchTrajectoryMessagePubSubType(), data.getFeedforwardTaskspaceTrajectoryMessage()); + + ser.write_type_a("taskspace_pid_gains_trajectory_message", new controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessagePubSubType(), data.getTaskspacePidGainsTrajectoryMessage()); + } @Override @@ -139,6 +155,10 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_ ser.read_type_a("jointspace_trajectory_message", new controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType(), data.getJointspaceTrajectoryMessage()); + ser.read_type_a("feedforward_taskspace_trajectory_message", new controller_msgs.msg.dds.WrenchTrajectoryMessagePubSubType(), data.getFeedforwardTaskspaceTrajectoryMessage()); + + ser.read_type_a("taskspace_pid_gains_trajectory_message", new controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessagePubSubType(), data.getTaskspacePidGainsTrajectoryMessage()); + } public static void staticCopy(controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTrajectoryMessage src, controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTrajectoryMessage dest) diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGains.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGains.java new file mode 100644 index 00000000000..85d77009a2f --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGains.java @@ -0,0 +1,202 @@ +package controller_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * this represents the 3d gains for a PID controller + */ +public class PID3DGains extends Packet implements Settable, EpsilonComparable +{ + public controller_msgs.msg.dds.PIDGains gains_x_; + public controller_msgs.msg.dds.PIDGains gains_y_; + public controller_msgs.msg.dds.PIDGains gains_z_; + public double maximum_feedback_; + public double maximum_feedback_rate_; + public double maximum_integral_error_; + public double maximum_derivative_error_; + public double maximum_proportional_error_; + + public PID3DGains() + { + gains_x_ = new controller_msgs.msg.dds.PIDGains(); + gains_y_ = new controller_msgs.msg.dds.PIDGains(); + gains_z_ = new controller_msgs.msg.dds.PIDGains(); + } + + public PID3DGains(PID3DGains other) + { + this(); + set(other); + } + + public void set(PID3DGains other) + { + controller_msgs.msg.dds.PIDGainsPubSubType.staticCopy(other.gains_x_, gains_x_); + controller_msgs.msg.dds.PIDGainsPubSubType.staticCopy(other.gains_y_, gains_y_); + controller_msgs.msg.dds.PIDGainsPubSubType.staticCopy(other.gains_z_, gains_z_); + maximum_feedback_ = other.maximum_feedback_; + + maximum_feedback_rate_ = other.maximum_feedback_rate_; + + maximum_integral_error_ = other.maximum_integral_error_; + + maximum_derivative_error_ = other.maximum_derivative_error_; + + maximum_proportional_error_ = other.maximum_proportional_error_; + + } + + + public controller_msgs.msg.dds.PIDGains getGainsX() + { + return gains_x_; + } + + + public controller_msgs.msg.dds.PIDGains getGainsY() + { + return gains_y_; + } + + + public controller_msgs.msg.dds.PIDGains getGainsZ() + { + return gains_z_; + } + + public void setMaximumFeedback(double maximum_feedback) + { + maximum_feedback_ = maximum_feedback; + } + public double getMaximumFeedback() + { + return maximum_feedback_; + } + + public void setMaximumFeedbackRate(double maximum_feedback_rate) + { + maximum_feedback_rate_ = maximum_feedback_rate; + } + public double getMaximumFeedbackRate() + { + return maximum_feedback_rate_; + } + + public void setMaximumIntegralError(double maximum_integral_error) + { + maximum_integral_error_ = maximum_integral_error; + } + public double getMaximumIntegralError() + { + return maximum_integral_error_; + } + + public void setMaximumDerivativeError(double maximum_derivative_error) + { + maximum_derivative_error_ = maximum_derivative_error; + } + public double getMaximumDerivativeError() + { + return maximum_derivative_error_; + } + + public void setMaximumProportionalError(double maximum_proportional_error) + { + maximum_proportional_error_ = maximum_proportional_error; + } + public double getMaximumProportionalError() + { + return maximum_proportional_error_; + } + + + public static Supplier getPubSubType() + { + return PID3DGainsPubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return PID3DGainsPubSubType::new; + } + + @Override + public boolean epsilonEquals(PID3DGains other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.gains_x_.epsilonEquals(other.gains_x_, epsilon)) return false; + if (!this.gains_y_.epsilonEquals(other.gains_y_, epsilon)) return false; + if (!this.gains_z_.epsilonEquals(other.gains_z_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.maximum_feedback_, other.maximum_feedback_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.maximum_feedback_rate_, other.maximum_feedback_rate_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.maximum_integral_error_, other.maximum_integral_error_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.maximum_derivative_error_, other.maximum_derivative_error_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.maximum_proportional_error_, other.maximum_proportional_error_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof PID3DGains)) return false; + + PID3DGains otherMyClass = (PID3DGains) other; + + if (!this.gains_x_.equals(otherMyClass.gains_x_)) return false; + if (!this.gains_y_.equals(otherMyClass.gains_y_)) return false; + if (!this.gains_z_.equals(otherMyClass.gains_z_)) return false; + if(this.maximum_feedback_ != otherMyClass.maximum_feedback_) return false; + + if(this.maximum_feedback_rate_ != otherMyClass.maximum_feedback_rate_) return false; + + if(this.maximum_integral_error_ != otherMyClass.maximum_integral_error_) return false; + + if(this.maximum_derivative_error_ != otherMyClass.maximum_derivative_error_) return false; + + if(this.maximum_proportional_error_ != otherMyClass.maximum_proportional_error_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("PID3DGains {"); + builder.append("gains_x="); + builder.append(this.gains_x_); builder.append(", "); + builder.append("gains_y="); + builder.append(this.gains_y_); builder.append(", "); + builder.append("gains_z="); + builder.append(this.gains_z_); builder.append(", "); + builder.append("maximum_feedback="); + builder.append(this.maximum_feedback_); builder.append(", "); + builder.append("maximum_feedback_rate="); + builder.append(this.maximum_feedback_rate_); builder.append(", "); + builder.append("maximum_integral_error="); + builder.append(this.maximum_integral_error_); builder.append(", "); + builder.append("maximum_derivative_error="); + builder.append(this.maximum_derivative_error_); builder.append(", "); + builder.append("maximum_proportional_error="); + builder.append(this.maximum_proportional_error_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGainsPubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGainsPubSubType.java new file mode 100644 index 00000000000..cf9c4f94b10 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PID3DGainsPubSubType.java @@ -0,0 +1,218 @@ +package controller_msgs.msg.dds; + +/** +* +* Topic data type of the struct "PID3DGains" defined in "PID3DGains_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from PID3DGains_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit PID3DGains_.idl instead. +* +*/ +public class PID3DGainsPubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "controller_msgs::msg::dds_::PID3DGains_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "d4da515998705e0bd9e95e5b3676322e8e22957f12aba60eeaa7349227a66fe5"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(controller_msgs.msg.dds.PID3DGains data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.PID3DGains data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += controller_msgs.msg.dds.PIDGainsPubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += controller_msgs.msg.dds.PIDGainsPubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += controller_msgs.msg.dds.PIDGainsPubSubType.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); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + 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; + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.PID3DGains data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.PID3DGains data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += controller_msgs.msg.dds.PIDGainsPubSubType.getCdrSerializedSize(data.getGainsX(), current_alignment); + + current_alignment += controller_msgs.msg.dds.PIDGainsPubSubType.getCdrSerializedSize(data.getGainsY(), current_alignment); + + current_alignment += controller_msgs.msg.dds.PIDGainsPubSubType.getCdrSerializedSize(data.getGainsZ(), current_alignment); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + 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; + } + + public static void write(controller_msgs.msg.dds.PID3DGains data, us.ihmc.idl.CDR cdr) + { + controller_msgs.msg.dds.PIDGainsPubSubType.write(data.getGainsX(), cdr); + controller_msgs.msg.dds.PIDGainsPubSubType.write(data.getGainsY(), cdr); + controller_msgs.msg.dds.PIDGainsPubSubType.write(data.getGainsZ(), cdr); + cdr.write_type_6(data.getMaximumFeedback()); + + cdr.write_type_6(data.getMaximumFeedbackRate()); + + cdr.write_type_6(data.getMaximumIntegralError()); + + cdr.write_type_6(data.getMaximumDerivativeError()); + + cdr.write_type_6(data.getMaximumProportionalError()); + + } + + public static void read(controller_msgs.msg.dds.PID3DGains data, us.ihmc.idl.CDR cdr) + { + controller_msgs.msg.dds.PIDGainsPubSubType.read(data.getGainsX(), cdr); + controller_msgs.msg.dds.PIDGainsPubSubType.read(data.getGainsY(), cdr); + controller_msgs.msg.dds.PIDGainsPubSubType.read(data.getGainsZ(), cdr); + data.setMaximumFeedback(cdr.read_type_6()); + + data.setMaximumFeedbackRate(cdr.read_type_6()); + + data.setMaximumIntegralError(cdr.read_type_6()); + + data.setMaximumDerivativeError(cdr.read_type_6()); + + data.setMaximumProportionalError(cdr.read_type_6()); + + + } + + @Override + public final void serialize(controller_msgs.msg.dds.PID3DGains data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("gains_x", new controller_msgs.msg.dds.PIDGainsPubSubType(), data.getGainsX()); + + ser.write_type_a("gains_y", new controller_msgs.msg.dds.PIDGainsPubSubType(), data.getGainsY()); + + ser.write_type_a("gains_z", new controller_msgs.msg.dds.PIDGainsPubSubType(), data.getGainsZ()); + + ser.write_type_6("maximum_feedback", data.getMaximumFeedback()); + ser.write_type_6("maximum_feedback_rate", data.getMaximumFeedbackRate()); + ser.write_type_6("maximum_integral_error", data.getMaximumIntegralError()); + ser.write_type_6("maximum_derivative_error", data.getMaximumDerivativeError()); + ser.write_type_6("maximum_proportional_error", data.getMaximumProportionalError()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.PID3DGains data) + { + ser.read_type_a("gains_x", new controller_msgs.msg.dds.PIDGainsPubSubType(), data.getGainsX()); + + ser.read_type_a("gains_y", new controller_msgs.msg.dds.PIDGainsPubSubType(), data.getGainsY()); + + ser.read_type_a("gains_z", new controller_msgs.msg.dds.PIDGainsPubSubType(), data.getGainsZ()); + + data.setMaximumFeedback(ser.read_type_6("maximum_feedback")); + data.setMaximumFeedbackRate(ser.read_type_6("maximum_feedback_rate")); + data.setMaximumIntegralError(ser.read_type_6("maximum_integral_error")); + data.setMaximumDerivativeError(ser.read_type_6("maximum_derivative_error")); + data.setMaximumProportionalError(ser.read_type_6("maximum_proportional_error")); + } + + public static void staticCopy(controller_msgs.msg.dds.PID3DGains src, controller_msgs.msg.dds.PID3DGains dest) + { + dest.set(src); + } + + @Override + public controller_msgs.msg.dds.PID3DGains createData() + { + return new controller_msgs.msg.dds.PID3DGains(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(controller_msgs.msg.dds.PID3DGains data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(controller_msgs.msg.dds.PID3DGains data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(controller_msgs.msg.dds.PID3DGains src, controller_msgs.msg.dds.PID3DGains dest) + { + staticCopy(src, dest); + } + + @Override + public PID3DGainsPubSubType newInstance() + { + return new PID3DGainsPubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGains.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGains.java new file mode 100644 index 00000000000..1faa5ee2c8b --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGains.java @@ -0,0 +1,145 @@ +package controller_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * this represents the gains for a PID controller + */ +public class PIDGains extends Packet implements Settable, EpsilonComparable +{ + public double kp_; + public double kd_; + public double ki_; + public double zeta_; + + public PIDGains() + { + } + + public PIDGains(PIDGains other) + { + this(); + set(other); + } + + public void set(PIDGains other) + { + kp_ = other.kp_; + + kd_ = other.kd_; + + ki_ = other.ki_; + + zeta_ = other.zeta_; + + } + + public void setKp(double kp) + { + kp_ = kp; + } + public double getKp() + { + return kp_; + } + + public void setKd(double kd) + { + kd_ = kd; + } + public double getKd() + { + return kd_; + } + + public void setKi(double ki) + { + ki_ = ki; + } + public double getKi() + { + return ki_; + } + + public void setZeta(double zeta) + { + zeta_ = zeta; + } + public double getZeta() + { + return zeta_; + } + + + public static Supplier getPubSubType() + { + return PIDGainsPubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return PIDGainsPubSubType::new; + } + + @Override + public boolean epsilonEquals(PIDGains other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kp_, other.kp_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kd_, other.kd_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.ki_, other.ki_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.zeta_, other.zeta_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof PIDGains)) return false; + + PIDGains otherMyClass = (PIDGains) other; + + if(this.kp_ != otherMyClass.kp_) return false; + + if(this.kd_ != otherMyClass.kd_) return false; + + if(this.ki_ != otherMyClass.ki_) return false; + + if(this.zeta_ != otherMyClass.zeta_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("PIDGains {"); + builder.append("kp="); + builder.append(this.kp_); builder.append(", "); + builder.append("kd="); + builder.append(this.kd_); builder.append(", "); + builder.append("ki="); + builder.append(this.ki_); builder.append(", "); + builder.append("zeta="); + builder.append(this.zeta_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGainsPubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGainsPubSubType.java new file mode 100644 index 00000000000..a751346b43f --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PIDGainsPubSubType.java @@ -0,0 +1,177 @@ +package controller_msgs.msg.dds; + +/** +* +* Topic data type of the struct "PIDGains" defined in "PIDGains_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from PIDGains_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit PIDGains_.idl instead. +* +*/ +public class PIDGainsPubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "controller_msgs::msg::dds_::PIDGains_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "6562b6de3dd8a12acd757a519470e6e60412ec757589c7df4c6f94afcccafc47"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(controller_msgs.msg.dds.PIDGains data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.PIDGains data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + 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; + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.PIDGains data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.PIDGains data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + 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; + } + + public static void write(controller_msgs.msg.dds.PIDGains data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_6(data.getKp()); + + cdr.write_type_6(data.getKd()); + + cdr.write_type_6(data.getKi()); + + cdr.write_type_6(data.getZeta()); + + } + + public static void read(controller_msgs.msg.dds.PIDGains data, us.ihmc.idl.CDR cdr) + { + data.setKp(cdr.read_type_6()); + + data.setKd(cdr.read_type_6()); + + data.setKi(cdr.read_type_6()); + + data.setZeta(cdr.read_type_6()); + + + } + + @Override + public final void serialize(controller_msgs.msg.dds.PIDGains data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_6("kp", data.getKp()); + ser.write_type_6("kd", data.getKd()); + ser.write_type_6("ki", data.getKi()); + ser.write_type_6("zeta", data.getZeta()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.PIDGains data) + { + data.setKp(ser.read_type_6("kp")); + data.setKd(ser.read_type_6("kd")); + data.setKi(ser.read_type_6("ki")); + data.setZeta(ser.read_type_6("zeta")); + } + + public static void staticCopy(controller_msgs.msg.dds.PIDGains src, controller_msgs.msg.dds.PIDGains dest) + { + dest.set(src); + } + + @Override + public controller_msgs.msg.dds.PIDGains createData() + { + return new controller_msgs.msg.dds.PIDGains(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(controller_msgs.msg.dds.PIDGains data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(controller_msgs.msg.dds.PIDGains data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(controller_msgs.msg.dds.PIDGains src, controller_msgs.msg.dds.PIDGains dest) + { + staticCopy(src, dest); + } + + @Override + public PIDGainsPubSubType newInstance() + { + return new PIDGainsPubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessage.java new file mode 100644 index 00000000000..b5a75949873 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessage.java @@ -0,0 +1,146 @@ +package controller_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * This message is part of the IHMC whole-body controller API. + * This class is used to change the PID gains over time. + */ +public class SE3PIDGainsTrajectoryMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long sequence_id_; + /** + * List of trajectory points (in taskpsace) to go through while executing the trajectory. + */ + public us.ihmc.idl.IDLSequence.Object pid_gains_trajectory_points_; + /** + * Properties for queueing trajectories. + */ + public ihmc_common_msgs.msg.dds.QueueableMessage queueing_properties_; + + public SE3PIDGainsTrajectoryMessage() + { + pid_gains_trajectory_points_ = new us.ihmc.idl.IDLSequence.Object (200, new controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessagePubSubType()); + queueing_properties_ = new ihmc_common_msgs.msg.dds.QueueableMessage(); + + } + + public SE3PIDGainsTrajectoryMessage(SE3PIDGainsTrajectoryMessage other) + { + this(); + set(other); + } + + public void set(SE3PIDGainsTrajectoryMessage other) + { + sequence_id_ = other.sequence_id_; + + pid_gains_trajectory_points_.set(other.pid_gains_trajectory_points_); + ihmc_common_msgs.msg.dds.QueueableMessagePubSubType.staticCopy(other.queueing_properties_, queueing_properties_); + } + + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public void setSequenceId(long sequence_id) + { + sequence_id_ = sequence_id; + } + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long getSequenceId() + { + return sequence_id_; + } + + + /** + * List of trajectory points (in taskpsace) to go through while executing the trajectory. + */ + public us.ihmc.idl.IDLSequence.Object getPidGainsTrajectoryPoints() + { + return pid_gains_trajectory_points_; + } + + + /** + * Properties for queueing trajectories. + */ + public ihmc_common_msgs.msg.dds.QueueableMessage getQueueingProperties() + { + return queueing_properties_; + } + + + public static Supplier getPubSubType() + { + return SE3PIDGainsTrajectoryMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return SE3PIDGainsTrajectoryMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(SE3PIDGainsTrajectoryMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; + + if (this.pid_gains_trajectory_points_.size() != other.pid_gains_trajectory_points_.size()) { return false; } + else + { + for (int i = 0; i < this.pid_gains_trajectory_points_.size(); i++) + { if (!this.pid_gains_trajectory_points_.get(i).epsilonEquals(other.pid_gains_trajectory_points_.get(i), epsilon)) return false; } + } + + if (!this.queueing_properties_.epsilonEquals(other.queueing_properties_, epsilon)) return false; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof SE3PIDGainsTrajectoryMessage)) return false; + + SE3PIDGainsTrajectoryMessage otherMyClass = (SE3PIDGainsTrajectoryMessage) other; + + if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + + if (!this.pid_gains_trajectory_points_.equals(otherMyClass.pid_gains_trajectory_points_)) return false; + if (!this.queueing_properties_.equals(otherMyClass.queueing_properties_)) return false; + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("SE3PIDGainsTrajectoryMessage {"); + builder.append("sequence_id="); + builder.append(this.sequence_id_); builder.append(", "); + builder.append("pid_gains_trajectory_points="); + builder.append(this.pid_gains_trajectory_points_); builder.append(", "); + builder.append("queueing_properties="); + builder.append(this.queueing_properties_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessagePubSubType.java new file mode 100644 index 00000000000..c4d626ae2b0 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryMessagePubSubType.java @@ -0,0 +1,169 @@ +package controller_msgs.msg.dds; + +/** +* +* Topic data type of the struct "SE3PIDGainsTrajectoryMessage" defined in "SE3PIDGainsTrajectoryMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from SE3PIDGainsTrajectoryMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit SE3PIDGainsTrajectoryMessage_.idl instead. +* +*/ +public class SE3PIDGainsTrajectoryMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "controller_msgs::msg::dds_::SE3PIDGainsTrajectoryMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "18a9ffcf2c5a4f7ec8db4ffbd3db4c8888f187e6b1ecb820365201b8a108ec90"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) + { + current_alignment += controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += ihmc_common_msgs.msg.dds.QueueableMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getPidGainsTrajectoryPoints().size(); ++i0) + { + current_alignment += controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getPidGainsTrajectoryPoints().get(i0), current_alignment);} + + current_alignment += ihmc_common_msgs.msg.dds.QueueableMessagePubSubType.getCdrSerializedSize(data.getQueueingProperties(), current_alignment); + + + return current_alignment - initial_alignment; + } + + public static void write(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_4(data.getSequenceId()); + + if(data.getPidGainsTrajectoryPoints().size() <= 200) + cdr.write_type_e(data.getPidGainsTrajectoryPoints());else + throw new RuntimeException("pid_gains_trajectory_points field exceeds the maximum length"); + + ihmc_common_msgs.msg.dds.QueueableMessagePubSubType.write(data.getQueueingProperties(), cdr); + } + + public static void read(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, us.ihmc.idl.CDR cdr) + { + data.setSequenceId(cdr.read_type_4()); + + cdr.read_type_e(data.getPidGainsTrajectoryPoints()); + ihmc_common_msgs.msg.dds.QueueableMessagePubSubType.read(data.getQueueingProperties(), cdr); + + } + + @Override + public final void serialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_4("sequence_id", data.getSequenceId()); + ser.write_type_e("pid_gains_trajectory_points", data.getPidGainsTrajectoryPoints()); + ser.write_type_a("queueing_properties", new ihmc_common_msgs.msg.dds.QueueableMessagePubSubType(), data.getQueueingProperties()); + + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data) + { + data.setSequenceId(ser.read_type_4("sequence_id")); + ser.read_type_e("pid_gains_trajectory_points", data.getPidGainsTrajectoryPoints()); + ser.read_type_a("queueing_properties", new ihmc_common_msgs.msg.dds.QueueableMessagePubSubType(), data.getQueueingProperties()); + + } + + public static void staticCopy(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage src, controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage dest) + { + dest.set(src); + } + + @Override + public controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage createData() + { + return new controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage src, controller_msgs.msg.dds.SE3PIDGainsTrajectoryMessage dest) + { + staticCopy(src, dest); + } + + @Override + public SE3PIDGainsTrajectoryMessagePubSubType newInstance() + { + return new SE3PIDGainsTrajectoryMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessage.java new file mode 100644 index 00000000000..5a171c9599e --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessage.java @@ -0,0 +1,167 @@ +package controller_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * This message is part of the IHMC whole-body controller API. + * This class is used to change the PID gains over time. + * It holds the necessary information for one trajectory point. + */ +public class SE3PIDGainsTrajectoryPointMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long sequence_id_; + /** + * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts. + */ + public double time_; + /** + * Define the desired linear PID gains for the trajectory point. + */ + public controller_msgs.msg.dds.PID3DGains linear_gains_; + /** + * Define the desired angular PID gains for the trajectory point. + */ + public controller_msgs.msg.dds.PID3DGains angular_gains_; + + public SE3PIDGainsTrajectoryPointMessage() + { + linear_gains_ = new controller_msgs.msg.dds.PID3DGains(); + angular_gains_ = new controller_msgs.msg.dds.PID3DGains(); + } + + public SE3PIDGainsTrajectoryPointMessage(SE3PIDGainsTrajectoryPointMessage other) + { + this(); + set(other); + } + + public void set(SE3PIDGainsTrajectoryPointMessage other) + { + sequence_id_ = other.sequence_id_; + + time_ = other.time_; + + controller_msgs.msg.dds.PID3DGainsPubSubType.staticCopy(other.linear_gains_, linear_gains_); + controller_msgs.msg.dds.PID3DGainsPubSubType.staticCopy(other.angular_gains_, angular_gains_); + } + + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public void setSequenceId(long sequence_id) + { + sequence_id_ = sequence_id; + } + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long getSequenceId() + { + return sequence_id_; + } + + /** + * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts. + */ + public void setTime(double time) + { + time_ = time; + } + /** + * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts. + */ + public double getTime() + { + return time_; + } + + + /** + * Define the desired linear PID gains for the trajectory point. + */ + public controller_msgs.msg.dds.PID3DGains getLinearGains() + { + return linear_gains_; + } + + + /** + * Define the desired angular PID gains for the trajectory point. + */ + public controller_msgs.msg.dds.PID3DGains getAngularGains() + { + return angular_gains_; + } + + + public static Supplier getPubSubType() + { + return SE3PIDGainsTrajectoryPointMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return SE3PIDGainsTrajectoryPointMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(SE3PIDGainsTrajectoryPointMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false; + + if (!this.linear_gains_.epsilonEquals(other.linear_gains_, epsilon)) return false; + if (!this.angular_gains_.epsilonEquals(other.angular_gains_, epsilon)) return false; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof SE3PIDGainsTrajectoryPointMessage)) return false; + + SE3PIDGainsTrajectoryPointMessage otherMyClass = (SE3PIDGainsTrajectoryPointMessage) other; + + if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + + if(this.time_ != otherMyClass.time_) return false; + + if (!this.linear_gains_.equals(otherMyClass.linear_gains_)) return false; + if (!this.angular_gains_.equals(otherMyClass.angular_gains_)) return false; + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("SE3PIDGainsTrajectoryPointMessage {"); + builder.append("sequence_id="); + builder.append(this.sequence_id_); builder.append(", "); + builder.append("time="); + builder.append(this.time_); builder.append(", "); + builder.append("linear_gains="); + builder.append(this.linear_gains_); builder.append(", "); + builder.append("angular_gains="); + builder.append(this.angular_gains_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessagePubSubType.java new file mode 100644 index 00000000000..b6ca268fbdf --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3PIDGainsTrajectoryPointMessagePubSubType.java @@ -0,0 +1,175 @@ +package controller_msgs.msg.dds; + +/** +* +* Topic data type of the struct "SE3PIDGainsTrajectoryPointMessage" defined in "SE3PIDGainsTrajectoryPointMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from SE3PIDGainsTrajectoryPointMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit SE3PIDGainsTrajectoryPointMessage_.idl instead. +* +*/ +public class SE3PIDGainsTrajectoryPointMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "controller_msgs::msg::dds_::SE3PIDGainsTrajectoryPointMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "e04cdc111cd7487d07550d375bb203fa8ebd0e8dd045e829d11c40569dbad462"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += controller_msgs.msg.dds.PID3DGainsPubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += controller_msgs.msg.dds.PID3DGainsPubSubType.getMaxCdrSerializedSize(current_alignment); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += controller_msgs.msg.dds.PID3DGainsPubSubType.getCdrSerializedSize(data.getLinearGains(), current_alignment); + + current_alignment += controller_msgs.msg.dds.PID3DGainsPubSubType.getCdrSerializedSize(data.getAngularGains(), current_alignment); + + + return current_alignment - initial_alignment; + } + + public static void write(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_4(data.getSequenceId()); + + cdr.write_type_6(data.getTime()); + + controller_msgs.msg.dds.PID3DGainsPubSubType.write(data.getLinearGains(), cdr); + controller_msgs.msg.dds.PID3DGainsPubSubType.write(data.getAngularGains(), cdr); + } + + public static void read(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, us.ihmc.idl.CDR cdr) + { + data.setSequenceId(cdr.read_type_4()); + + data.setTime(cdr.read_type_6()); + + controller_msgs.msg.dds.PID3DGainsPubSubType.read(data.getLinearGains(), cdr); + controller_msgs.msg.dds.PID3DGainsPubSubType.read(data.getAngularGains(), cdr); + + } + + @Override + public final void serialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_4("sequence_id", data.getSequenceId()); + ser.write_type_6("time", data.getTime()); + ser.write_type_a("linear_gains", new controller_msgs.msg.dds.PID3DGainsPubSubType(), data.getLinearGains()); + + ser.write_type_a("angular_gains", new controller_msgs.msg.dds.PID3DGainsPubSubType(), data.getAngularGains()); + + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data) + { + data.setSequenceId(ser.read_type_4("sequence_id")); + data.setTime(ser.read_type_6("time")); + ser.read_type_a("linear_gains", new controller_msgs.msg.dds.PID3DGainsPubSubType(), data.getLinearGains()); + + ser.read_type_a("angular_gains", new controller_msgs.msg.dds.PID3DGainsPubSubType(), data.getAngularGains()); + + } + + public static void staticCopy(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage src, controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage dest) + { + dest.set(src); + } + + @Override + public controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage createData() + { + return new controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage src, controller_msgs.msg.dds.SE3PIDGainsTrajectoryPointMessage dest) + { + staticCopy(src, dest); + } + + @Override + public SE3PIDGainsTrajectoryPointMessagePubSubType newInstance() + { + return new SE3PIDGainsTrajectoryPointMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg index d4f504b1e21..c8d3187ccdd 100644 --- a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg @@ -24,4 +24,10 @@ ihmc_common_msgs/SE3TrajectoryMessage taskspace_trajectory_message # The indexing for the joints goes increasingly from the first shoulder joint to the last arm joint. controller_msgs/JointspaceTrajectoryMessage jointspace_trajectory_message +# The feedforward taskspace trajectory information. +controller_msgs/WrenchTrajectoryMessage feedforward_taskspace_trajectory_message + +# The PIDGains for the taskspace controller. +controller_msgs/SE3PIDGainsTrajectoryMessage taskspace_pid_gains_trajectory_message + diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PID3DGains.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PID3DGains.msg new file mode 100644 index 00000000000..b2f2764ebcf --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PID3DGains.msg @@ -0,0 +1,19 @@ +# this represents the 3d gains for a PID controller + +controller_msgs/PIDGains gains_x + +controller_msgs/PIDGains gains_y + +controller_msgs/PIDGains gains_z + +float64 maximum_feedback + +float64 maximum_feedback_rate + +float64 maximum_integral_error + +float64 maximum_derivative_error + +float64 maximum_proportional_error + + diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PIDGains.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PIDGains.msg new file mode 100644 index 00000000000..2cdf61a943b --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/PIDGains.msg @@ -0,0 +1,11 @@ +# this represents the gains for a PID controller + +float64 kp + +float64 kd + +float64 ki + +float64 zeta + + diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg new file mode 100644 index 00000000000..2df78028e21 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryMessage.msg @@ -0,0 +1,13 @@ +# This message is part of the IHMC whole-body controller API. +# This class is used to change the PID gains over time. + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + +# List of trajectory points (in taskpsace) to go through while executing the trajectory. +controller_msgs/SE3PIDGainsTrajectoryPointMessage[] pid_gains_trajectory_points + +# Properties for queueing trajectories. +ihmc_common_msgs/QueueableMessage queueing_properties + + diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg new file mode 100644 index 00000000000..5e5f38421c4 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/SE3PIDGainsTrajectoryPointMessage.msg @@ -0,0 +1,17 @@ +# This message is part of the IHMC whole-body controller API. +# This class is used to change the PID gains over time. +# It holds the necessary information for one trajectory point. + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + +# Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts. +float64 time + +# Define the desired linear PID gains for the trajectory point. +controller_msgs/PID3DGains linear_gains + +# Define the desired angular PID gains for the trajectory point. +controller_msgs/PID3DGains angular_gains + + From ad5c4bd0532a6ae742e4c63079cfc0ec62cfb216 Mon Sep 17 00:00:00 2001 From: lpenco Date: Thu, 12 Dec 2024 12:09:35 -0600 Subject: [PATCH 3/5] fixed imports --- .../pelvis/PelvisHeightControlState.java | 2 +- .../pelvis/UserPelvisOrientationManager.java | 2 +- .../RigidBodyOrientationController.java | 15 ++++++++ .../rigidBody/RigidBodyPoseController.java | 2 ++ .../RigidBodyPositionController.java | 18 ++++++++++ .../RigidBodyTaskspaceControlState.java | 1 + .../WalkingCommandConsumer.java | 36 +------------------ .../OrientationFeedbackController.java | 1 + .../taskspace/PointFeedbackController.java | 1 + .../taskspace/SpatialFeedbackController.java | 6 +++- 10 files changed, 46 insertions(+), 38 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisHeightControlState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisHeightControlState.java index 01c307a8494..ae568f624be 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisHeightControlState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisHeightControlState.java @@ -109,7 +109,7 @@ public PelvisHeightControlState(HighLevelHumanoidControllerToolbox controllerToo YoDouble yoTime = controllerToolbox.getYoTime(); YoGraphicsListRegistry graphicsListRegistry = controllerToolbox.getYoGraphicsListRegistry(); - positionController = new RigidBodyPositionController(pelvis, elevator, elevator, pelvisFrame, baseFrame, yoTime, false, registry, graphicsListRegistry); + positionController = new RigidBodyPositionController(pelvis, elevator, elevator, pelvisFrame, baseFrame, yoTime, false, false, registry, graphicsListRegistry); defaultHeight = new DoubleParameter(getClass().getSimpleName() + "DefaultHeight", registry); minHeight = new DoubleParameter(getClass().getSimpleName() + "MinHeight", registry, 0.0); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/UserPelvisOrientationManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/UserPelvisOrientationManager.java index 53fe9b07fe3..bbf8f98c3bf 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/UserPelvisOrientationManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/UserPelvisOrientationManager.java @@ -30,7 +30,7 @@ public UserPelvisOrientationManager(PID3DGainsReadOnly gains, HighLevelHumanoidC baseFrame = controllerToolbox.getReferenceFrames().getMidFootZUpGroundFrame(); YoDouble yoTime = controllerToolbox.getYoTime(); - orientationController = new RigidBodyOrientationController(pelvis, elevator, elevator, baseFrame, yoTime, null, false, registry); + orientationController = new RigidBodyOrientationController(pelvis, elevator, elevator, baseFrame, yoTime, null, false, false, registry); orientationController.setGains(gains); homeOrientation = new FrameQuaternion(baseFrame); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java index 331df4cf678..8f51513aa93 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyOrientationController.java @@ -91,6 +91,21 @@ public RigidBodyOrientationController(RigidBodyBasics bodyToControl, statusHelper = new TaskspaceTrajectoryStatusMessageHelper(bodyToControl); } + public RigidBodyOrientationController(RigidBodyBasics bodyToControl, + RigidBodyBasics baseBody, + RigidBodyBasics elevator, + ReferenceFrame baseFrame, + YoDouble yoTime, + RigidBodyJointControlHelper jointControlHelper, + boolean enableFunctionGenerators, + boolean enableImpedanceControl, + YoRegistry parentRegistry) + { + this(bodyToControl, baseBody, elevator, baseFrame, yoTime, jointControlHelper, enableFunctionGenerators, new YoBoolean(bodyToControl.getName() + "EnableImpedanceControlOrientation", parentRegistry), parentRegistry); + YoBoolean enableImpedanceControlYoBoolean = (YoBoolean) parentRegistry.getVariable(bodyToControl.getName() + "EnableImpedanceControlOrientation"); + enableImpedanceControlYoBoolean.set(enableImpedanceControl); + } + public void setGains(PID3DGainsReadOnly gains) { orientationHelper.setGains(gains); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java index cc2ffca142d..0e23788df56 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPoseController.java @@ -17,8 +17,10 @@ import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.EuclideanTrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.JointspaceTrajectoryCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3PIDGainsTrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.WrenchTrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.converter.CommandConversionTools; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java index 8d169ef3646..05c4a77e9d7 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyPositionController.java @@ -94,6 +94,24 @@ public RigidBodyPositionController(RigidBodyBasics bodyToControl, statusHelper = new TaskspaceTrajectoryStatusMessageHelper(bodyToControl); } + public RigidBodyPositionController(RigidBodyBasics bodyToControl, + RigidBodyBasics baseBody, + RigidBodyBasics elevator, + ReferenceFrame controlFrame, + ReferenceFrame baseFrame, + YoDouble yoTime, + boolean enableFunctionGenerators, + boolean enableImpedanceControl, + YoRegistry parentRegistry, + YoGraphicsListRegistry graphicsListRegistry) + { + this(bodyToControl, baseBody, elevator, controlFrame, baseFrame, yoTime, enableFunctionGenerators, + new YoBoolean(bodyToControl.getName() + "EnableImpedanceControlPosition", parentRegistry), + parentRegistry, graphicsListRegistry); + YoBoolean enableImpedanceControlYoBoolean = (YoBoolean) parentRegistry.getVariable(bodyToControl.getName() + "EnableImpedanceControlPosition"); + enableImpedanceControlYoBoolean.set(enableImpedanceControl); + } + public void setGains(PID3DGainsReadOnly gains) { positionHelper.setGains(gains); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java index 265dd22921f..d4611cb912f 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyTaskspaceControlState.java @@ -3,6 +3,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.EuclideanTrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.JointspaceTrajectoryCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3PIDGainsTrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.WrenchTrajectoryControllerCommand; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java index cc8dec9eac5..999e4020b63 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java @@ -18,41 +18,7 @@ import us.ihmc.communication.controllerAPI.StatusMessageOutputManager; import us.ihmc.communication.controllerAPI.command.Command; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.AbortWalkingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ArmDesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ArmTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.AutomaticManipulationAbortCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.CenterOfMassTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ChestHybridJointspaceTaskspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ChestTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ClearDelayQueueCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.DesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootLoadBearingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.FootstepDataListCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.GoHomeCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandHybridJointspaceTaskspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandLoadBearingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HandWrenchTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HeadHybridJointspaceTaskspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.HeadTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.JointspaceTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.LegTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.MomentumTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.NeckDesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.NeckTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PauseWalkingCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PelvisHeightTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PelvisOrientationTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PelvisTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.PrepareForLocomotionCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SpineDesiredAccelerationsCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SpineTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.StopAllTrajectoryCommand; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.WrenchTrajectoryControllerCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.*; import us.ihmc.humanoidRobotics.communication.directionalControlToolboxAPI.DirectionalControlInputCommand; import us.ihmc.humanoidRobotics.communication.fastWalkingAPI.FastWalkingGaitParametersCommand; import us.ihmc.humanoidRobotics.communication.packets.walking.HumanoidBodyPart; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java index 5372b202e94..63caaefb9d7 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/OrientationFeedbackController.java @@ -33,6 +33,7 @@ import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.Twist; import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.MatrixMissingTools; import us.ihmc.robotics.controllers.pidGains.YoPID3DGains; import us.ihmc.robotics.screwTheory.SelectionMatrix6D; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java index 2612732258b..11e18db01ff 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/PointFeedbackController.java @@ -37,6 +37,7 @@ import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.Twist; import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.MatrixMissingTools; import us.ihmc.robotics.controllers.pidGains.YoPID3DGains; import us.ihmc.robotics.screwTheory.SelectionMatrix6D; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java index f0fe5bdc35e..18573184a2a 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/taskspace/SpatialFeedbackController.java @@ -34,6 +34,7 @@ import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.Twist; import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.MatrixMissingTools; import us.ihmc.robotics.controllers.pidGains.YoPID3DGains; import us.ihmc.robotics.controllers.pidGains.YoPIDSE3Gains; import us.ihmc.robotics.screwTheory.SelectionMatrix6D; @@ -459,6 +460,9 @@ public void submitFeedbackControlCommand(SpatialFeedbackControlCommand command) @Override public void setEnabled(boolean isEnabled) + { + this.isEnabled.set(isEnabled); + } @Override public void setImpedanceEnabled(boolean isImpedanceEnabled) @@ -1168,7 +1172,7 @@ protected void proccessInverseDynamicsDesiredAcceleration(MovingReferenceFrame c private void computeInverseInertiaMatrix() { jacobianCalculator.clear(); - jacobianCalculator.setKinematicChain(bodyBase, endEffector); + jacobianCalculator.setKinematicChain(base.getChildrenJoints().get(0).getSuccessor(), endEffector); jacobianCalculator.setJacobianFrame(controlFrame); jacobianCalculator.reset(); From 8a91523c2769410e664f6964cd52beccd0ce7ed4 Mon Sep 17 00:00:00 2001 From: lpenco Date: Thu, 12 Dec 2024 16:27:46 -0600 Subject: [PATCH 4/5] added flag for impedance in message hybrid traj --- .../rigidBody/RigidBodyControlManager.java | 7 +++++ .../WalkingCommandConsumer.java | 1 + .../rdx/ui/affordances/RDXArmControlMode.java | 2 +- .../rdx/ui/affordances/RDXArmManager.java | 7 ++++- .../RDXTeleoperationManager.java | 6 +++-- ...dJointspaceTaskspaceTrajectoryCommand.java | 24 +++++++++++++++++ ...dJointspaceTaskspaceTrajectoryMessage_.idl | 4 +++ ...dJointspaceTaskspaceTrajectoryMessage.java | 27 +++++++++++++++++++ ...eTaskspaceTrajectoryMessagePubSubType.java | 13 ++++++++- ...idJointspaceTaskspaceTrajectoryMessage.msg | 3 +++ ...idJointspaceTaskspaceTrajectoryMessage.msg | 3 +++ 11 files changed, 92 insertions(+), 5 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java index 359c12358c3..d53e6c818db 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java @@ -58,6 +58,7 @@ public class RigidBodyControlManager implements SCS2YoGraphicHolder private final String bodyName; private final RigidBodyBasics bodyToControl; + private final YoBoolean isImpedanceEnabled; private final YoRegistry registry; private final StateMachine stateMachine; private final YoEnum requestedState; @@ -106,6 +107,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl, YoRegistry parentRegistry) { this.bodyToControl = bodyToControl; + this.isImpedanceEnabled = isImpedanceEnabled; bodyName = bodyToControl.getName(); String namePrefix = bodyName + "Manager"; registry = new YoRegistry(namePrefix); @@ -763,6 +765,11 @@ public FeedbackControlCommandList createFeedbackControlTemplate() return ret; } + public YoBoolean getImpedanceEnabled() + { + return isImpedanceEnabled; + } + public RigidBodyBasics getBodyToControl() { return bodyToControl; diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java index 999e4020b63..c9221528d73 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java @@ -413,6 +413,7 @@ public void consumeManipulationCommands(WalkingState currentState, boolean allow HandHybridJointspaceTaskspaceTrajectoryCommand command = handHybridCommands.get(i); RobotSide robotSide = command.getRobotSide(); RigidBodyControlManager handManager = handManagers.get(robotSide); + handManager.getImpedanceEnabled().set(command.getImpedanceEnabled()); if (handManager != null && (allowCommand || command.getForceExecution())) { SE3TrajectoryControllerCommand taskspaceTrajectoryCommand = command.getTaskspaceTrajectoryCommand(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java index ac0aff274ae..f161c04111a 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java @@ -2,5 +2,5 @@ public enum RDXArmControlMode { - JOINTSPACE, TASKSPACE, HYBRID + JOINTSPACE, TASKSPACE, HYBRID, HYBRID_IMPEDANCE } \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java index 1121a9a54e9..de06740a881 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java @@ -249,6 +249,10 @@ public void renderImGuiWidgets() { armControlMode = RDXArmControlMode.HYBRID; } + if (ImGui.radioButton(labels.get("Hybrid Impedance"), armControlMode == RDXArmControlMode.HYBRID_IMPEDANCE)) + { + armControlMode = RDXArmControlMode.HYBRID_IMPEDANCE; + } ImGui.text("Taskspace trajectory frame:"); ImGui.sameLine(); @@ -388,10 +392,11 @@ public void executeDesiredArmCommand(RobotSide robotSide) RDXBaseUI.pushNotification("Commanding taskspace %s frame trajectory...".formatted(taskspaceTrajectoryFrame.getName())); communicationHelper.publishToController(handTrajectoryMessage); } - case HYBRID -> + case HYBRID, HYBRID_IMPEDANCE -> { HandHybridJointspaceTaskspaceTrajectoryMessage handHybridJointspaceTaskspaceTrajectoryMessage = new HandHybridJointspaceTaskspaceTrajectoryMessage(); + handHybridJointspaceTaskspaceTrajectoryMessage.setImpedanceEnabled(armControlMode == RDXArmControlMode.HYBRID_IMPEDANCE); handHybridJointspaceTaskspaceTrajectoryMessage.setRobotSide(robotSide.toByte()); handHybridJointspaceTaskspaceTrajectoryMessage.getTaskspaceTrajectoryMessage().set(se3TrajectoryMessage); handHybridJointspaceTaskspaceTrajectoryMessage.getJointspaceTrajectoryMessage().set(jointspaceTrajectoryMessage); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java index 48cbcc53b03..fc1256f9c6c 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java @@ -403,8 +403,10 @@ else if (hasEitherArm) { for (RobotSide side : interactableHands.sides()) { - desiredRobot.setArmShowing(side, !interactableHands.get(side).isDeleted() - && (armManager.getArmControlMode() == RDXArmControlMode.JOINTSPACE || armManager.getArmControlMode() == RDXArmControlMode.HYBRID)); + desiredRobot.setArmShowing(side, + !interactableHands.get(side).isDeleted() && (armManager.getArmControlMode() == RDXArmControlMode.JOINTSPACE + || armManager.getArmControlMode() == RDXArmControlMode.HYBRID + || armManager.getArmControlMode() == RDXArmControlMode.HYBRID_IMPEDANCE)); desiredRobot.setArmColor(side, RDXIKSolverColors.getColor(armManager.getArmIKSolvers().get(side).getQuality())); } } diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java index 1d0d9954464..f963d4ed15c 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java @@ -15,6 +15,7 @@ public class HandHybridJointspaceTaskspaceTrajectoryCommand private long sequenceId; private RobotSide robotSide; private boolean forceExecution = false; + private boolean impedanceEnabled = false; private final JointspaceTrajectoryCommand jointspaceTrajectoryCommand = new JointspaceTrajectoryCommand(); private final SE3TrajectoryControllerCommand taskspaceTrajectoryCommand = new SE3TrajectoryControllerCommand(); private final WrenchTrajectoryControllerCommand feedForwardTrajectoryCommand = new WrenchTrajectoryControllerCommand(); @@ -24,6 +25,16 @@ public HandHybridJointspaceTaskspaceTrajectoryCommand() { } + public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, boolean impedanceEnabled, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand, + JointspaceTrajectoryCommand jointspaceTrajectoryCommand) + { + this.robotSide = robotSide; + this.setForceExecution(forceExecution); + this.impedanceEnabled = impedanceEnabled; + this.jointspaceTrajectoryCommand.set(jointspaceTrajectoryCommand); + this.taskspaceTrajectoryCommand.set(taskspaceTrajectoryCommand); + } + public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand, JointspaceTrajectoryCommand jointspaceTrajectoryCommand) { @@ -86,6 +97,7 @@ public void clear() sequenceId = 0; robotSide = null; setForceExecution(false); + setImpedanceEnabled(false); jointspaceTrajectoryCommand.clear(); feedForwardTrajectoryCommand.clear(); pidGainsTrajectoryCommand.clear(); @@ -103,6 +115,7 @@ public void set(ReferenceFrameHashCodeResolver resolver, HandHybridJointspaceTas sequenceId = message.getSequenceId(); robotSide = RobotSide.fromByte(message.getRobotSide()); setForceExecution(message.getForceExecution()); + setImpedanceEnabled(message.getImpedanceEnabled()); jointspaceTrajectoryCommand.setFromMessage(message.getJointspaceTrajectoryMessage()); taskspaceTrajectoryCommand.set(resolver, message.getTaskspaceTrajectoryMessage()); feedForwardTrajectoryCommand.set(resolver, message.getFeedforwardTaskspaceTrajectoryMessage()); @@ -122,6 +135,7 @@ public void set(HandHybridJointspaceTaskspaceTrajectoryCommand other) sequenceId = other.sequenceId; robotSide = other.robotSide; setForceExecution(other.getForceExecution()); + setImpedanceEnabled(other.getImpedanceEnabled()); taskspaceTrajectoryCommand.set(other.getTaskspaceTrajectoryCommand()); jointspaceTrajectoryCommand.set(other.getJointspaceTrajectoryCommand()); feedForwardTrajectoryCommand.set(other.getFeedForwardTrajectoryCommand()); @@ -143,6 +157,16 @@ public void setForceExecution(boolean forceExecution) this.forceExecution = forceExecution; } + public boolean getImpedanceEnabled() + { + return impedanceEnabled; + } + + public void setImpedanceEnabled(boolean impedanceEnabled) + { + this.impedanceEnabled = impedanceEnabled; + } + public JointspaceTrajectoryCommand getJointspaceTrajectoryCommand() { return jointspaceTrajectoryCommand; diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl index f223909b5cd..f8b33c9bded 100644 --- a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl @@ -35,6 +35,10 @@ module controller_msgs * To by-pass the safety check and force the execution of this message, set this field to true. */ boolean force_execution; + /** + * Whether impedance control should be enabled or not. + */ + boolean impedance_enabled; /** * Specifies the side of the robot that will execute the trajectory. */ diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java index 9081d582c40..8dc64841a16 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java @@ -24,6 +24,10 @@ public class HandHybridJointspaceTaskspaceTrajectoryMessage extends Packet Date: Fri, 13 Dec 2024 13:40:36 -0600 Subject: [PATCH 5/5] added impedance option in behavior action --- .../rdx/ui/affordances/RDXArmControlMode.java | 2 +- .../rdx/ui/affordances/RDXArmManager.java | 22 ++++----------- .../behavior/actions/RDXHandPoseAction.java | 14 +++++++--- .../RDXTeleoperationManager.java | 5 +--- .../actions/HandPoseActionDefinition.java | 21 +++++++++++++++ .../actions/HandPoseActionExecutor.java | 1 + .../msg/HandPoseActionDefinitionMessage_.idl | 4 +++ .../BehaviorTreeStateMessagePubSubType.java | 2 +- .../dds/HandPoseActionDefinitionMessage.java | 27 +++++++++++++++++++ ...PoseActionDefinitionMessagePubSubType.java | 13 ++++++++- .../HandPoseActionStateMessagePubSubType.java | 2 +- .../msg/HandPoseActionDefinitionMessage.msg | 3 +++ .../msg/HandPoseActionDefinitionMessage.msg | 3 +++ 13 files changed, 90 insertions(+), 29 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java index f161c04111a..74d2ce86847 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java @@ -2,5 +2,5 @@ public enum RDXArmControlMode { - JOINTSPACE, TASKSPACE, HYBRID, HYBRID_IMPEDANCE + JOINTSPACE, TASKSPACE, IMPEDANCE } \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java index de06740a881..b5315983af9 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java @@ -245,13 +245,9 @@ public void renderImGuiWidgets() armControlMode = RDXArmControlMode.TASKSPACE; } ImGui.sameLine(); - if (ImGui.radioButton(labels.get("Hybrid"), armControlMode == RDXArmControlMode.HYBRID)) + if (ImGui.radioButton(labels.get("Impedance"), armControlMode == RDXArmControlMode.IMPEDANCE)) { - armControlMode = RDXArmControlMode.HYBRID; - } - if (ImGui.radioButton(labels.get("Hybrid Impedance"), armControlMode == RDXArmControlMode.HYBRID_IMPEDANCE)) - { - armControlMode = RDXArmControlMode.HYBRID_IMPEDANCE; + armControlMode = RDXArmControlMode.IMPEDANCE; } ImGui.text("Taskspace trajectory frame:"); @@ -384,23 +380,15 @@ public void executeDesiredArmCommand(RobotSide robotSide) RDXBaseUI.pushNotification("Commanding arm jointspace trajectory..."); communicationHelper.publishToController(armTrajectoryMessage); } - case TASKSPACE -> - { - HandTrajectoryMessage handTrajectoryMessage = new HandTrajectoryMessage(); - handTrajectoryMessage.setRobotSide(robotSide.toByte()); - handTrajectoryMessage.getSe3Trajectory().set(se3TrajectoryMessage); - RDXBaseUI.pushNotification("Commanding taskspace %s frame trajectory...".formatted(taskspaceTrajectoryFrame.getName())); - communicationHelper.publishToController(handTrajectoryMessage); - } - case HYBRID, HYBRID_IMPEDANCE -> + case TASKSPACE, IMPEDANCE -> { HandHybridJointspaceTaskspaceTrajectoryMessage handHybridJointspaceTaskspaceTrajectoryMessage = new HandHybridJointspaceTaskspaceTrajectoryMessage(); - handHybridJointspaceTaskspaceTrajectoryMessage.setImpedanceEnabled(armControlMode == RDXArmControlMode.HYBRID_IMPEDANCE); + handHybridJointspaceTaskspaceTrajectoryMessage.setImpedanceEnabled(armControlMode == RDXArmControlMode.IMPEDANCE); handHybridJointspaceTaskspaceTrajectoryMessage.setRobotSide(robotSide.toByte()); handHybridJointspaceTaskspaceTrajectoryMessage.getTaskspaceTrajectoryMessage().set(se3TrajectoryMessage); handHybridJointspaceTaskspaceTrajectoryMessage.getJointspaceTrajectoryMessage().set(jointspaceTrajectoryMessage); - RDXBaseUI.pushNotification("Commanding arm hybrid jointspace taskpace trajectory..."); + RDXBaseUI.pushNotification("Commanding arm hybrid jointspace taskpace trajectory...".formatted(taskspaceTrajectoryFrame.getName())); communicationHelper.publishToController(handHybridJointspaceTaskspaceTrajectoryMessage); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXHandPoseAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXHandPoseAction.java index 696574cf396..a7407e7f377 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXHandPoseAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXHandPoseAction.java @@ -81,6 +81,7 @@ public class RDXHandPoseAction extends RDXActionNode { - if (ImGui.radioButton(labels.get("Hybrid"), !imBoolean.get())) - imBoolean.set(false); - ImGui.sameLine(); - if (ImGui.radioButton(labels.get("Jointspace Only"), imBoolean.get())) + if (ImGui.radioButton(labels.get("Jointspace"), imBoolean.get())) imBoolean.set(true); + ImGui.sameLine(); + if (ImGui.radioButton(labels.get("Taskspace"), !imBoolean.get())) + imBoolean.set(false); }); + impedanceControlWrapper = new ImBooleanWrapper(definition::getImpedanceControl, + definition::setImpedanceControl, + imBoolean -> ImGui.checkbox(labels.get("Impedance Control"), imBoolean)); ImGuiLabelledWidgetAligner widgetAligner = new ImGuiLabelledWidgetAligner(); linearPositionWeightWidget = new ImGuiSliderDoubleWrapper("Linear Position Weight", "%.2f", 0.0, 100.0, definition::getLinearPositionWeight, @@ -381,6 +385,8 @@ protected void renderImGuiWidgetsInternal() jointSpaceControlWrapper.renderImGuiWidget(); if (!definition.getJointspaceOnly()) { + ImGui.sameLine(); + impedanceControlWrapper.renderImGuiWidget(); ImGui.sameLine(); holdPoseInWorldLaterWrapper.renderImGuiWidget(); } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java index fc1256f9c6c..c5714b94b0e 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java @@ -403,10 +403,7 @@ else if (hasEitherArm) { for (RobotSide side : interactableHands.sides()) { - desiredRobot.setArmShowing(side, - !interactableHands.get(side).isDeleted() && (armManager.getArmControlMode() == RDXArmControlMode.JOINTSPACE - || armManager.getArmControlMode() == RDXArmControlMode.HYBRID - || armManager.getArmControlMode() == RDXArmControlMode.HYBRID_IMPEDANCE)); + desiredRobot.setArmShowing(side, !interactableHands.get(side).isDeleted()); desiredRobot.setArmColor(side, RDXIKSolverColors.getColor(armManager.getArmIKSolvers().get(side).getQuality())); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionDefinition.java index c81910cb62b..897acba47c4 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionDefinition.java @@ -23,6 +23,7 @@ public class HandPoseActionDefinition extends ActionNodeDefinition implements Si public static final int MAX_NUMBER_OF_JOINTS = 7; public static final String CUSTOM_ANGLES_NAME = "CUSTOM_ANGLES"; public static final boolean DEFAULT_HOLD_POSE = false; + public static final boolean DEFAULT_IMPEDANCE_CONTROL = false; public static final double DEFAULT_LINEAR_POSITION_WEIGHT = 50.0; public static final double DEFAULT_ANGULAR_POSITION_WEIGHT = 50.0; public static final double DEFAULT_JOINTSPACE_WEIGHT = -1.0; @@ -32,6 +33,7 @@ public class HandPoseActionDefinition extends ActionNodeDefinition implements Si private final CRDTBidirectionalEnumField side; private final CRDTBidirectionalDouble trajectoryDuration; private final CRDTBidirectionalBoolean holdPoseInWorldLater; + private final CRDTBidirectionalBoolean impedanceControl; private final CRDTBidirectionalBoolean jointspaceOnly; private final CRDTBidirectionalBoolean usePredefinedJointAngles; /** Preset is null when using explicitly specified custom joint angles */ @@ -49,6 +51,7 @@ public class HandPoseActionDefinition extends ActionNodeDefinition implements Si private RobotSide onDiskSide; private double onDiskTrajectoryDuration; private boolean onDiskHoldPoseInWorldLater; + private boolean onDiskImpedanceControl; private boolean onDiskJointspaceOnly; private boolean onDiskUsePredefinedJointAngles; private PresetArmConfiguration onDiskPreset; @@ -68,6 +71,7 @@ public HandPoseActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory sa side = new CRDTBidirectionalEnumField<>(this, RobotSide.LEFT); trajectoryDuration = new CRDTBidirectionalDouble(this, DEFAULT_TRAJECTORY_DURATION); holdPoseInWorldLater = new CRDTBidirectionalBoolean(this, DEFAULT_HOLD_POSE); + impedanceControl = new CRDTBidirectionalBoolean(this, DEFAULT_IMPEDANCE_CONTROL); jointspaceOnly = new CRDTBidirectionalBoolean(this, DEFAULT_IS_JOINTSPACE_MODE); usePredefinedJointAngles = new CRDTBidirectionalBoolean(this, DEFAULT_USE_PREDEFINED_JOINT_ANGLES); preset = new CRDTBidirectionalEnumField<>(this, PresetArmConfiguration.HOME); @@ -107,6 +111,7 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("parentFrame", palmParentFrameName.getValue()); JSONTools.toJSON(jsonNode, palmTransformToParent.getValueReadOnly()); jsonNode.put("jointspaceOnly", jointspaceOnly.getValue()); + jsonNode.put("impedanceControl", impedanceControl.getValue()); jsonNode.put("holdPoseInWorldLater", holdPoseInWorldLater.getValue()); jsonNode.put("linearPositionWeight", linearPositionWeight.getValue()); jsonNode.put("angularPositionWeight", angularPositionWeight.getValue()); @@ -143,6 +148,7 @@ public void loadFromFile(JsonNode jsonNode) palmParentFrameName.setValue(jsonNode.get("parentFrame").textValue()); JSONTools.toEuclid(jsonNode, palmTransformToParent.getValueAndFreeze()); holdPoseInWorldLater.setValue(jsonNode.get("holdPoseInWorldLater").asBoolean()); + impedanceControl.setValue(jsonNode.get("impedanceControl").asBoolean()); jointspaceOnly.setValue(jsonNode.get("jointspaceOnly").asBoolean()); linearPositionWeight.setValue(jsonNode.get("linearPositionWeight").asDouble()); angularPositionWeight.setValue(jsonNode.get("angularPositionWeight").asDouble()); @@ -161,6 +167,7 @@ public void setOnDiskFields() onDiskSide = side.getValue(); onDiskTrajectoryDuration = trajectoryDuration.getValue(); onDiskHoldPoseInWorldLater = holdPoseInWorldLater.getValue(); + onDiskImpedanceControl = impedanceControl.getValue(); onDiskJointspaceOnly = jointspaceOnly.getValue(); onDiskUsePredefinedJointAngles = usePredefinedJointAngles.getValue(); onDiskPreset = preset.getValue(); @@ -183,6 +190,7 @@ public void undoAllNontopologicalChanges() side.setValue(onDiskSide); trajectoryDuration.setValue(onDiskTrajectoryDuration); holdPoseInWorldLater.setValue(onDiskHoldPoseInWorldLater); + impedanceControl.setValue(onDiskImpedanceControl); jointspaceOnly.setValue(onDiskJointspaceOnly); usePredefinedJointAngles.setValue(onDiskUsePredefinedJointAngles); preset.setValue(onDiskPreset); @@ -216,6 +224,7 @@ public boolean hasChanges() else { unchanged &= holdPoseInWorldLater.getValue() == onDiskHoldPoseInWorldLater; + unchanged &= impedanceControl.getValue() == onDiskImpedanceControl; unchanged &= jointspaceOnly.getValue() == onDiskJointspaceOnly; unchanged &= palmParentFrameName.getValue().equals(onDiskPalmParentFrameName); unchanged &= palmTransformToParent.getValueReadOnly().equals(onDiskPalmTransformToParent); @@ -239,6 +248,7 @@ public void toMessage(HandPoseActionDefinitionMessage message) message.setRobotSide(side.toMessage().toByte()); message.setTrajectoryDuration(trajectoryDuration.toMessage()); message.setHoldPoseInWorld(holdPoseInWorldLater.toMessage()); + message.setImpedanceControl(impedanceControl.toMessage()); message.setJointSpaceControl(jointspaceOnly.toMessage()); message.setUsePredefinedJointAngles(usePredefinedJointAngles.toMessage()); message.setPreset(preset.toMessageOrdinal()); @@ -259,6 +269,7 @@ public void fromMessage(HandPoseActionDefinitionMessage message) side.fromMessage(RobotSide.fromByte(message.getRobotSide())); trajectoryDuration.fromMessage(message.getTrajectoryDuration()); holdPoseInWorldLater.fromMessage(message.getHoldPoseInWorld()); + impedanceControl.fromMessage(message.getImpedanceControl()); jointspaceOnly.fromMessage(message.getJointSpaceControl()); usePredefinedJointAngles.fromMessage(message.getUsePredefinedJointAngles()); preset.fromMessageOrdinal(message.getPreset(), PresetArmConfiguration.values); @@ -301,6 +312,16 @@ public void setHoldPoseInWorldLater(boolean holdPoseInWorldLater) this.holdPoseInWorldLater.setValue(holdPoseInWorldLater); } + public boolean getImpedanceControl() + { + return impedanceControl.getValue(); + } + + public void setImpedanceControl(boolean impedanceControl) + { + this.impedanceControl.setValue(impedanceControl); + } + public boolean getJointspaceOnly() { return jointspaceOnly.getValue(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java index bea37a61b2e..cece1249575 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java @@ -227,6 +227,7 @@ else if (state.getPalmFrame().isChildOfWorld()) HandHybridJointspaceTaskspaceTrajectoryMessage handHybridJointspaceTaskspaceTrajectoryMessage = new HandHybridJointspaceTaskspaceTrajectoryMessage(); handHybridJointspaceTaskspaceTrajectoryMessage.setRobotSide(definition.getSide().toByte()); + handHybridJointspaceTaskspaceTrajectoryMessage.setImpedanceEnabled(definition.getImpedanceControl()); handHybridJointspaceTaskspaceTrajectoryMessage.getTaskspaceTrajectoryMessage().set(se3TrajectoryMessage); handHybridJointspaceTaskspaceTrajectoryMessage.getJointspaceTrajectoryMessage().set(jointspaceTrajectoryMessage); state.getLogger().info("Publishing arm hybrid jointspace taskpace"); diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/HandPoseActionDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/HandPoseActionDefinitionMessage_.idl index 7585b08a19f..b0915ed8cc8 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/HandPoseActionDefinitionMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/HandPoseActionDefinitionMessage_.idl @@ -50,6 +50,10 @@ module behavior_msgs * The trajectory duration */ double trajectory_duration; + /** + * Whether impedance control is active + */ + boolean impedance_control; /** * Whether maintaining the rigid body controlled in world after the action is complete */ diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java index 6c6d515c2a8..6291ebdbdf7 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java @@ -15,7 +15,7 @@ public class BehaviorTreeStateMessagePubSubType implements us.ihmc.pubsub.TopicD @Override public final java.lang.String getDefinitionChecksum() { - return "7209f90a58b2a4948db884ddf526779b083614b3b561c87c72d8ac4af6de1c6f"; + return "f7740c251c76e2ea67a69ab51e9ebc307964c3f623e3d34ad8c1de280e423793"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/HandPoseActionDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/HandPoseActionDefinitionMessage.java index 3d5933925dd..5a52ca9247e 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/HandPoseActionDefinitionMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/HandPoseActionDefinitionMessage.java @@ -44,6 +44,10 @@ public class HandPoseActionDefinitionMessage extends Packet