From 8183169fc09dedbddacb2a06edbca4d8e6fbf170 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Mon, 25 Mar 2024 16:46:33 -0500 Subject: [PATCH 01/32] Framework for a trigger kick message. --- .../factories/ControllerAPIDefinition.java | 1 + .../WalkingHighLevelHumanoidController.java | 1 + .../WalkingCommandConsumer.java | 44 ++---- .../command/TriggerKickCommand.java | 54 +++++++ .../msg/TriggerKickMessage_.idl | 27 ++++ .../msg/dds/TriggerKickMessage.java | 100 ++++++++++++ .../msg/dds/TriggerKickMessagePubSubType.java | 143 ++++++++++++++++++ .../msg/TriggerKickMessage.msg | 5 + .../msg/TriggerKickMessage.msg | 7 + 9 files changed, 348 insertions(+), 34 deletions(-) create mode 100644 ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java create mode 100644 ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java index 811ce449a51c..06c6b68da69c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java @@ -72,6 +72,7 @@ public class ControllerAPIDefinition commands.add(AbortWalkingCommand.class); commands.add(PrepareForLocomotionCommand.class); commands.add(PauseWalkingCommand.class); + commands.add(TriggerKickCommand.class); commands.add(SpineDesiredAccelerationsCommand.class); commands.add(HandLoadBearingCommand.class); commands.add(HandHybridJointspaceTaskspaceTrajectoryCommand.class); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java index af4406d45250..d80f124322f3 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java @@ -641,6 +641,7 @@ public void doAction() commandConsumer.consumeStopAllTrajectoryCommands(); commandConsumer.consumeFootCommands(); commandConsumer.consumeAbortWalkingCommands(abortWalkingRequested); +// commandConsumer.consumeTriggerKickCommands(); commandConsumer.consumePelvisCommands(currentState, allowUpperBodyMotionDuringLocomotion.getBooleanValue()); commandConsumer.consumeManipulationCommands(currentState, allowUpperBodyMotionDuringLocomotion.getBooleanValue()); commandConsumer.handleAutomaticManipulationAbortOnICPError(currentState); 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 92368b66e6fe..d375fd6a7569 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,40 +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.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; @@ -156,6 +123,7 @@ public WalkingCommandConsumer(CommandInputManager commandInputManager, commandsToRegister.add(CenterOfMassTrajectoryCommand.class); commandsToRegister.add(AbortWalkingCommand.class); commandsToRegister.add(PrepareForLocomotionCommand.class); + commandsToRegister.add(TriggerKickCommand.class); commandsToRegister.add(DirectionalControlInputCommand.class); commandsToRegister.add(FastWalkingGaitParametersCommand.class); @@ -605,6 +573,14 @@ public void consumeAbortWalkingCommands(YoBoolean abortWalkingRequested) abortWalkingRequested.set(commandConsumerWithDelayBuffers.pollNewestCommand(AbortWalkingCommand.class).isAbortWalkingRequested()); } + public void consumeTriggerKickCommands(YoBoolean triggerKickRequested) + { + if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(TriggerKickCommand.class)) + return; + + triggerKickRequested.set(commandConsumerWithDelayBuffers.pollNewestCommand(TriggerKickCommand.class).isTriggerKickRequested()); + } + public void consumePrepareForLocomotionCommands() { if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(PrepareForLocomotionCommand.class)) diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java new file mode 100644 index 000000000000..a50e5ea8a1fd --- /dev/null +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java @@ -0,0 +1,54 @@ +package us.ihmc.humanoidRobotics.communication.controllerAPI.command; + +import controller_msgs.msg.dds.TriggerKickMessage; +import us.ihmc.communication.controllerAPI.command.Command; + +public class TriggerKickCommand implements Command +{ + private long sequenceId; + private boolean triggerKickRequested = false; + + @Override + public void clear() + { + sequenceId = 0; + triggerKickRequested = false; + } + + @Override + public void set(TriggerKickCommand other) + { + sequenceId = other.sequenceId; + triggerKickRequested = other.isTriggerKickRequested(); + } + + @Override + public void setFromMessage(TriggerKickMessage message) + { + sequenceId = message.getSequenceId(); + triggerKickRequested = true; + } + + public boolean isTriggerKickRequested() + { + return triggerKickRequested; + } + + @Override + public Class getMessageClass() + { + return TriggerKickMessage.class; + } + + @Override + public boolean isCommandValid() + { + return true; + } + + @Override + public long getSequenceId() + { + return sequenceId; + } +} diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl new file mode 100644 index 000000000000..10253493eeec --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl @@ -0,0 +1,27 @@ +#ifndef __controller_msgs__msg__TriggerKickMessage__idl__ +#define __controller_msgs__msg__TriggerKickMessage__idl__ + +module controller_msgs +{ + module msg + { + module dds + { + + /** + * This message is part of the IHMC whole-body controller API. + * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + */ + @TypeCode(type="controller_msgs::msg::dds_::TriggerKickMessage_") + struct TriggerKickMessage + { + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + unsigned long sequence_id; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java new file mode 100644 index 000000000000..5dbca0b8fd16 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java @@ -0,0 +1,100 @@ +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 message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + */ +public class TriggerKickMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ + public long sequence_id_; + + public TriggerKickMessage() + { + } + + public TriggerKickMessage(TriggerKickMessage other) + { + this(); + set(other); + } + + public void set(TriggerKickMessage other) + { + sequence_id_ = other.sequence_id_; + + } + + /** + * 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_; + } + + + public static Supplier getPubSubType() + { + return TriggerKickMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return TriggerKickMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(TriggerKickMessage 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; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof TriggerKickMessage)) return false; + + TriggerKickMessage otherMyClass = (TriggerKickMessage) other; + + if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("TriggerKickMessage {"); + builder.append("sequence_id="); + builder.append(this.sequence_id_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java new file mode 100644 index 000000000000..22e0ac3b1856 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java @@ -0,0 +1,143 @@ +package controller_msgs.msg.dds; + +/** +* +* Topic data type of the struct "TriggerKickMessage" defined in "TriggerKickMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from TriggerKickMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit TriggerKickMessage_.idl instead. +* +*/ +public class TriggerKickMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "controller_msgs::msg::dds_::TriggerKickMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "79a156c18de158aaca32e57b52e06d1d08f658115f0d4e04a0c09e454e942e15"; + } + + @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.TriggerKickMessage 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.TriggerKickMessage 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); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKickMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKickMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + + + return current_alignment - initial_alignment; + } + + public static void write(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_4(data.getSequenceId()); + + } + + public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + { + data.setSequenceId(cdr.read_type_4()); + + + } + + @Override + public final void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_4("sequence_id", data.getSequenceId()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.TriggerKickMessage data) + { + data.setSequenceId(ser.read_type_4("sequence_id")); } + + public static void staticCopy(controller_msgs.msg.dds.TriggerKickMessage src, controller_msgs.msg.dds.TriggerKickMessage dest) + { + dest.set(src); + } + + @Override + public controller_msgs.msg.dds.TriggerKickMessage createData() + { + return new controller_msgs.msg.dds.TriggerKickMessage(); + } + @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.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(controller_msgs.msg.dds.TriggerKickMessage src, controller_msgs.msg.dds.TriggerKickMessage dest) + { + staticCopy(src, dest); + } + + @Override + public TriggerKickMessagePubSubType newInstance() + { + return new TriggerKickMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg new file mode 100644 index 000000000000..d16744313f1a --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg @@ -0,0 +1,5 @@ +# This message is part of the IHMC whole-body controller API. +# This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg new file mode 100644 index 000000000000..4b1e3490ff11 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg @@ -0,0 +1,7 @@ +# This message is part of the IHMC whole-body controller API. +# This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + +# Unique ID used to identify this message, should preferably be consecutively increasing. +uint32 sequence_id + + From ed74294f2d5a3e48e9606cdbd65f1672ae47ce24 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Tue, 26 Mar 2024 15:53:07 -0500 Subject: [PATCH 02/32] Kick input parameters are now set through TriggerKickCommand. --- .../WalkingCommandConsumer.java | 7 +- .../command/TriggerKickCommand.java | 69 ++++++++- .../msg/TriggerKickMessage_.idl | 26 ++++ .../msg/dds/TriggerKickMessage.java | 140 +++++++++++++++++- .../msg/dds/TriggerKickMessagePubSubType.java | 60 +++++++- .../msg/TriggerKickMessage.msg | 20 ++- .../msg/TriggerKickMessage.msg | 19 +++ 7 files changed, 333 insertions(+), 8 deletions(-) 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 d375fd6a7569..fccb538a95e7 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 @@ -573,12 +573,13 @@ public void consumeAbortWalkingCommands(YoBoolean abortWalkingRequested) abortWalkingRequested.set(commandConsumerWithDelayBuffers.pollNewestCommand(AbortWalkingCommand.class).isAbortWalkingRequested()); } - public void consumeTriggerKickCommands(YoBoolean triggerKickRequested) + public TriggerKickCommand consumeTriggerKickCommands() { if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(TriggerKickCommand.class)) - return; + return null; - triggerKickRequested.set(commandConsumerWithDelayBuffers.pollNewestCommand(TriggerKickCommand.class).isTriggerKickRequested()); + TriggerKickCommand command = commandConsumerWithDelayBuffers.pollNewestCommand(TriggerKickCommand.class); + return command; } public void consumePrepareForLocomotionCommands() diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java index a50e5ea8a1fd..2f56f6b9cc22 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java @@ -2,17 +2,26 @@ import controller_msgs.msg.dds.TriggerKickMessage; import us.ihmc.communication.controllerAPI.command.Command; +import us.ihmc.robotics.robotSide.RobotSide; public class TriggerKickCommand implements Command { private long sequenceId; private boolean triggerKickRequested = false; + private RobotSide robotSide; + private double kickHeight; + private double kickImpulse; + private double kickTargetDistance; @Override public void clear() { sequenceId = 0; triggerKickRequested = false; + robotSide = null; + kickHeight = 0; + kickImpulse = 0; + kickTargetDistance = 0; } @Override @@ -20,6 +29,10 @@ public void set(TriggerKickCommand other) { sequenceId = other.sequenceId; triggerKickRequested = other.isTriggerKickRequested(); + robotSide = other.robotSide; + kickHeight = other.kickHeight; + kickImpulse = other.kickImpulse; + kickTargetDistance = other.kickTargetDistance; } @Override @@ -27,6 +40,35 @@ public void setFromMessage(TriggerKickMessage message) { sequenceId = message.getSequenceId(); triggerKickRequested = true; + robotSide = RobotSide.fromByte(message.getRobotSide()); + kickHeight = message.getKickHeight(); + kickImpulse = message.getKickImpulse(); + kickTargetDistance = message.getKickTargetDistance(); + } + + public RobotSide getRobotSide() + { + return robotSide; + } + + public void setRobotSide(RobotSide robotSide) + { + this.robotSide = robotSide; + } + + public double getKickHeight() + { + return kickHeight; + } + + public void setKickHeight(double kickHeight) + { + this.kickHeight = kickHeight; + } + + public double getKickImpulse() + { + return kickImpulse; } public boolean isTriggerKickRequested() @@ -34,6 +76,21 @@ public boolean isTriggerKickRequested() return triggerKickRequested; } + public void setKickImpulse(double kickImpulse) + { + this.kickImpulse = kickImpulse; + } + + public double getKickTargetDistance() + { + return kickTargetDistance; + } + + public void setKickTargetDistance(double kickTargetDistance) + { + this.kickTargetDistance = kickTargetDistance; + } + @Override public Class getMessageClass() { @@ -43,6 +100,16 @@ public Class getMessageClass() @Override public boolean isCommandValid() { + if (robotSide == null) + { + return false; + } + + if (kickHeight < 0 || kickImpulse < 0 || kickTargetDistance < 0) + { + return false; + } + return true; } @@ -51,4 +118,4 @@ public long getSequenceId() { return sequenceId; } -} +} \ No newline at end of file diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl index 10253493eeec..e863dd1f6440 100644 --- a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl @@ -7,6 +7,12 @@ module controller_msgs { module dds { + const octet ROBOT_SIDE_LEFT = + 0; + + const octet ROBOT_SIDE_RIGHT = + 1; + /** * This message is part of the IHMC whole-body controller API. @@ -19,6 +25,26 @@ module controller_msgs * Unique ID used to identify this message, should preferably be consecutively increasing. */ unsigned long sequence_id; + /** + * Specifies the side of the robot that will execute the kick. + */ + octet robot_side; + /** + * The height at which the kick should be targeted. + */ + double kick_height; + /** + * The impulse with which the kick should be executed. + */ + double kick_impulse; + /** + * The target distance from the robot to where the kick should be aimed. + */ + double kick_target_distance; + /** + * A boolean for tracking whether a kick has been requested. + */ + boolean trigger_kick_request; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java index 5dbca0b8fd16..dd557657d0ba 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java @@ -12,10 +12,32 @@ */ public class TriggerKickMessage extends Packet implements Settable, EpsilonComparable { + public static final byte ROBOT_SIDE_LEFT = (byte) 0; + public static final byte ROBOT_SIDE_RIGHT = (byte) 1; /** * Unique ID used to identify this message, should preferably be consecutively increasing. */ public long sequence_id_; + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte robot_side_; + /** + * The height at which the kick should be targeted. + */ + public double kick_height_; + /** + * The impulse with which the kick should be executed. + */ + public double kick_impulse_; + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double kick_target_distance_; + /** + * A boolean for tracking whether a kick has been requested. + */ + public boolean trigger_kick_request_; public TriggerKickMessage() { @@ -31,6 +53,16 @@ public void set(TriggerKickMessage other) { sequence_id_ = other.sequence_id_; + robot_side_ = other.robot_side_; + + kick_height_ = other.kick_height_; + + kick_impulse_ = other.kick_impulse_; + + kick_target_distance_ = other.kick_target_distance_; + + trigger_kick_request_ = other.trigger_kick_request_; + } /** @@ -48,6 +80,81 @@ public long getSequenceId() return sequence_id_; } + /** + * Specifies the side of the robot that will execute the kick. + */ + public void setRobotSide(byte robot_side) + { + robot_side_ = robot_side; + } + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte getRobotSide() + { + return robot_side_; + } + + /** + * The height at which the kick should be targeted. + */ + public void setKickHeight(double kick_height) + { + kick_height_ = kick_height; + } + /** + * The height at which the kick should be targeted. + */ + public double getKickHeight() + { + return kick_height_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickImpulse(double kick_impulse) + { + kick_impulse_ = kick_impulse; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickImpulse() + { + return kick_impulse_; + } + + /** + * The target distance from the robot to where the kick should be aimed. + */ + public void setKickTargetDistance(double kick_target_distance) + { + kick_target_distance_ = kick_target_distance; + } + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double getKickTargetDistance() + { + return kick_target_distance_; + } + + /** + * A boolean for tracking whether a kick has been requested. + */ + public void setTriggerKickRequest(boolean trigger_kick_request) + { + trigger_kick_request_ = trigger_kick_request; + } + /** + * A boolean for tracking whether a kick has been requested. + */ + public boolean getTriggerKickRequest() + { + return trigger_kick_request_; + } + public static Supplier getPubSubType() { @@ -68,6 +175,17 @@ public boolean epsilonEquals(TriggerKickMessage other, double epsilon) if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) return false; + + return true; } @@ -82,6 +200,16 @@ public boolean equals(Object other) if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + if(this.robot_side_ != otherMyClass.robot_side_) return false; + + if(this.kick_height_ != otherMyClass.kick_height_) return false; + + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + + if(this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) return false; + return true; } @@ -93,7 +221,17 @@ public java.lang.String toString() builder.append("TriggerKickMessage {"); builder.append("sequence_id="); - builder.append(this.sequence_id_); + builder.append(this.sequence_id_); builder.append(", "); + builder.append("robot_side="); + builder.append(this.robot_side_); builder.append(", "); + builder.append("kick_height="); + builder.append(this.kick_height_); builder.append(", "); + builder.append("kick_impulse="); + builder.append(this.kick_impulse_); builder.append(", "); + builder.append("kick_target_distance="); + builder.append(this.kick_target_distance_); builder.append(", "); + builder.append("trigger_kick_request="); + builder.append(this.trigger_kick_request_); builder.append("}"); return builder.toString(); } diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java index 22e0ac3b1856..a97a5dd24992 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java @@ -15,7 +15,7 @@ public class TriggerKickMessagePubSubType implements us.ihmc.pubsub.TopicDataTyp @Override public final java.lang.String getDefinitionChecksum() { - return "79a156c18de158aaca32e57b52e06d1d08f658115f0d4e04a0c09e454e942e15"; + return "e00cbb5d1447b4eb4b44f6bb2d83de46bdbe6a05041cfc0cec676000064cf64f"; } @Override @@ -54,6 +54,16 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + return current_alignment - initial_alignment; } @@ -70,6 +80,21 @@ public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKick current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + return current_alignment - initial_alignment; } @@ -78,12 +103,32 @@ public static void write(controller_msgs.msg.dds.TriggerKickMessage data, us.ihm { cdr.write_type_4(data.getSequenceId()); + cdr.write_type_9(data.getRobotSide()); + + cdr.write_type_6(data.getKickHeight()); + + cdr.write_type_6(data.getKickImpulse()); + + cdr.write_type_6(data.getKickTargetDistance()); + + cdr.write_type_7(data.getTriggerKickRequest()); + } public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) { data.setSequenceId(cdr.read_type_4()); + data.setRobotSide(cdr.read_type_9()); + + data.setKickHeight(cdr.read_type_6()); + + data.setKickImpulse(cdr.read_type_6()); + + data.setKickTargetDistance(cdr.read_type_6()); + + data.setTriggerKickRequest(cdr.read_type_7()); + } @@ -91,12 +136,23 @@ public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc public final void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.InterchangeSerializer ser) { ser.write_type_4("sequence_id", data.getSequenceId()); + ser.write_type_9("robot_side", data.getRobotSide()); + ser.write_type_6("kick_height", data.getKickHeight()); + ser.write_type_6("kick_impulse", data.getKickImpulse()); + ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); + ser.write_type_7("trigger_kick_request", data.getTriggerKickRequest()); } @Override public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.TriggerKickMessage data) { - data.setSequenceId(ser.read_type_4("sequence_id")); } + data.setSequenceId(ser.read_type_4("sequence_id")); + data.setRobotSide(ser.read_type_9("robot_side")); + data.setKickHeight(ser.read_type_6("kick_height")); + data.setKickImpulse(ser.read_type_6("kick_impulse")); + data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); + data.setTriggerKickRequest(ser.read_type_7("trigger_kick_request")); + } public static void staticCopy(controller_msgs.msg.dds.TriggerKickMessage src, controller_msgs.msg.dds.TriggerKickMessage dest) { diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg index d16744313f1a..4f4118e0dcc8 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg @@ -1,5 +1,23 @@ # This message is part of the IHMC whole-body controller API. # This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. +byte ROBOT_SIDE_LEFT = 0 +byte ROBOT_SIDE_RIGHT = 1 + # Unique ID used to identify this message, should preferably be consecutively increasing. -uint32 sequence_id \ No newline at end of file +uint32 sequence_id + +# Specifies the side of the robot that will execute the kick. +byte robot_side + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# A boolean for tracking whether a kick has been requested. +bool trigger_kick_request \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg index 4b1e3490ff11..44a8f429e64c 100644 --- a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg @@ -1,7 +1,26 @@ # This message is part of the IHMC whole-body controller API. # This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. +int8 ROBOT_SIDE_LEFT=0 + +int8 ROBOT_SIDE_RIGHT=1 + # Unique ID used to identify this message, should preferably be consecutively increasing. uint32 sequence_id +# Specifies the side of the robot that will execute the kick. +int8 robot_side + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# A boolean for tracking whether a kick has been requested. +bool trigger_kick_request + From 07b496ca34a8798f4665789a08e4f62b1107b767 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Tue, 26 Mar 2024 16:07:35 -0500 Subject: [PATCH 03/32] Removing unneeded change. --- .../highLevelStates/WalkingHighLevelHumanoidController.java | 1 - 1 file changed, 1 deletion(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java index d80f124322f3..af4406d45250 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/WalkingHighLevelHumanoidController.java @@ -641,7 +641,6 @@ public void doAction() commandConsumer.consumeStopAllTrajectoryCommands(); commandConsumer.consumeFootCommands(); commandConsumer.consumeAbortWalkingCommands(abortWalkingRequested); -// commandConsumer.consumeTriggerKickCommands(); commandConsumer.consumePelvisCommands(currentState, allowUpperBodyMotionDuringLocomotion.getBooleanValue()); commandConsumer.consumeManipulationCommands(currentState, allowUpperBodyMotionDuringLocomotion.getBooleanValue()); commandConsumer.handleAutomaticManipulationAbortOnICPError(currentState); From 2191111749fa1ef194afb900c8acc7b16d661e3d Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Mon, 1 Apr 2024 15:13:42 -0500 Subject: [PATCH 04/32] Kick message now has a weight distribution parameter. --- .../command/TriggerKickCommand.java | 21 +- .../msg/TriggerKickMessage_.idl | 4 + .../msg/dds/TriggerKickMessage.java | 179 ++++++++++++------ .../msg/dds/TriggerKickMessagePubSubType.java | 13 +- .../msg/TriggerKickMessage.msg | 5 +- .../msg/TriggerKickMessage.msg | 3 + 6 files changed, 161 insertions(+), 64 deletions(-) diff --git a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java index 2f56f6b9cc22..1a530c5a7aa2 100644 --- a/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/TriggerKickCommand.java @@ -12,6 +12,7 @@ public class TriggerKickCommand implements Command getMessageClass() { @@ -110,6 +124,11 @@ public boolean isCommandValid() return false; } + if (prekickWeightDistribution < 0 || prekickWeightDistribution > 1) + { + return false; + } + return true; } diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl index e863dd1f6440..144cf4c08ed8 100644 --- a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl @@ -45,6 +45,10 @@ module controller_msgs * A boolean for tracking whether a kick has been requested. */ boolean trigger_kick_request; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + double prekick_weight_distribution; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java index dd557657d0ba..da7fc1038d40 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java @@ -3,41 +3,47 @@ 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 message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. - */ + * This message is part of the IHMC whole-body controller API. + * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + */ public class TriggerKickMessage extends Packet implements Settable, EpsilonComparable { public static final byte ROBOT_SIDE_LEFT = (byte) 0; public static final byte ROBOT_SIDE_RIGHT = (byte) 1; /** - * Unique ID used to identify this message, should preferably be consecutively increasing. - */ + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ public long sequence_id_; /** - * Specifies the side of the robot that will execute the kick. - */ + * Specifies the side of the robot that will execute the kick. + */ public byte robot_side_; /** - * The height at which the kick should be targeted. - */ + * The height at which the kick should be targeted. + */ public double kick_height_; /** - * The impulse with which the kick should be executed. - */ + * The impulse with which the kick should be executed. + */ public double kick_impulse_; /** - * The target distance from the robot to where the kick should be aimed. - */ + * The target distance from the robot to where the kick should be aimed. + */ public double kick_target_distance_; /** - * A boolean for tracking whether a kick has been requested. - */ + * A boolean for tracking whether a kick has been requested. + */ public boolean trigger_kick_request_; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double prekick_weight_distribution_; public TriggerKickMessage() { @@ -63,98 +69,120 @@ public void set(TriggerKickMessage other) trigger_kick_request_ = other.trigger_kick_request_; + prekick_weight_distribution_ = other.prekick_weight_distribution_; } /** - * Unique ID used to identify this message, should preferably be consecutively increasing. - */ + * 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. - */ + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ public long getSequenceId() { return sequence_id_; } /** - * Specifies the side of the robot that will execute the kick. - */ + * Specifies the side of the robot that will execute the kick. + */ public void setRobotSide(byte robot_side) { robot_side_ = robot_side; } + /** - * Specifies the side of the robot that will execute the kick. - */ + * Specifies the side of the robot that will execute the kick. + */ public byte getRobotSide() { return robot_side_; } /** - * The height at which the kick should be targeted. - */ + * The height at which the kick should be targeted. + */ public void setKickHeight(double kick_height) { kick_height_ = kick_height; } + /** - * The height at which the kick should be targeted. - */ + * The height at which the kick should be targeted. + */ public double getKickHeight() { return kick_height_; } /** - * The impulse with which the kick should be executed. - */ + * The impulse with which the kick should be executed. + */ public void setKickImpulse(double kick_impulse) { kick_impulse_ = kick_impulse; } + /** - * The impulse with which the kick should be executed. - */ + * The impulse with which the kick should be executed. + */ public double getKickImpulse() { return kick_impulse_; } /** - * The target distance from the robot to where the kick should be aimed. - */ + * The target distance from the robot to where the kick should be aimed. + */ public void setKickTargetDistance(double kick_target_distance) { kick_target_distance_ = kick_target_distance; } + /** - * The target distance from the robot to where the kick should be aimed. - */ + * The target distance from the robot to where the kick should be aimed. + */ public double getKickTargetDistance() { return kick_target_distance_; } /** - * A boolean for tracking whether a kick has been requested. - */ + * A boolean for tracking whether a kick has been requested. + */ public void setTriggerKickRequest(boolean trigger_kick_request) { trigger_kick_request_ = trigger_kick_request; } + /** - * A boolean for tracking whether a kick has been requested. - */ + * A boolean for tracking whether a kick has been requested. + */ public boolean getTriggerKickRequest() { return trigger_kick_request_; } + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public void setPrekickWeightDistribution(double prekick_weight_distribution) + { + prekick_weight_distribution_ = prekick_weight_distribution; + } + + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double getPrekickWeightDistribution() + { + return prekick_weight_distribution_; + } public static Supplier getPubSubType() { @@ -170,21 +198,31 @@ public Supplier getPubSubTypePacket() @Override public boolean epsilonEquals(TriggerKickMessage other, double epsilon) { - if(other == null) return false; - if(other == this) return true; + 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.sequence_id_, other.sequence_id_, epsilon)) + return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) + return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) + return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) + return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) + return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) + return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) + return false; return true; } @@ -192,24 +230,35 @@ public boolean epsilonEquals(TriggerKickMessage other, double epsilon) @Override public boolean equals(Object other) { - if(other == null) return false; - if(other == this) return true; - if(!(other instanceof TriggerKickMessage)) return false; + if (other == null) + return false; + if (other == this) + return true; + if (!(other instanceof TriggerKickMessage)) + return false; TriggerKickMessage otherMyClass = (TriggerKickMessage) other; - if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + if (this.sequence_id_ != otherMyClass.sequence_id_) + return false; - if(this.robot_side_ != otherMyClass.robot_side_) return false; + if (this.robot_side_ != otherMyClass.robot_side_) + return false; - if(this.kick_height_ != otherMyClass.kick_height_) return false; + if (this.kick_height_ != otherMyClass.kick_height_) + return false; - if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + if (this.kick_impulse_ != otherMyClass.kick_impulse_) + return false; - if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + if (this.kick_target_distance_ != otherMyClass.kick_target_distance_) + return false; - if(this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) return false; + if (this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) + return false; + if (this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) + return false; return true; } @@ -221,17 +270,25 @@ public java.lang.String toString() builder.append("TriggerKickMessage {"); builder.append("sequence_id="); - builder.append(this.sequence_id_); builder.append(", "); + builder.append(this.sequence_id_); + builder.append(", "); builder.append("robot_side="); - builder.append(this.robot_side_); builder.append(", "); + builder.append(this.robot_side_); + builder.append(", "); builder.append("kick_height="); - builder.append(this.kick_height_); builder.append(", "); + builder.append(this.kick_height_); + builder.append(", "); builder.append("kick_impulse="); - builder.append(this.kick_impulse_); builder.append(", "); + builder.append(this.kick_impulse_); + builder.append(", "); builder.append("kick_target_distance="); - builder.append(this.kick_target_distance_); builder.append(", "); + builder.append(this.kick_target_distance_); + builder.append(", "); builder.append("trigger_kick_request="); builder.append(this.trigger_kick_request_); + builder.append(", "); + builder.append("prekick_weight_distribution="); + builder.append(this.prekick_weight_distribution_); builder.append("}"); return builder.toString(); } diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java index a97a5dd24992..c718daf9b0dd 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java @@ -15,7 +15,7 @@ public class TriggerKickMessagePubSubType implements us.ihmc.pubsub.TopicDataTyp @Override public final java.lang.String getDefinitionChecksum() { - return "e00cbb5d1447b4eb4b44f6bb2d83de46bdbe6a05041cfc0cec676000064cf64f"; + return "60d1adda16bc13fae6bca562cfca24a13f70658c3d5a25cb2f68dd8ba55645d2"; } @Override @@ -64,6 +64,8 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + return current_alignment - initial_alignment; } @@ -95,6 +97,9 @@ public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKick current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + return current_alignment - initial_alignment; } @@ -113,6 +118,8 @@ public static void write(controller_msgs.msg.dds.TriggerKickMessage data, us.ihm cdr.write_type_7(data.getTriggerKickRequest()); + cdr.write_type_6(data.getPrekickWeightDistribution()); + } public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) @@ -129,6 +136,8 @@ public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc data.setTriggerKickRequest(cdr.read_type_7()); + data.setPrekickWeightDistribution(cdr.read_type_6()); + } @@ -141,6 +150,7 @@ public final void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us. ser.write_type_6("kick_impulse", data.getKickImpulse()); ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); ser.write_type_7("trigger_kick_request", data.getTriggerKickRequest()); + ser.write_type_6("prekick_weight_distribution", data.getPrekickWeightDistribution()); } @Override @@ -152,6 +162,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_ data.setKickImpulse(ser.read_type_6("kick_impulse")); data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); data.setTriggerKickRequest(ser.read_type_7("trigger_kick_request")); + data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); } public static void staticCopy(controller_msgs.msg.dds.TriggerKickMessage src, controller_msgs.msg.dds.TriggerKickMessage dest) diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg index 4f4118e0dcc8..6fcb1e5434c3 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg @@ -20,4 +20,7 @@ float64 kick_impulse float64 kick_target_distance # A boolean for tracking whether a kick has been requested. -bool trigger_kick_request \ No newline at end of file +bool trigger_kick_request + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg index 44a8f429e64c..39c71594e1c3 100644 --- a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg @@ -23,4 +23,7 @@ float64 kick_target_distance # A boolean for tracking whether a kick has been requested. bool trigger_kick_request +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution + From a3a8758d780e9498d945dae3a48db49dc962cb3c Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Thu, 4 Apr 2024 16:14:23 -0500 Subject: [PATCH 05/32] Kick door behavior boiler plate. --- .../msg/KickDoorActionDefinitionMessage_.idl | 49 ++++ .../msg/KickDoorActionStateMessage_.idl | 29 ++ .../dds/KickDoorActionDefinitionMessage.java | 251 ++++++++++++++++++ ...DoorActionDefinitionMessagePubSubType.java | 209 +++++++++++++++ .../msg/dds/KickDoorActionStateMessage.java | 108 ++++++++ .../KickDoorActionStateMessagePubSubType.java | 153 +++++++++++ .../msg/KickDoorActionDefinitionMessage.msg | 20 ++ .../msg/KickDoorActionStateMessage.msg | 5 + .../msg/KickDoorActionDefinitionMessage.msg | 24 ++ .../msg/KickDoorActionStateMessage.msg | 8 + 10 files changed, 856 insertions(+) create mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl new file mode 100644 index 000000000000..8e1ca896db4f --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl @@ -0,0 +1,49 @@ +#ifndef __behavior_msgs__msg__KickDoorActionDefinitionMessage__idl__ +#define __behavior_msgs__msg__KickDoorActionDefinitionMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeDefinitionMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorActionDefinitionMessage_") + struct KickDoorActionDefinitionMessage + { + /** + * Parent definition fields + */ + behavior_msgs::msg::dds::ActionNodeDefinitionMessage definition; + /** + * Specifies the side of the robot that will execute the kick. + */ + @defaultValue(value=255) + octet robot_side; + /** + * The height at which the kick should be targeted. + */ + double kick_height; + /** + * The impulse with which the kick should be executed. + */ + double kick_impulse; + /** + * The target distance from the robot to where the kick should be aimed. + */ + double kick_target_distance; + /** + * A boolean for tracking whether a kick has been requested. + */ + boolean trigger_kick_request; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + double prekick_weight_distribution; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl new file mode 100644 index 000000000000..76da01cdafa5 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl @@ -0,0 +1,29 @@ +#ifndef __behavior_msgs__msg__KickDoorActionStateMessage__idl__ +#define __behavior_msgs__msg__KickDoorActionStateMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorActionDefinitionMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorActionStateMessage_") + struct KickDoorActionStateMessage + { + /** + * Parent state fields + */ + behavior_msgs::msg::dds::ActionNodeStateMessage state; + /** + * Definition + */ + behavior_msgs::msg::dds::KickDoorActionDefinitionMessage definition; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java new file mode 100644 index 000000000000..c74456c0f46b --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java @@ -0,0 +1,251 @@ +package behavior_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; + +public class KickDoorActionDefinitionMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage definition_; + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte robot_side_ = (byte) 255; + /** + * The height at which the kick should be targeted. + */ + public double kick_height_; + /** + * The impulse with which the kick should be executed. + */ + public double kick_impulse_; + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double kick_target_distance_; + /** + * A boolean for tracking whether a kick has been requested. + */ + public boolean trigger_kick_request_; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double prekick_weight_distribution_; + + public KickDoorActionDefinitionMessage() + { + definition_ = new behavior_msgs.msg.dds.ActionNodeDefinitionMessage(); + } + + public KickDoorActionDefinitionMessage(KickDoorActionDefinitionMessage other) + { + this(); + set(other); + } + + public void set(KickDoorActionDefinitionMessage other) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + robot_side_ = other.robot_side_; + + kick_height_ = other.kick_height_; + + kick_impulse_ = other.kick_impulse_; + + kick_target_distance_ = other.kick_target_distance_; + + trigger_kick_request_ = other.trigger_kick_request_; + + prekick_weight_distribution_ = other.prekick_weight_distribution_; + + } + + + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage getDefinition() + { + return definition_; + } + + /** + * Specifies the side of the robot that will execute the kick. + */ + public void setRobotSide(byte robot_side) + { + robot_side_ = robot_side; + } + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte getRobotSide() + { + return robot_side_; + } + + /** + * The height at which the kick should be targeted. + */ + public void setKickHeight(double kick_height) + { + kick_height_ = kick_height; + } + /** + * The height at which the kick should be targeted. + */ + public double getKickHeight() + { + return kick_height_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickImpulse(double kick_impulse) + { + kick_impulse_ = kick_impulse; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickImpulse() + { + return kick_impulse_; + } + + /** + * The target distance from the robot to where the kick should be aimed. + */ + public void setKickTargetDistance(double kick_target_distance) + { + kick_target_distance_ = kick_target_distance; + } + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double getKickTargetDistance() + { + return kick_target_distance_; + } + + /** + * A boolean for tracking whether a kick has been requested. + */ + public void setTriggerKickRequest(boolean trigger_kick_request) + { + trigger_kick_request_ = trigger_kick_request; + } + /** + * A boolean for tracking whether a kick has been requested. + */ + public boolean getTriggerKickRequest() + { + return trigger_kick_request_; + } + + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public void setPrekickWeightDistribution(double prekick_weight_distribution) + { + prekick_weight_distribution_ = prekick_weight_distribution; + } + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double getPrekickWeightDistribution() + { + return prekick_weight_distribution_; + } + + + public static Supplier getPubSubType() + { + return KickDoorActionDefinitionMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorActionDefinitionMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorActionDefinitionMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorActionDefinitionMessage)) return false; + + KickDoorActionDefinitionMessage otherMyClass = (KickDoorActionDefinitionMessage) other; + + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if(this.robot_side_ != otherMyClass.robot_side_) return false; + + if(this.kick_height_ != otherMyClass.kick_height_) return false; + + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + + if(this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) return false; + + if(this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorActionDefinitionMessage {"); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("robot_side="); + builder.append(this.robot_side_); builder.append(", "); + builder.append("kick_height="); + builder.append(this.kick_height_); builder.append(", "); + builder.append("kick_impulse="); + builder.append(this.kick_impulse_); builder.append(", "); + builder.append("kick_target_distance="); + builder.append(this.kick_target_distance_); builder.append(", "); + builder.append("trigger_kick_request="); + builder.append(this.trigger_kick_request_); builder.append(", "); + builder.append("prekick_weight_distribution="); + builder.append(this.prekick_weight_distribution_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java new file mode 100644 index 000000000000..b9828f7e9ab0 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessagePubSubType.java @@ -0,0 +1,209 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorActionDefinitionMessage" defined in "KickDoorActionDefinitionMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorActionDefinitionMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorActionDefinitionMessage_.idl instead. +* +*/ +public class KickDoorActionDefinitionMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorActionDefinitionMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "7ee1f0336da083ef687b9e6f772acd4eeaa4562c689ce863cf81d3445a4c99f1"; + } + + @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(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage 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, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage 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 += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + cdr.write_type_9(data.getRobotSide()); + + cdr.write_type_6(data.getKickHeight()); + + cdr.write_type_6(data.getKickImpulse()); + + cdr.write_type_6(data.getKickTargetDistance()); + + cdr.write_type_7(data.getTriggerKickRequest()); + + cdr.write_type_6(data.getPrekickWeightDistribution()); + + } + + public static void read(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + data.setRobotSide(cdr.read_type_9()); + + data.setKickHeight(cdr.read_type_6()); + + data.setKickImpulse(cdr.read_type_6()); + + data.setKickTargetDistance(cdr.read_type_6()); + + data.setTriggerKickRequest(cdr.read_type_7()); + + data.setPrekickWeightDistribution(cdr.read_type_6()); + + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_9("robot_side", data.getRobotSide()); + ser.write_type_6("kick_height", data.getKickHeight()); + ser.write_type_6("kick_impulse", data.getKickImpulse()); + ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); + ser.write_type_7("trigger_kick_request", data.getTriggerKickRequest()); + ser.write_type_6("prekick_weight_distribution", data.getPrekickWeightDistribution()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data) + { + ser.read_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + data.setRobotSide(ser.read_type_9("robot_side")); + data.setKickHeight(ser.read_type_6("kick_height")); + data.setKickImpulse(ser.read_type_6("kick_impulse")); + data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); + data.setTriggerKickRequest(ser.read_type_7("trigger_kick_request")); + data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage src, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorActionDefinitionMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorActionDefinitionMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorActionDefinitionMessage src, behavior_msgs.msg.dds.KickDoorActionDefinitionMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorActionDefinitionMessagePubSubType newInstance() + { + return new KickDoorActionDefinitionMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java new file mode 100644 index 000000000000..118bd839fd02 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java @@ -0,0 +1,108 @@ +package behavior_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; + +public class KickDoorActionStateMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage state_; + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorActionDefinitionMessage definition_; + + public KickDoorActionStateMessage() + { + state_ = new behavior_msgs.msg.dds.ActionNodeStateMessage(); + definition_ = new behavior_msgs.msg.dds.KickDoorActionDefinitionMessage(); + } + + public KickDoorActionStateMessage(KickDoorActionStateMessage other) + { + this(); + set(other); + } + + public void set(KickDoorActionStateMessage other) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.staticCopy(other.state_, state_); + behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + } + + + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage getState() + { + return state_; + } + + + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorActionDefinitionMessage getDefinition() + { + return definition_; + } + + + public static Supplier getPubSubType() + { + return KickDoorActionStateMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorActionStateMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorActionStateMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorActionStateMessage)) return false; + + KickDoorActionStateMessage otherMyClass = (KickDoorActionStateMessage) other; + + if (!this.state_.equals(otherMyClass.state_)) return false; + if (!this.definition_.equals(otherMyClass.definition_)) return false; + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorActionStateMessage {"); + builder.append("state="); + builder.append(this.state_); builder.append(", "); + builder.append("definition="); + builder.append(this.definition_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java new file mode 100644 index 000000000000..1efdbe01e20c --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java @@ -0,0 +1,153 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorActionStateMessage" defined in "KickDoorActionStateMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorActionStateMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorActionStateMessage_.idl instead. +* +*/ +public class KickDoorActionStateMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorActionStateMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "5ac8d9a3a2c4ac6fd14833c1e5c12b6a9a564891534656907b8ff2e37e88b287"; + } + + @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(behavior_msgs.msg.dds.KickDoorActionStateMessage 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, behavior_msgs.msg.dds.KickDoorActionStateMessage 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 += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionStateMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getCdrSerializedSize(data.getState(), current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + } + + public static void read(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType(), data.getDefinition()); + + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorActionStateMessage data) + { + ser.read_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType(), data.getDefinition()); + + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorActionStateMessage src, behavior_msgs.msg.dds.KickDoorActionStateMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorActionStateMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorActionStateMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorActionStateMessage src, behavior_msgs.msg.dds.KickDoorActionStateMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorActionStateMessagePubSubType newInstance() + { + return new KickDoorActionStateMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg new file mode 100644 index 000000000000..5b92647d1942 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg @@ -0,0 +1,20 @@ +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Specifies the side of the robot that will execute the kick. +byte robot_side 255 + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# A boolean for tracking whether a kick has been requested. +bool trigger_kick_request + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg new file mode 100644 index 000000000000..f32b13be4cb2 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg @@ -0,0 +1,5 @@ +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorActionDefinitionMessage definition diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg new file mode 100644 index 000000000000..7b3d4528a1af --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionDefinitionMessage.msg @@ -0,0 +1,24 @@ + +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Specifies the side of the robot that will execute the kick. +# Field default value 255 +int8 robot_side + +# The height at which the kick should be targeted. +float64 kick_height + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# A boolean for tracking whether a kick has been requested. +bool trigger_kick_request + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution + + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg new file mode 100644 index 000000000000..1fef75ee98bb --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg @@ -0,0 +1,8 @@ + +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorActionDefinitionMessage definition + + From a1b306feaef49210b9a8b17383eab0c2124efb5b Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Thu, 4 Apr 2024 16:35:28 -0500 Subject: [PATCH 06/32] Behavior tree state message boiler plate. --- .../msg/BehaviorTreeStateMessage_.idl | 5 ++++ .../msg/KickDoorActionDefinitionMessage_.idl | 4 --- .../msg/dds/BehaviorTreeStateMessage.java | 22 ++++++++++++++- .../BehaviorTreeStateMessagePubSubType.java | 17 +++++++++++- .../dds/KickDoorActionDefinitionMessage.java | 27 ------------------- ...DoorActionDefinitionMessagePubSubType.java | 13 +-------- .../KickDoorActionStateMessagePubSubType.java | 2 +- .../msg/BehaviorTreeStateMessage.msg | 3 +++ .../msg/KickDoorActionDefinitionMessage.msg | 3 --- .../msg/BehaviorTreeStateMessage.msg | 4 +++ .../msg/KickDoorActionDefinitionMessage.msg | 3 --- 11 files changed, 51 insertions(+), 52 deletions(-) diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl index bbb1836b72dd..aed5593bcd36 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl @@ -8,6 +8,7 @@ #include "behavior_msgs/msg/./FootstepPlanActionStateMessage_.idl" #include "behavior_msgs/msg/./HandPoseActionStateMessage_.idl" #include "behavior_msgs/msg/./HandWrenchActionStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorActionStateMessage_.idl" #include "behavior_msgs/msg/./PelvisHeightPitchActionStateMessage_.idl" #include "behavior_msgs/msg/./SakeHandCommandActionStateMessage_.idl" #include "behavior_msgs/msg/./ScrewPrimitiveActionStateMessage_.idl" @@ -52,6 +53,9 @@ module behavior_msgs const octet WAIT_DURATION_ACTION = 17; + const octet KICK_DOOR_ACTION = + 18; + /** * Gives the current state of the complete collection of behavior tree nodes. @@ -95,6 +99,7 @@ module behavior_msgs sequence screw_primitive_actions; sequence pelvis_height_actions; sequence wait_duration_actions; + sequence kick_door_actions; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl index 8e1ca896db4f..88796fc249f7 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl @@ -33,10 +33,6 @@ module behavior_msgs * The target distance from the robot to where the kick should be aimed. */ double kick_target_distance; - /** - * A boolean for tracking whether a kick has been requested. - */ - boolean trigger_kick_request; /** * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. */ diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java index d4d493252be5..6489033cc036 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java @@ -25,6 +25,7 @@ public class BehaviorTreeStateMessage extends Packet i public static final byte SCREW_PRIMITIVE_ACTION = (byte) 15; public static final byte PELVIS_HEIGHT_PITCH_ACTION = (byte) 16; public static final byte WAIT_DURATION_ACTION = (byte) 17; + public static final byte KICK_DOOR_ACTION = (byte) 18; /** * Monotonically increasing message ID that matches the CRDTInfo update number */ @@ -58,6 +59,7 @@ public class BehaviorTreeStateMessage extends Packet i public us.ihmc.idl.IDLSequence.Object screw_primitive_actions_; public us.ihmc.idl.IDLSequence.Object pelvis_height_actions_; public us.ihmc.idl.IDLSequence.Object wait_duration_actions_; + public us.ihmc.idl.IDLSequence.Object kick_door_actions_; public BehaviorTreeStateMessage() { @@ -77,6 +79,7 @@ public BehaviorTreeStateMessage() screw_primitive_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.ScrewPrimitiveActionStateMessagePubSubType()); pelvis_height_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.PelvisHeightPitchActionStateMessagePubSubType()); wait_duration_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType()); + kick_door_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType()); } @@ -106,6 +109,7 @@ public void set(BehaviorTreeStateMessage other) screw_primitive_actions_.set(other.screw_primitive_actions_); pelvis_height_actions_.set(other.pelvis_height_actions_); wait_duration_actions_.set(other.wait_duration_actions_); + kick_door_actions_.set(other.kick_door_actions_); } /** @@ -234,6 +238,12 @@ public us.ihmc.idl.IDLSequence.Object getKickDoorActions() + { + return kick_door_actions_; + } + + public static Supplier getPubSubType() { return BehaviorTreeStateMessagePubSubType::new; @@ -337,6 +347,13 @@ public boolean epsilonEquals(BehaviorTreeStateMessage other, double epsilon) { if (!this.wait_duration_actions_.get(i).epsilonEquals(other.wait_duration_actions_.get(i), epsilon)) return false; } } + if (this.kick_door_actions_.size() != other.kick_door_actions_.size()) { return false; } + else + { + for (int i = 0; i < this.kick_door_actions_.size(); i++) + { if (!this.kick_door_actions_.get(i).epsilonEquals(other.kick_door_actions_.get(i), epsilon)) return false; } + } + return true; } @@ -368,6 +385,7 @@ public boolean equals(Object other) if (!this.screw_primitive_actions_.equals(otherMyClass.screw_primitive_actions_)) return false; if (!this.pelvis_height_actions_.equals(otherMyClass.pelvis_height_actions_)) return false; if (!this.wait_duration_actions_.equals(otherMyClass.wait_duration_actions_)) return false; + if (!this.kick_door_actions_.equals(otherMyClass.kick_door_actions_)) return false; return true; } @@ -409,7 +427,9 @@ public java.lang.String toString() builder.append("pelvis_height_actions="); builder.append(this.pelvis_height_actions_); builder.append(", "); builder.append("wait_duration_actions="); - builder.append(this.wait_duration_actions_); + builder.append(this.wait_duration_actions_); builder.append(", "); + builder.append("kick_door_actions="); + builder.append(this.kick_door_actions_); builder.append("}"); return builder.toString(); } 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 d6055eac843e..4e9cc5485086 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 "37891ce73268750f598d784a233d6dd991adb94d0e022a75190f7be3d3e8336c"; + return "e66183e283438d181acd6be6c8ef5752ee99182d00b9819c1bbf4776b6024a33"; } @Override @@ -95,6 +95,9 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) { current_alignment += behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} return current_alignment - initial_alignment; } @@ -179,6 +182,11 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeS { current_alignment += behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType.getCdrSerializedSize(data.getWaitDurationActions().get(i0), current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getKickDoorActions().size(); ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getCdrSerializedSize(data.getKickDoorActions().get(i0), current_alignment);} + return current_alignment - initial_alignment; } @@ -242,6 +250,10 @@ public static void write(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us cdr.write_type_e(data.getWaitDurationActions());else throw new RuntimeException("wait_duration_actions field exceeds the maximum length"); + if(data.getKickDoorActions().size() <= 200) + cdr.write_type_e(data.getKickDoorActions());else + throw new RuntimeException("kick_door_actions field exceeds the maximum length"); + } public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us.ihmc.idl.CDR cdr) @@ -264,6 +276,7 @@ public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us. cdr.read_type_e(data.getScrewPrimitiveActions()); cdr.read_type_e(data.getPelvisHeightActions()); cdr.read_type_e(data.getWaitDurationActions()); + cdr.read_type_e(data.getKickDoorActions()); } @@ -287,6 +300,7 @@ public final void serialize(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, ser.write_type_e("screw_primitive_actions", data.getScrewPrimitiveActions()); ser.write_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.write_type_e("wait_duration_actions", data.getWaitDurationActions()); + ser.write_type_e("kick_door_actions", data.getKickDoorActions()); } @Override @@ -309,6 +323,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_e("screw_primitive_actions", data.getScrewPrimitiveActions()); ser.read_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.read_type_e("wait_duration_actions", data.getWaitDurationActions()); + ser.read_type_e("kick_door_actions", data.getKickDoorActions()); } public static void staticCopy(behavior_msgs.msg.dds.BehaviorTreeStateMessage src, behavior_msgs.msg.dds.BehaviorTreeStateMessage dest) diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java index c74456c0f46b..3fb338cd8d3d 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java @@ -28,10 +28,6 @@ public class KickDoorActionDefinitionMessage extends Packet Date: Fri, 5 Apr 2024 14:52:10 -0500 Subject: [PATCH 07/32] All (hopefully) of the behavior tree boilerplate for kicking is in place. --- .../behavior/actions/RDXKickDoorAction.java | 94 +++++++++ .../tree/RDXBehaviorTreeNodeBuilder.java | 11 ++ .../tree/RDXBehaviorTreeNodeCreationMenu.java | 1 + .../BehaviorTreeDefinitionRegistry.java | 1 + .../BehaviorTreeExecutorNodeBuilder.java | 4 + .../ros2/ROS2BehaviorTreeMessageTools.java | 7 + .../ROS2BehaviorTreeSubscriptionNode.java | 12 ++ .../actions/KickDoorActionDefinition.java | 179 ++++++++++++++++++ .../actions/KickDoorActionExecutionState.java | 23 +++ .../actions/KickDoorActionExecutor.java | 119 ++++++++++++ .../sequence/actions/KickDoorActionState.java | 52 +++++ .../msg/KickDoorActionStateMessage_.idl | 4 + .../BehaviorTreeStateMessagePubSubType.java | 2 +- .../msg/dds/KickDoorActionStateMessage.java | 29 ++- .../KickDoorActionStateMessagePubSubType.java | 13 +- .../msg/KickDoorActionStateMessage.msg | 3 + .../msg/KickDoorActionStateMessage.msg | 3 + 17 files changed, 554 insertions(+), 3 deletions(-) create mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java new file mode 100644 index 000000000000..e97813fe8808 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java @@ -0,0 +1,94 @@ +package us.ihmc.rdx.ui.behavior.actions; + +import imgui.ImGui; +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.behaviors.sequence.actions.KickDoorActionDefinition; +import us.ihmc.behaviors.sequence.actions.KickDoorActionState; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; +import us.ihmc.rdx.imgui.ImDoubleWrapper; +import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.rdx.imgui.ImStringWrapper; +import us.ihmc.rdx.ui.RDXBaseUI; +import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class RDXKickDoorAction extends RDXActionNode +{ + private final DRCRobotModel robotModel; + private final ROS2SyncedRobotModel syncedRobot; + private final KickDoorActionState state; + private final KickDoorActionDefinition definition; + private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final ImDoubleWrapper kickHeight; + private final ImDoubleWrapper kickImpulse; + private final ImDoubleWrapper kickDistance; + private final ImDoubleWrapper preKickWeightDistribution; + + public RDXKickDoorAction(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + RDXBaseUI baseUI, + DRCRobotModel robotModel, + ROS2SyncedRobotModel syncedRobot, + ReferenceFrameLibrary referenceFrameLibrary, + FootstepPlannerParametersBasics footstepPlannerParameters) + { + super(new KickDoorActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + this.robotModel = robotModel; + this.syncedRobot = syncedRobot; + + definition.setName("Kick Door"); + + //TODO: add robot side to the gui + kickHeight = new ImDoubleWrapper(definition::getKickHeight, + definition::setKickHeight, + imDouble -> ImGui.inputDouble(labels.get("Kick height"), imDouble)); + kickImpulse = new ImDoubleWrapper(definition::getKickImpulse, + definition::setKickImpulse, + imDouble -> ImGui.inputDouble(labels.get("Kick impulse"), imDouble)); + kickDistance = new ImDoubleWrapper(definition::getKickTargetDistance, + definition::setKickTargetDistance, + imDouble -> ImGui.inputDouble(labels.get("Kick distance"), imDouble)); + preKickWeightDistribution = new ImDoubleWrapper(definition::getPrekickWeightDistribution, + definition::setPrekickWeightDistribution, + imDouble -> ImGui.inputDouble(labels.get("Weight distribution"), imDouble)); + } + + @Override + public void update() + { + super.update(); + } + + @Override + public void renderTreeViewIconArea() + { + super.renderTreeViewIconArea(); + + ImGui.sameLine(); + } + + @Override + protected void renderImGuiWidgetsInternal() + { + ImGui.pushItemWidth(80.0f); + kickHeight.renderImGuiWidget(); + kickImpulse.renderImGuiWidget(); + kickDistance.renderImGuiWidget(); + preKickWeightDistribution.renderImGuiWidget(); + ImGui.popItemWidth(); + } + + @Override + public String getActionTypeTitle() + { + return "Kick Door"; + } +} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java index 15221fef4c02..559906411055 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java @@ -128,6 +128,17 @@ public RDXBehaviorTreeNodeBuilder(DRCRobotModel robotModel, { return new RDXWaitDurationAction(id, crdtInfo, saveFileDirectory); } + if (nodeType == KickDoorActionDefinition.class) + { + return new RDXKickDoorAction(id, + crdtInfo, + saveFileDirectory, + baseUI, + robotModel, + syncedRobot, + referenceFrameLibrary, + footstepPlannerParametersBasics); + } else { return null; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java index dbb7337d8239..4aa8a1457b3d 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java @@ -174,6 +174,7 @@ public void renderImGuiWidgets(RDXBehaviorTreeNode relativeNode, BehaviorT renderNodeCreationClickable(relativeNode, insertionType, "Chest Orientation", ChestOrientationActionDefinition.class, null); renderNodeCreationClickable(relativeNode, insertionType, "Pelvis Height", PelvisHeightPitchActionDefinition.class, null); renderNodeCreationClickable(relativeNode, insertionType, "Wait", WaitDurationActionDefinition.class, null); + renderNodeCreationClickable(relativeNode, insertionType, "Kick Door", KickDoorActionDefinition.class, null); ImGui.text("Screw Primitive: "); for (RobotSide side : RobotSide.values) { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java index 42e6b74b93e8..e4b7b8e3c3a3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java @@ -23,6 +23,7 @@ private record RegistryRecord(Class typeClass, byte messageByte) { } new RegistryRecord(PelvisHeightPitchActionDefinition.class, BehaviorTreeStateMessage.PELVIS_HEIGHT_PITCH_ACTION), new RegistryRecord(SakeHandCommandActionDefinition.class, BehaviorTreeStateMessage.SAKE_HAND_COMMAND_ACTION), new RegistryRecord(WaitDurationActionDefinition.class, BehaviorTreeStateMessage.WAIT_DURATION_ACTION), + new RegistryRecord(KickDoorActionDefinition.class, BehaviorTreeStateMessage.KICK_DOOR_ACTION) }; public static Class getClassFromTypeName(String typeName) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java index 770431630c6d..622611866349 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java @@ -104,6 +104,10 @@ public BehaviorTreeExecutorNodeBuilder(DRCRobotModel robotModel, { return new WaitDurationActionExecutor(id, crdtInfo, saveFileDirectory, syncedRobot); } + if (nodeType == KickDoorActionDefinition.class) + { + return new KickDoorActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, syncedRobot, controllerStatusTracker, referenceFrameLibrary, walkingControllerParameters); + } return null; } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index 06e51adbfde2..397244a3e530 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -28,6 +28,7 @@ public static void clearLists(BehaviorTreeStateMessage treeStateMessage) treeStateMessage.getPelvisHeightActions().clear(); treeStateMessage.getSakeHandCommandActions().clear(); treeStateMessage.getWaitDurationActions().clear(); + treeStateMessage.getKickDoorActions().clear(); } public static void packMessage(BehaviorTreeNodeState nodeState, BehaviorTreeStateMessage treeStateMessage) @@ -92,6 +93,12 @@ else if (nodeState instanceof WaitDurationActionState waitDurationActionState) treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getWaitDurationActions().size()); waitDurationActionState.toMessage(treeStateMessage.getWaitDurationActions().add()); } + else if (nodeState instanceof KickDoorActionState kickDoorActionState) + { + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_ACTION); + treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); + kickDoorActionState.toMessage(treeStateMessage.getKickDoorActions().add()); + } else { treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.BASIC_NODE); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java index 4a0281b5fcb2..0c2ab795ccfb 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java @@ -21,6 +21,7 @@ public class ROS2BehaviorTreeSubscriptionNode private ScrewPrimitiveActionStateMessage screwPrimitiveActionStateMessage; private PelvisHeightPitchActionStateMessage pelvisHeightPitchActionStateMessage; private WaitDurationActionStateMessage waitDurationActionStateMessage; + private KickDoorActionStateMessage kickDoorActionStateMessage; private final List children = new ArrayList<>(); public void clear() @@ -39,6 +40,7 @@ public void clear() screwPrimitiveActionStateMessage = null; pelvisHeightPitchActionStateMessage = null; waitDurationActionStateMessage = null; + kickDoorActionStateMessage = null; children.clear(); } @@ -182,6 +184,16 @@ public void setWaitDurationActionStateMessage(WaitDurationActionStateMessage wai this.waitDurationActionStateMessage = waitDurationActionStateMessage; } + public KickDoorActionStateMessage getKickDoorActionStateMessage() + { + return kickDoorActionStateMessage; + } + + public void setKickDoorActionStateMessage(KickDoorActionStateMessage kickDoorActionStateMessage) + { + this.kickDoorActionStateMessage = kickDoorActionStateMessage; + } + public List getChildren() { return children; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java new file mode 100644 index 000000000000..bd532f308525 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -0,0 +1,179 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorActionDefinitionMessage; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.node.ObjectNode; +import us.ihmc.behaviors.sequence.ActionNodeDefinition; +import us.ihmc.communication.crdt.*; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SidedObject; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorActionDefinition extends ActionNodeDefinition implements SidedObject +{ + public static final double KICK_HEIGHT = 0.55; + public static final double KICK_IMPULSE = 55.0; + public static final double KICK_TARGET_DISTANCE = 0.75; + public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; + + private final CRDTUnidirectionalEnumField kickSide; + private final CRDTUnidirectionalDouble kickHeight; + private final CRDTUnidirectionalDouble kickImpulse; + private final CRDTUnidirectionalDouble kickTargetDistance; + private final CRDTUnidirectionalDouble prekickWeightDistribution; + + // On disk fields + private RobotSide onDiskSide; + private double onDiskKickHeight; + private double onDiskKickImpulse; + private double onDiskKickTargetDistance; + private double onDiskPrekickWeightDistribution; + + public KickDoorActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) + { + super(crdtInfo, saveFileDirectory); + + kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); + kickHeight = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_HEIGHT); + kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); + kickTargetDistance = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_TARGET_DISTANCE); + prekickWeightDistribution = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, PREKICK_WEIGHT_DISTRIBUTION); + + } + + @Override + public void saveToFile(ObjectNode jsonNode) + { + super.saveToFile(jsonNode); + + jsonNode.put("side", kickSide.getValue().getLowerCaseName()); + jsonNode.put("kickHeight", kickHeight.getValue()); + jsonNode.put("kickImpulse", kickImpulse.getValue()); + jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); + jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); + } + + @Override + public void loadFromFile(JsonNode jsonNode) + { + super.loadFromFile(jsonNode); + + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("side").asText())); + kickHeight.setValue(jsonNode.get("kickHeight").asDouble()); + kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); + kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); + prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); + } + + @Override + public void setOnDiskFields() + { + super.setOnDiskFields(); + + onDiskSide = kickSide.getValue(); + onDiskKickHeight = kickHeight.getValue(); + onDiskKickImpulse = kickImpulse.getValue(); + onDiskKickTargetDistance = kickTargetDistance.getValue(); + onDiskPrekickWeightDistribution = prekickWeightDistribution.getValue(); + } + + @Override + public void undoAllNontopologicalChanges() + { + super.undoAllNontopologicalChanges(); + + kickSide.setValue(onDiskSide); + kickHeight.setValue(onDiskKickHeight); + kickImpulse.setValue(onDiskKickImpulse); + kickTargetDistance.setValue(onDiskKickTargetDistance); + prekickWeightDistribution.setValue(onDiskPrekickWeightDistribution); + } + + @Override + public boolean hasChanges() + { + boolean unchanged = !super.hasChanges(); + + unchanged &= kickSide.getValue() == onDiskSide; + unchanged &= kickHeight.getValue() == onDiskKickHeight; + unchanged &= kickImpulse.getValue() == onDiskKickImpulse; + unchanged &= kickTargetDistance.getValue() == onDiskKickTargetDistance; + unchanged &= prekickWeightDistribution.getValue() == onDiskPrekickWeightDistribution; + + return !unchanged; + } + + public void toMessage(KickDoorActionDefinitionMessage message) + { + super.toMessage(message.getDefinition()); + + message.setRobotSide(kickSide.toMessage().toByte()); + message.setKickHeight(kickHeight.getValue()); + message.setKickImpulse(kickImpulse.getValue()); + message.setKickTargetDistance(kickTargetDistance.getValue()); + message.setPrekickWeightDistribution(prekickWeightDistribution.getValue()); + } + + public void fromMessage(KickDoorActionDefinitionMessage message) + { + super.fromMessage(message.getDefinition()); + + kickSide.fromMessage(RobotSide.fromByte(message.getRobotSide())); + kickHeight.fromMessage(message.getKickHeight()); + kickImpulse.fromMessage(message.getKickImpulse()); + kickTargetDistance.fromMessage(message.getKickTargetDistance()); + prekickWeightDistribution.fromMessage(message.getPrekickWeightDistribution()); + } + + @Override + public RobotSide getSide() + { + return kickSide.getValue(); + } + + public void setKickSide(RobotSide kickSide) + { + this.kickSide.setValue(kickSide); + } + + public double getKickHeight() + { + return kickHeight.getValue(); + } + + public void setKickHeight(double kickHeight) + { + this.kickHeight.setValue(kickHeight); + } + + public double getKickImpulse() + { + return kickImpulse.getValue(); + } + + public void setKickImpulse(double kickImpulse) + { + this.kickImpulse.setValue(kickImpulse); + } + + public double getKickTargetDistance() + { + return kickTargetDistance.getValue(); + } + + public void setKickTargetDistance(double kickTargetDistance) + { + this.kickTargetDistance.setValue(kickTargetDistance); + } + + public double getPrekickWeightDistribution() + { + return prekickWeightDistribution.getValue(); + } + + public void setPrekickWeightDistribution(double prekickWeightDistribution) + { + this.prekickWeightDistribution.setValue(prekickWeightDistribution); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java new file mode 100644 index 000000000000..233e3d2d6208 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java @@ -0,0 +1,23 @@ +package us.ihmc.behaviors.sequence.actions; + +public enum KickDoorActionExecutionState +{ + STANDING, + PREPARING_KICK_FOOT, + EXECUTING_KICKING, + KICK_COMPLETED; + + public static final KickDoorActionExecutionState[] values = values(); + + public byte toByte() + { + return (byte) ordinal(); + } + + public static KickDoorActionExecutionState fromByte(byte enumAsByte) + { + if (enumAsByte == -1) + return null; + return values[enumAsByte]; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java new file mode 100644 index 000000000000..334a3853f34f --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -0,0 +1,119 @@ +package us.ihmc.behaviors.sequence.actions; + +import controller_msgs.msg.dds.FootstepDataListMessage; +import controller_msgs.msg.dds.FootstepDataMessage; +import controller_msgs.msg.dds.HighLevelStateMessage; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.behaviors.sequence.ActionNodeExecutor; +import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.FrameQuaternion; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; +import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorActionExecutor extends ActionNodeExecutor +{ + + private final KickDoorActionState state; + private final KickDoorActionDefinition definition; + private final ROS2ControllerHelper ros2ControllerHelper; + private final ROS2SyncedRobotModel syncedRobot; + private final ControllerStatusTracker controllerStatusTracker; + private final WalkingControllerParameters walkingControllerParameters; + private final FramePose3D solePose = new FramePose3D(); + + public KickDoorActionExecutor(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + ROS2ControllerHelper ros2ControllerHelper, + ROS2SyncedRobotModel syncedRobot, + ControllerStatusTracker controllerStatusTracker, + ReferenceFrameLibrary referenceFrameLibrary, + WalkingControllerParameters walkingControllerParameters) + { + super(new KickDoorActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + this.ros2ControllerHelper = ros2ControllerHelper; + this.syncedRobot = syncedRobot; + this.controllerStatusTracker = controllerStatusTracker; + this.walkingControllerParameters = walkingControllerParameters; + } + + @Override + public void update() + { + super.update(); + } + + @Override + public void triggerActionExecution() + { + super.triggerActionExecution(); + + + state.getExecutionState().setValue(KickDoorActionExecutionState.PREPARING_KICK_FOOT); + } + @Override + public void updateCurrentlyExecuting() + { + switch (state.getExecutionState().getValue()) + { + case STANDING -> + { + //Change to the kicking controller + } + case PREPARING_KICK_FOOT -> + { + //Move the kicking foot to the desired location + state.setIsExecuting(true); + } + case EXECUTING_KICKING -> + { + //Execute the kick + } + case KICK_COMPLETED -> + { + //Do nothing + state.setIsExecuting(false); + } + } + } + + private void changeHighLevelState(HighLevelControllerName highLevelControllerName) + { + // Switch the high level state machine to the desired controller + HighLevelStateMessage highLevelStateMessage = new HighLevelStateMessage(); + highLevelStateMessage.setHighLevelControllerName(highLevelControllerName.toByte()); + ros2ControllerHelper.publishToController(highLevelStateMessage); + } + + /** + * Moves the kicking foot to a new step location based on the output of the {@link us.ihmc.closedSourceControl KickDynamicPlanner}. + * */ + private void prepareKickFoot(FramePoint2DReadOnly desiredSwingFootStartNominal) + { +// // Create the footstep message and send it to the command input manager. +// FootstepDataListMessage footsteps = HumanoidMessageTools.createFootstepDataListMessage(); +// ReferenceFrame pelvisFrame = avatarSimulation.getControllerFullRobotModel().getPelvis().getBodyFixedFrame(); +// FramePoint3D location = new FramePoint3D(desiredSwingFootStartNominal); +// location.setZ(0.0); +// FrameQuaternion orientation = new FrameQuaternion(pelvisFrame, new Quaternion(0.0, 0.0, 0.0, 1.0)); +// orientation.changeFrame(ReferenceFrame.getWorldFrame()); +// FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(definition.getKickSide(), location, orientation); +// footsteps.getFootstepDataList().add().set(footstepData); +// ros2ControllerHelper.publishToController(footsteps); +// avatarSimulation.getSimulationConstructionSet().simulateNow(2.0); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java new file mode 100644 index 000000000000..2fff283b4d9f --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java @@ -0,0 +1,52 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorActionStateMessage; +import us.ihmc.behaviors.sequence.ActionNodeState; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.crdt.CRDTUnidirectionalEnumField; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorActionState extends ActionNodeState +{ + + private CRDTUnidirectionalEnumField executionState; + + /** This is where the kicking foot should step to in preparation for executing the kick */ + + public KickDoorActionState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, ReferenceFrameLibrary referenceFrameLibrary) + { + super(id, new KickDoorActionDefinition(crdtInfo, saveFileDirectory), crdtInfo); + + executionState = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.ROBOT, crdtInfo, KickDoorActionExecutionState.STANDING); + } + + @Override + public void update() + { + } + + public void toMessage(KickDoorActionStateMessage message) + { + getDefinition().toMessage(message.getDefinition()); + + super.toMessage(message.getState()); + + message.setExecutionState(executionState.toMessage().toByte()); + } + + public void fromMessage(KickDoorActionStateMessage message) + { + super.fromMessage(message.getState()); + + getDefinition().fromMessage(message.getDefinition()); + + executionState.fromMessage(KickDoorActionExecutionState.fromByte(message.getExecutionState())); + } + + public CRDTUnidirectionalEnumField getExecutionState() + { + return executionState; + } +} diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl index 76da01cdafa5..8c3a6b26bdfa 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionStateMessage_.idl @@ -21,6 +21,10 @@ module behavior_msgs * Definition */ behavior_msgs::msg::dds::KickDoorActionDefinitionMessage definition; + /** + * Execution state + */ + octet execution_state; }; }; }; 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 4e9cc5485086..b767e8453737 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 "e66183e283438d181acd6be6c8ef5752ee99182d00b9819c1bbf4776b6024a33"; + return "be5bc8ca63f64d0da00ab1e420a45d7e625a19fcd47e3872d3d4a8fda6508046"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java index 118bd839fd02..dc76a9807df0 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessage.java @@ -16,6 +16,10 @@ public class KickDoorActionStateMessage extends Packet getPubSubType() { @@ -73,6 +94,8 @@ public boolean epsilonEquals(KickDoorActionStateMessage other, double epsilon) if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_state_, other.execution_state_, epsilon)) return false; + return true; } @@ -88,6 +111,8 @@ public boolean equals(Object other) if (!this.state_.equals(otherMyClass.state_)) return false; if (!this.definition_.equals(otherMyClass.definition_)) return false; + if(this.execution_state_ != otherMyClass.execution_state_) return false; + return true; } @@ -101,7 +126,9 @@ public java.lang.String toString() builder.append("state="); builder.append(this.state_); builder.append(", "); builder.append("definition="); - builder.append(this.definition_); + builder.append(this.definition_); builder.append(", "); + builder.append("execution_state="); + builder.append(this.execution_state_); builder.append("}"); return builder.toString(); } diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java index 56f6e218c0fe..3fcb1cfcdb4a 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java @@ -15,7 +15,7 @@ public class KickDoorActionStateMessagePubSubType implements us.ihmc.pubsub.Topi @Override public final java.lang.String getDefinitionChecksum() { - return "0961b89263843a0d6ab3e897340fb6b4daf296befde31b74d412955566cf1563"; + return "3a0ebd8fb98c287c4550e03a76a0419036befcca6abf564b0e87ba5fee59ecee"; } @Override @@ -56,6 +56,8 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + return current_alignment - initial_alignment; } @@ -73,6 +75,9 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorActio current_alignment += behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + return current_alignment - initial_alignment; } @@ -81,12 +86,16 @@ public static void write(behavior_msgs.msg.dds.KickDoorActionStateMessage data, { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + cdr.write_type_9(data.getExecutionState()); + } public static void read(behavior_msgs.msg.dds.KickDoorActionStateMessage data, us.ihmc.idl.CDR cdr) { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + data.setExecutionState(cdr.read_type_9()); + } @@ -97,6 +106,7 @@ public final void serialize(behavior_msgs.msg.dds.KickDoorActionStateMessage dat ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType(), data.getDefinition()); + ser.write_type_9("execution_state", data.getExecutionState()); } @Override @@ -106,6 +116,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorActionDefinitionMessagePubSubType(), data.getDefinition()); + data.setExecutionState(ser.read_type_9("execution_state")); } public static void staticCopy(behavior_msgs.msg.dds.KickDoorActionStateMessage src, behavior_msgs.msg.dds.KickDoorActionStateMessage dest) diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg index f32b13be4cb2..e216f1b810d3 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorActionStateMessage.msg @@ -3,3 +3,6 @@ behavior_msgs/ActionNodeStateMessage state # Definition behavior_msgs/KickDoorActionDefinitionMessage definition + +# Execution state +byte execution_state diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg index 1fef75ee98bb..3daba46305d8 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorActionStateMessage.msg @@ -5,4 +5,7 @@ behavior_msgs/ActionNodeStateMessage state # Definition behavior_msgs/KickDoorActionDefinitionMessage definition +# Execution state +int8 execution_state + From bc6ef84fa9578c73937e7da11fe161ebf8cb6b0e Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Fri, 5 Apr 2024 15:27:45 -0500 Subject: [PATCH 08/32] Fixed a communication issue. --- .../ros2/ROS2BehaviorTreeMessageTools.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index 397244a3e530..e7651564f746 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -151,6 +151,10 @@ else if (nodeState instanceof WaitDurationActionState waitDurationActionState) { waitDurationActionState.fromMessage(subscriptionNode.getWaitDurationActionStateMessage()); } + else if (nodeState instanceof KickDoorActionState kickDoorActionState) + { + kickDoorActionState.fromMessage(subscriptionNode.getKickDoorActionStateMessage()); + } else { nodeState.fromMessage(subscriptionNode.getBehaviorTreeNodeStateMessage()); @@ -240,6 +244,13 @@ public static void packSubscriptionNode(byte nodeType, subscriptionNode.setBehaviorTreeNodeStateMessage(waitDurationActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(waitDurationActionStateMessage.getDefinition().getDefinition().getDefinition()); } + case BehaviorTreeStateMessage.KICK_DOOR_ACTION -> + { + KickDoorActionStateMessage kickDoorActionStateMessage = treeStateMessage.getKickDoorActions().get(indexInTypesList); + subscriptionNode.setKickDoorActionStateMessage(kickDoorActionStateMessage); + subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorActionStateMessage.getState().getState()); + subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorActionStateMessage.getDefinition().getDefinition().getDefinition()); + } } } } From 03600dc5235b8ed9707f43dd68551edd35d96982 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Fri, 5 Apr 2024 16:00:07 -0500 Subject: [PATCH 09/32] Added side selection. --- .../rdx/ui/behavior/actions/RDXKickDoorAction.java | 2 -- .../tree/RDXBehaviorTreeNodeCreationMenu.java | 7 ++++++- .../behaviors/sequence/ActionNodeInitialization.java | 5 +++++ .../sequence/actions/KickDoorActionDefinition.java | 2 +- .../sequence/actions/KickDoorActionExecutor.java | 12 ++++-------- 5 files changed, 16 insertions(+), 12 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java index e97813fe8808..8dd476c7d823 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java @@ -9,7 +9,6 @@ import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; import us.ihmc.rdx.imgui.ImDoubleWrapper; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; -import us.ihmc.rdx.imgui.ImStringWrapper; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; @@ -46,7 +45,6 @@ public RDXKickDoorAction(long id, definition.setName("Kick Door"); - //TODO: add robot side to the gui kickHeight = new ImDoubleWrapper(definition::getKickHeight, definition::setKickHeight, imDouble -> ImGui.inputDouble(labels.get("Kick height"), imDouble)); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java index 4aa8a1457b3d..511aba8445ed 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeCreationMenu.java @@ -174,7 +174,6 @@ public void renderImGuiWidgets(RDXBehaviorTreeNode relativeNode, BehaviorT renderNodeCreationClickable(relativeNode, insertionType, "Chest Orientation", ChestOrientationActionDefinition.class, null); renderNodeCreationClickable(relativeNode, insertionType, "Pelvis Height", PelvisHeightPitchActionDefinition.class, null); renderNodeCreationClickable(relativeNode, insertionType, "Wait", WaitDurationActionDefinition.class, null); - renderNodeCreationClickable(relativeNode, insertionType, "Kick Door", KickDoorActionDefinition.class, null); ImGui.text("Screw Primitive: "); for (RobotSide side : RobotSide.values) { @@ -187,6 +186,12 @@ public void renderImGuiWidgets(RDXBehaviorTreeNode relativeNode, BehaviorT ImGui.sameLine(); renderNodeCreationClickable(relativeNode, insertionType, side.getPascalCaseName(), HandWrenchActionDefinition.class, side); } + ImGui.text("Kick Door: "); + for (RobotSide side : RobotSide.values) + { + ImGui.sameLine(); + renderNodeCreationClickable(relativeNode, insertionType, side.getPascalCaseName(), KickDoorActionDefinition.class, side); + } ImGui.unindent(); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java index a9388ff37443..ed80f0142e54 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java @@ -52,6 +52,11 @@ else if (newAction instanceof ScrewPrimitiveActionState screwPrimitiveAction) .setObjectFrameName(findConvenientParentFrameName(actionSequence, HandPoseActionState.class, indexOfInsertion, sideOfNewAction)); screwPrimitiveAction.getState().update(); } + else if ( newAction instanceof KickDoorActionState kickDoorAction) + { + kickDoorAction.getDefinition().setSide(sideOfNewAction); + kickDoorAction.update(); + } else if (newAction instanceof ChestOrientationActionState chestOrientationAction) { ChestOrientationActionState nextPreviousAction = findNextPreviousAction(actionSequence, ChestOrientationActionState.class, indexOfInsertion, null); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java index bd532f308525..ed712b3a8fb9 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -132,7 +132,7 @@ public RobotSide getSide() return kickSide.getValue(); } - public void setKickSide(RobotSide kickSide) + public void setSide(RobotSide kickSide) { this.kickSide.setValue(kickSide); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 334a3853f34f..ee07fd031b5c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -1,7 +1,5 @@ package us.ihmc.behaviors.sequence.actions; -import controller_msgs.msg.dds.FootstepDataListMessage; -import controller_msgs.msg.dds.FootstepDataMessage; import controller_msgs.msg.dds.HighLevelStateMessage; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.ros2.ROS2ControllerHelper; @@ -9,15 +7,11 @@ import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.communication.crdt.CRDTInfo; -import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.FrameQuaternion; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; -import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.tools.io.WorkspaceResourceDirectory; public class KickDoorActionExecutor extends ActionNodeExecutor @@ -30,6 +24,7 @@ public class KickDoorActionExecutor extends ActionNodeExecutor { - //Change to the kicking controller + //Idle until the kick is requested } case PREPARING_KICK_FOOT -> { From 2c91afe69003296865ab30ff485e66e50539a64d Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Tue, 9 Apr 2024 18:29:13 -0500 Subject: [PATCH 10/32] Added a button to add a box placeholder for the door handle. --- .../rdx/perception/sceneGraph/RDXSceneGraphUI.java | 8 ++++++++ .../sequence/actions/KickDoorActionExecutor.java | 11 +++++++++++ .../sequence/actions/KickDoorActionState.java | 2 -- 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java index 1362b47b6c19..29efe61366a3 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphUI.java @@ -199,6 +199,14 @@ private void renderMenuBar(SceneGraphModificationQueue modificationQueue) modificationQueue.accept(new SceneGraphNodeAddition(drill.getSceneNode(), predefinedRigidBodySceneNodeBuilder.getParent())); addUISceneNode(drill); } + //TODO: pattern match this to add a reference frame graphic for the door handle to be dragged around + if (ImGui.button(labels.get("Add Door Handle"))) + { + RDXPredefinedRigidBodySceneNode doorHandle = predefinedRigidBodySceneNodeBuilder.build("Box"); + modificationQueue.accept(new SceneGraphNodeAddition(doorHandle.getSceneNode(), predefinedRigidBodySceneNodeBuilder.getParent())); + addUISceneNode(doorHandle); + } + ImGui.endTable(); } ImGui.endDisabled(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index ee07fd031b5c..8d627593fe0b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -8,6 +8,7 @@ import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; @@ -25,6 +26,7 @@ public class KickDoorActionExecutor extends ActionNodeExecutor executionState; - /** This is where the kicking foot should step to in preparation for executing the kick */ - public KickDoorActionState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, ReferenceFrameLibrary referenceFrameLibrary) { super(id, new KickDoorActionDefinition(crdtInfo, saveFileDirectory), crdtInfo); From 39dcc7dc37494d8bda6798ee7fca961fe5356919 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Tue, 9 Apr 2024 23:10:47 -0500 Subject: [PATCH 11/32] started setting up the approach action for the door kick behavior --- .../donkeyKick/KickDynamicPlanner.java | 333 ++++++++++++ .../donkeyKick/KickInputParameters.java | 96 ++++ .../donkeyKick/KickParameters.java | 206 ++++++++ .../donkeyKick/KickingWBCCParameters.java | 176 +++++++ .../KickDoorApproachPlanActionDefinition.java | 321 ++++++++++++ .../KickDoorApproachPlanActionExecutor.java | 474 ++++++++++++++++++ .../KickDoorApproachPlanActionState.java | 186 +++++++ ...KickDoorApproachPlanDefinitionMessage_.idl | 65 +++ .../msg/KickDoorApproachPlanStateMessage_.idl | 84 ++++ .../BehaviorTreeStateMessagePubSubType.java | 17 +- .../KickDoorActionStateMessagePubSubType.java | 2 +- ...KickDoorApproachPlanDefinitionMessage.java | 370 ++++++++++++++ ...proachPlanDefinitionMessagePubSubType.java | 252 ++++++++++ .../dds/KickDoorApproachPlanStateMessage.java | 345 +++++++++++++ ...oorApproachPlanStateMessagePubSubType.java | 271 ++++++++++ .../msg/dds/TriggerKickMessage.java | 178 +++---- .../KickDoorApproachPlanDefinitionMessage.msg | 32 ++ .../msg/KickDoorApproachPlanStateMessage.msg | 40 ++ .../KickDoorApproachPlanDefinitionMessage.msg | 36 ++ .../msg/KickDoorApproachPlanStateMessage.msg | 46 ++ 20 files changed, 3424 insertions(+), 106 deletions(-) create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java create mode 100644 ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java create mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java new file mode 100644 index 000000000000..99aa3a5c1427 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java @@ -0,0 +1,333 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +import us.ihmc.commonWalkingControlModules.donkeyKick.KickParameters; +import us.ihmc.commonWalkingControlModules.capturePoint.ALIPTools; +import us.ihmc.commonWalkingControlModules.capturePoint.CapturePointTools; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickInputParameters; +import us.ihmc.commonWalkingControlModules.dynamicPlanning.comPlanning.CenterOfMassDynamicsTools; +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DReadOnly; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class KickDynamicPlanner +{ + private static final double assumedFinalShiftDuration = 0.2; + private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + ///////// State variables + private final ReferenceFrame centerOfMassControlFrame; + private final SideDependentList soleFrames; + + ///////// Inputs + private final KickParameters kickParameters; + private final YoDouble estimatedTouchdownDuration = new YoDouble("estimatedTouchdownDuration", registry); + private final double gravityZ; + private final double totalMass; + + ///////// Resulting output variables that are computed + private final YoDouble angularMomentumFromImpact = new YoDouble("angularMomentumFromImpact", registry); + private final YoDouble equivalentVelocityChangeFromImpact = new YoDouble("equivalentVelocityChangeFromImpact", registry); + private final YoDouble acpShiftFromImpact = new YoDouble("acpShiftFromImpact", registry); + + private final FramePoint2D desiredSwingFootStart = new FramePoint2D(); + private final FramePoint2D desiredShiftCoP = new FramePoint2D(); + private final FramePoint2D desiredStanceCoP = new FramePoint2D(); + private final FramePoint2D goalFootPositionAtTouchdown = new FramePoint2D(); + + private final FramePoint2D goalACPPosition = new FramePoint2D(); + private final FramePoint2D acpInWorldAtTouchdown = new FramePoint2D(); + private final FramePoint2D acpInWorldAfterImpact = new FramePoint2D(); + private final FramePoint2D acpInWorldBeforeImpact = new FramePoint2D(); + private final FramePoint2D acpAfterShift = new FramePoint2D(); + + private final FramePoint2D comPositionBeforeImpact = new FramePoint2D(); + private final FramePoint2D comPositionAfterShift = new FramePoint2D(); + private final FramePoint2D comPositionBeforeShift = new FramePoint2D(); + + private final FrameVector2D angularMomentumBeforeImpact = new FrameVector2D(); + private final FrameVector2D angularMomentumAfterShift = new FrameVector2D(); + private final FrameVector2D angularMomentumBeforeShift = new FrameVector2D(); + + // temp variables + private final FramePoint3D tempPoint = new FramePoint3D(); + private final FramePoint3D acpInWorldAtTouchdown3D = new FramePoint3D(); + private final FramePoint3D desiredStanceCoP3D = new FramePoint3D(); + + public KickDynamicPlanner(KickParameters kickParameters, + SideDependentList soleFrames, + ReferenceFrame centerOfMassControlFrame, + double gravityZ, + double totalMass, + YoRegistry parentRegistry) + { + this.kickParameters = kickParameters; + this.centerOfMassControlFrame = centerOfMassControlFrame; + this.soleFrames = soleFrames; + // figure out the touchdown duration after the fact. + this.gravityZ = gravityZ; + this.totalMass = totalMass; + + if (parentRegistry != null) + parentRegistry.addChild(registry); + } + + public FramePoint2DReadOnly getGoalFootPositionAtTouchdown() + { + return goalFootPositionAtTouchdown; + } + + public FramePoint2DReadOnly getGoalACPPosition() + { + return goalACPPosition; + } + + public FramePoint2DReadOnly getACPPositionAtTouchdown() + { + return acpInWorldAtTouchdown; + } + + public FramePoint2DReadOnly getACPPositionAfterImpact() + { + return acpInWorldAfterImpact; + } + + public FramePoint2DReadOnly getACPPositionBeforeImpact() + { + return acpInWorldBeforeImpact; + } + + public FramePoint2DReadOnly getACPPositionAfterShift() + { + return acpAfterShift; + } + + public FramePoint2DReadOnly getDesiredStanceFootCoP() + { + return desiredStanceCoP; + } + + public FramePoint2DReadOnly getDesiredShiftCoP() + { + return desiredShiftCoP; + } + + public FramePoint2DReadOnly getDesiredSwingFootStartNominal() + { + return desiredSwingFootStart; + } + + public FramePoint2DReadOnly getCoMPositionBeforeImpact() + { + return comPositionBeforeImpact; + } + + public FramePoint2DReadOnly getCoMPositionAfterShift() + { + return comPositionAfterShift; + } + + public FramePoint2DReadOnly getCoMPositionBeforeShift() + { + return comPositionBeforeShift; + } + + public FrameVector2DReadOnly getAngularMomentumBeforeImpact() + { + return angularMomentumBeforeImpact; + } + + public FrameVector2DReadOnly getAngularMomentumAfterShift() + { + return angularMomentumAfterShift; + } + + public void compute(KickInputParameters inputParameters) + { + double touchdownDuration = inputParameters.getKickHeight() / kickParameters.getTouchdownHeightSpeed(); + estimatedTouchdownDuration.set(touchdownDuration); + + RobotSide supportSide = inputParameters.getKickFootSide().getOppositeSide(); + double omega = Math.sqrt(gravityZ / kickParameters.getDesiredCoMHeight()); + angularMomentumFromImpact.set(getChangeInAngularMomentumFromImpact(inputParameters)); + equivalentVelocityChangeFromImpact.set(angularMomentumFromImpact.getDoubleValue() / (kickParameters.getDesiredCoMHeight() * totalMass)); + acpShiftFromImpact.set(equivalentVelocityChangeFromImpact.getDoubleValue() / omega); + + // This is the desired CoP position of the stance foot throughout the kick + desiredStanceCoP3D.setToZero(soleFrames.get(supportSide)); + desiredStanceCoP3D.setY(supportSide.negateIfLeftSide(kickParameters.getCopShiftInside())); + desiredStanceCoP3D.changeFrame(ReferenceFrame.getWorldFrame()); + desiredStanceCoP.setIncludingFrame(desiredStanceCoP3D); + + computeGoalFootstepPosition(inputParameters); + computeACPAtTouchdown(supportSide, omega); + computePositionAfterImpactFromGoal(omega); + computePositionBeforeImpact(omega, inputParameters); + computePositionAfterShift(omega); + computeInitialCoPAndCoM(inputParameters, omega); + } + + private void computeGoalFootstepPosition(KickInputParameters inputParameters) + { + RobotSide kickSide = inputParameters.getKickFootSide(); + RobotSide stanceSide = kickSide.getOppositeSide(); + + // Compute the foot position at touchdown, as given by the kick position. This is the foot position in the world once the kick is complete. + double distanceOfTouchdown = inputParameters.getKickTargetDistance() + kickParameters.getDesiredTouchdownPositionRelativeToTarget(); + + tempPoint.setToZero(soleFrames.get(kickSide)); + tempPoint.changeFrame(soleFrames.get(stanceSide)); + tempPoint.setX(-distanceOfTouchdown); + + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + goalFootPositionAtTouchdown.set(tempPoint); + } + + private void computeACPAtTouchdown(RobotSide supportSide, double omega) + { + ///// This method computes where the ACP should be after the final touchdown. When the foot hits the ground, the weight is shifted from one foot to the + ///// goal position, which is where the robot should be stably resting. + goalACPPosition.setIncludingFrame(goalFootPositionAtTouchdown); + goalACPPosition.changeFrameAndProjectToXYPlane(soleFrames.get(supportSide)); + goalACPPosition.scale(0.5); + goalACPPosition.addY(supportSide.negateIfLeftSide(kickParameters.getEndACPCheatInside())); + goalACPPosition.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + + tempPoint.setIncludingFrame(goalACPPosition, 0.0); + acpInWorldAtTouchdown3D.setToZero(ReferenceFrame.getWorldFrame()); + + CenterOfMassDynamicsTools.computeDesiredDCMPositionBackwardTime(omega, + assumedFinalShiftDuration, + assumedFinalShiftDuration, + tempPoint, + desiredStanceCoP3D, + tempPoint, + acpInWorldAtTouchdown3D); + acpInWorldAtTouchdown.set(acpInWorldAtTouchdown3D); + } + + // Back calculate the position of the ACP after the impact using the ACP at touchdown and the desired stance CoP + private void computePositionAfterImpactFromGoal(double omega) + { + double touchdownDuration = estimatedTouchdownDuration.getValue(); + + CapturePointTools.computeDesiredCapturePointPosition(omega, -touchdownDuration, acpInWorldAtTouchdown, desiredStanceCoP, acpInWorldAfterImpact); + } + + // Back calculate the position of the ACP before the impact using the ACP after the impact and the shift from the impact + private void computePositionBeforeImpact(double omega, KickInputParameters inputParameters) + { + tempPoint.setIncludingFrame(acpInWorldAfterImpact, 0.0); + tempPoint.changeFrame(centerOfMassControlFrame); + // this is the velocity change from the impact itself + tempPoint.subX(acpShiftFromImpact.getValue()); + + double angularMomentumFromImpact = getChangeInAngularMomentumFromImpact(inputParameters); + double velocityEquivalent = angularMomentumFromImpact / (totalMass * kickParameters.getDesiredCoMHeight()); + + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + acpInWorldBeforeImpact.set(tempPoint); + + // get the com position right before impact + tempPoint.changeFrame(centerOfMassControlFrame); + tempPoint.addX(velocityEquivalent / omega); + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + comPositionBeforeImpact.set(tempPoint); + + tempPoint.setToZero(centerOfMassControlFrame); + tempPoint.setY(-angularMomentumFromImpact); + tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); + angularMomentumBeforeImpact.set(tempPoint); + } + + // Back calculate the position of the ACP after the shift using the ACP before the impact + private void computePositionAfterShift(double omega) + { + // Compute the ACP translation during the kick itself + double kickMotionDuration = kickParameters.getPushDuration() + kickParameters.getChamberDuration(); + + CapturePointTools.computeDesiredCapturePointPosition(omega, -kickMotionDuration, acpInWorldBeforeImpact, desiredStanceCoP, acpAfterShift); + + //////// Begin "the lines above" //////// + ALIPTools.computeCenterOfMassPosition(-kickMotionDuration, + omega, + totalMass, + gravityZ, + comPositionBeforeImpact, + angularMomentumBeforeImpact, + desiredStanceCoP, + comPositionAfterShift); + ALIPTools.computeAngularMomentum(-kickMotionDuration, + omega, + totalMass, + gravityZ, + comPositionBeforeImpact, + angularMomentumBeforeImpact, + desiredStanceCoP, + angularMomentumAfterShift); + } + + private void computeInitialCoPAndCoM(KickInputParameters inputParameters, double omega) + { + RobotSide kickSide = inputParameters.getKickFootSide(); + RobotSide supportSide = kickSide.getOppositeSide(); + double shiftDuration = kickParameters.getShiftDuration(); + double exponential = Math.exp(omega * shiftDuration); + // double initialWeightDistribution = inputParameters.getPrekickWeightDistribution(); + + ReferenceFrame supportSoleFrame = soleFrames.get(supportSide); + // from the y icp dynamics, we can figure out what the necessary distribution between the stance and swing foot are for weight for the constant cop + acpAfterShift.changeFrameAndProjectToXYPlane(supportSoleFrame); + comPositionAfterShift.changeFrameAndProjectToXYPlane(supportSoleFrame); + tempPoint.setToZero(soleFrames.get(kickSide)); + tempPoint.changeFrame(supportSoleFrame); + + double stanceWidth = tempPoint.getY(); + double initialWeightDistribution = kickParameters.getPreShiftWeightDistribution(); + double initialICPY = initialWeightDistribution * stanceWidth; + double desiredShiftCoPY = (-exponential * initialICPY + acpAfterShift.getY()) / (1.0 - exponential); + + // this is the fraction to go from the + double shiftWeightDistribution = desiredShiftCoPY / stanceWidth; + + // compute the necessary swing foot initial position + double desiredSwingFootInitialPositionX = comPositionAfterShift.getX() / ((initialWeightDistribution - shiftWeightDistribution) * Math.cosh(omega * kickParameters.getShiftDuration()) + shiftWeightDistribution); + double desiredShiftCoPX = shiftWeightDistribution * desiredSwingFootInitialPositionX; + + desiredShiftCoP.setIncludingFrame(supportSoleFrame, desiredShiftCoPX, desiredShiftCoPY); + desiredSwingFootStart.setIncludingFrame(supportSoleFrame, desiredSwingFootInitialPositionX, tempPoint.getY()); + + acpAfterShift.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + comPositionAfterShift.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + desiredShiftCoP.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + desiredSwingFootStart.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); + + ALIPTools.computeCenterOfMassPosition(-shiftDuration, + omega, + totalMass, + gravityZ, + comPositionAfterShift, + angularMomentumAfterShift, + desiredStanceCoP, + comPositionBeforeShift); + ALIPTools.computeAngularMomentum(-shiftDuration, + omega, + totalMass, + gravityZ, + comPositionAfterShift, + angularMomentumAfterShift, + desiredStanceCoP, + angularMomentumBeforeShift); + } + + private static double getChangeInAngularMomentumFromImpact(KickInputParameters inputParameters) + { + return inputParameters.getKickImpulse() * inputParameters.getKickHeight(); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java new file mode 100644 index 000000000000..97dad93f2e7c --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java @@ -0,0 +1,96 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +public class KickInputParameters +{ + private YoEnum kickFootSide; // side for the foot to kick + private YoDouble kickHeight; // height of where you want the kick + private YoDouble kickImpulse; // Newton-seconds + private YoDouble kickTargetDistance; + private YoDouble prekickWeightDistribution; + + public KickInputParameters(YoRegistry parentRegistry) + { + YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + kickFootSide = new YoEnum<>("inputKickFootSide", registry, RobotSide.class); + kickHeight = new YoDouble("inputKickHeight", registry); + kickImpulse = new YoDouble("inputKickImpulse", registry); + kickTargetDistance = new YoDouble("inputKickTargetDistance", registry); + prekickWeightDistribution = new YoDouble("inputPrekickWeightDistribution", registry); + + // These default parameters are generally overwritten with set(TriggerKickCommand kickMessage) + kickFootSide.set(RobotSide.LEFT); + kickHeight.set(0.55); + kickImpulse.set(55.0); + kickTargetDistance.set(0.75); + prekickWeightDistribution.set(0.5); + + if (parentRegistry != null) + parentRegistry.addChild(registry); + } + + public double getKickHeight() + { + return kickHeight.getDoubleValue(); + } + + public double getKickImpulse() + { + return kickImpulse.getDoubleValue(); + } + + public double getKickTargetDistance() + { + return kickTargetDistance.getDoubleValue(); + } + + public RobotSide getKickFootSide() + { + return kickFootSide.getEnumValue(); + } + + public double getPrekickWeightDistribution() + { + return prekickWeightDistribution.getDoubleValue(); + } + + public void setKickHeight(double kickHeight) + { + this.kickHeight.set(kickHeight); + } + + public void setKickImpulse(double kickImpulse) + { + this.kickImpulse.set(kickImpulse); + } + + public void setKickTargetDistance(double kickTargetDistance) + { + this.kickTargetDistance.set(kickTargetDistance); + } + + public void setKickFootSide(RobotSide kickFootSide) + { + this.kickFootSide.set(kickFootSide); + } + + public void setPrekickWeightDistribution(double prekickWeightDistribution) + { + this.prekickWeightDistribution.set(prekickWeightDistribution); + } + + public void set(TriggerKickCommand kickMessage) + { + setKickFootSide(kickMessage.getRobotSide()); + setKickHeight(kickMessage.getKickHeight()); + setKickImpulse(kickMessage.getKickImpulse()); + setKickTargetDistance(kickMessage.getKickTargetDistance()); + setPrekickWeightDistribution(kickMessage.getPrekickWeightDistribution()); + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java new file mode 100644 index 000000000000..dc25fa7d9cd8 --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java @@ -0,0 +1,206 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +public class KickParameters +{ + private static final double desiredCoMHeight = 0.85; + + private static final double coefficientOfFriction = 0.75; + + // When computing the touchdown position to remain stable, this is the extra length to step, as a safety margin + private static final double extraStepLengthInTouchdownForSafety = 0.25; // m + // When computing the touchdown positino to remain stable, this is the minimum step length + private static final double minTouchdownStepDistance = 0.15; // m + + // This is how long we can estimate the force will be applied. This is used to determine how hard the foot should push for + private static final double estimatedImpactDuration = 0.05; // s + // This is the desired height of the foot at the end of chambering, relative to stance + private static final double kickChamberHeight = 0.18; // m + // This is the position of the foot at the end of chambering, relative to stance. + private static final double kickChamberX = 0.25; // m + // This is the mass of the kick leg to assume in order to generate the right amount of angular momentum + private static final double kickLegMass = 100.0; // kg + private static final double kickPickUpVelocity = 0.25; // m/s + + // This is a shift of the desired CoP position towards the inside of the foot when computing the dynamic planner. + private static final double copShiftInside = 0.02; // m + // This cheats the goal position of hte ACP at the touchdown event towards the inside, trying to incentivize the ACP position to be further + // horizontally the stance foot + private static final double endACPCheatInside = 0.02; // m + + // Defines how far past the target the touchdown position should be. Positive is further away, negative is closer. + private static final double desiredTouchdownPositionRelativeToTarget = -0.15; // m + private static final double touchdownHeightSpeed = 1.0; // m /s + + private static final double preShiftWeightDistribution = 0.75; // percentage + private static final double preShiftDuration = 2.5; // s + private static final double shiftDuration = 0.5; // s // TODO make this a computed value + private static final double chamberDuration = 0.25; // s + private static final double pushDuration = 0.2; // s + + private static final double maxAnkleVelocity = 2.5; // rad/s + + private static final double kickShiftProximityThreshold = 0.01; + private static final double kickMinTimeInContact = 0.05; + private static final double minFractionThroughTouchdownToDetectContact = 0.85; + + private static final double kickPenetrationThreshold = 0.04; + + private static final double kpICP = 15.0; + private static final double maxCMPFeedack = 0.10; + private static final double minCMPFeedack = 0.10; + private static final double cmpDistanceInside = 0.005; + private static final double cmpDistanceInsideInTouchdown = 0.025; + private static final double maxCMPRate = 5.0; + + private final KickingWBCCParameters fastWalkingWBCCParameters = new KickingWBCCParameters(); + + public double getDesiredCoMHeight() + { + return desiredCoMHeight; + } + + public double getCoefficientOfFriction() + { + return coefficientOfFriction; + } + + public double getExtraStepLengthInTouchdownForSafety() + { + return extraStepLengthInTouchdownForSafety; + } + + public double getMinTouchdownStepDistance() + { + return minTouchdownStepDistance; + } + + public double getEstimatedImpactDuration() + { + return estimatedImpactDuration; + } + + public double getKickChamberX() + { + return kickChamberX; + } + + public double getKickChamberHeight() + { + return kickChamberHeight; + } + + public double getKickLegMass() + { + return kickLegMass; + } + + public double getKickPickUpVelocity() + { + return kickPickUpVelocity; + } + + public double getCopShiftInside() + { + return copShiftInside; + } + + public double getEndACPCheatInside() + { + return endACPCheatInside; + } + + public double getDesiredTouchdownPositionRelativeToTarget() + { + return desiredTouchdownPositionRelativeToTarget; + } + + public double getTouchdownHeightSpeed() + { + return touchdownHeightSpeed; + } + + public double getPreShiftWeightDistribution() + { + return preShiftWeightDistribution; + } + + public double getPreShiftDuration() + { + return preShiftDuration; + } + + public double getShiftDuration() + { + return shiftDuration; + } + + public double getChamberDuration() + { + return chamberDuration; + } + + public double getPushDuration() + { + return pushDuration; + } + + public double getMaxAnkleVelocity() + { + return maxAnkleVelocity; + } + + public double getKickShiftProximityThreshold() + { + return kickShiftProximityThreshold; + } + + public double getKickMinTimeInContact() + { + return kickMinTimeInContact; + } + + public double getMinFractionThroughTouchdownToDetectContact() + { + return minFractionThroughTouchdownToDetectContact; + } + + public double getKpICP() + { + return kpICP; + } + + public double getMaxCMPFeedack() + { + return maxCMPFeedack; + } + + public double getMinCMPFeedack() + { + return minCMPFeedack; + } + + public double getCmpDistanceInside() + { + return cmpDistanceInside; + } + + public double getCmpDistanceInsideInTouchdown() + { + return cmpDistanceInsideInTouchdown; + } + + public double getMaxCMPRate() + { + return maxCMPRate; + } + + public double getKickPenetrationThreshold() + { + return kickPenetrationThreshold; + } + + public KickingWBCCParameters getKickingWBCCParameters() + { + return fastWalkingWBCCParameters; + } +} diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java new file mode 100644 index 000000000000..8cc4e7a731dc --- /dev/null +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java @@ -0,0 +1,176 @@ +package us.ihmc.commonWalkingControlModules.donkeyKick; + +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.robotics.controllers.pidGains.*; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPID3DGains; +import us.ihmc.robotics.controllers.pidGains.implementations.DefaultPIDSE3Gains; +import us.ihmc.robotics.controllers.pidGains.implementations.PDGains; +import us.ihmc.robotics.weightMatrices.SolverWeightLevels; + +public class KickingWBCCParameters +{ + private final PID3DGains chestOrientationGains = new DefaultPID3DGains(); + private final static double chestOrientationWeight = 50.0; + + private final PID3DGains pelvisOrientationGains = new DefaultPID3DGains(); + private final static double pelvisOrientationWeight = 50.0; + + private final static double stanceFootWeight = SolverWeightLevels.FOOT_SUPPORT_WEIGHT; + + private final PID3DGains chamberFootGains = new DefaultPID3DGains(); + private final static double chamberFootWeight = 50.0; + + private final PID3DGains touchdownFootGains = new DefaultPID3DGains(); + private final static double touchdownFootWeight = 50.0; + + private final PID3DGains settlingFootGains = new DefaultPID3DGains(); + + private final PID3DGains pushFootGains = new DefaultPID3DGains(); + private final static double pushFootWeight = 100.0; + + private final PDGains kickingAnkleGains = new PDGains(); + private final PDGains kickingHipYawGains = new PDGains(); + private final static double kickingAnkleWeight = 50.0; + private final static double kickingHipYawWeight = 50.0; + + private final static double angularMomentumRateWeight = 5.0; + private final static Vector3DReadOnly linearMomentumRateWeight = new Vector3D(0.1, 0.1, 0.1); + + private final PDGains standingHeightControlGains = new PDGains(); + + private final static double desiredCoPPositionWeight = 10.0; + + public KickingWBCCParameters() + { + standingHeightControlGains.setKp(50.0); + standingHeightControlGains.setZeta(0.7); + + chamberFootGains.setProportionalGains(300.0, 300.0, 100.0); + chamberFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(chamberFootGains.getProportionalGains()[0], 0.7), + GainCalculator.computeDerivativeGain(chamberFootGains.getProportionalGains()[1], 0.7), + GainCalculator.computeDerivativeGain(chamberFootGains.getProportionalGains()[2], 0.2)); + + touchdownFootGains.setProportionalGains(300.0); + touchdownFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(300.0, 0.7)); + + settlingFootGains.setProportionalGains(200.0); + settlingFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(200.0, 0.7)); + + pushFootGains.setProportionalGains(100.0, 50.0, 5.0); + pushFootGains.setDerivativeGains(GainCalculator.computeDerivativeGain(pushFootGains.getProportionalGains()[0], 0.7), + GainCalculator.computeDerivativeGain(pushFootGains.getProportionalGains()[1], 0.7), + GainCalculator.computeDerivativeGain(pushFootGains.getProportionalGains()[2], 0.7)); + + chestOrientationGains.setProportionalGains(40.0); + chestOrientationGains.setDerivativeGains(GainCalculator.computeDerivativeGain(40.0, 0.8)); + + pelvisOrientationGains.setProportionalGains(100.0); + pelvisOrientationGains.setDerivativeGains(GainCalculator.computeDerivativeGain(100.0, 0.8)); + + kickingAnkleGains.setKp(50.0); + kickingAnkleGains.setZeta(0.75); + kickingHipYawGains.setKp(50.0); + kickingHipYawGains.setZeta(0.75); + } + + public PDGainsReadOnly getStandingHeightControlGains() + { + return standingHeightControlGains; + } + + public double getChestOrientationWeight() + { + return chestOrientationWeight; + } + + public double getPelvisOrientationWeight() + { + return pelvisOrientationWeight; + } + + public double getChamberFootWeight() + { + return chamberFootWeight; + } + + public double getTouchdownFootWeight() + { + return touchdownFootWeight; + } + + public PID3DGainsReadOnly getChamberFootGains() + { + return chamberFootGains; + } + + public PID3DGainsReadOnly getTouchdownFootGains() + { + return touchdownFootGains; + } + + public PID3DGainsReadOnly getSettlingFootGains() + { + return settlingFootGains; + } + + public double getPushFootWeight() + { + return pushFootWeight; + } + + public PID3DGainsReadOnly getPushFootGains() + { + return pushFootGains; + } + + public PID3DGainsReadOnly getChestOrientationGains() + { + return chestOrientationGains; + } + + public PID3DGainsReadOnly getPelvisOrientationGains() + { + return pelvisOrientationGains; + } + + public Vector3DReadOnly getLinearMomentumRateWeight() + { + return linearMomentumRateWeight; + } + + public double getAngularMomentumRateWeight() + { + return angularMomentumRateWeight; + } + + public double getStanceFootWeight() + { + return stanceFootWeight; + } + + public PDGainsReadOnly getKickingAnkleGains() + { + return kickingAnkleGains; + } + + public PDGainsReadOnly getKickingHipYawGains() + { + return kickingHipYawGains; + } + + public double getKickingAnkleWeight() + { + return kickingAnkleWeight; + } + + public double getKickingHipYawWeight() + { + return kickingHipYawWeight; + } + + public double getDesiredCoPPositionWeight() + { + return desiredCoPPositionWeight; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java new file mode 100644 index 000000000000..d29dacd9ca64 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -0,0 +1,321 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.node.ArrayNode; +import com.fasterxml.jackson.databind.node.ObjectNode; +import us.ihmc.behaviors.sequence.ActionNodeDefinition; +import us.ihmc.commons.MathTools; +import us.ihmc.commons.lists.RecyclingArrayList; +import us.ihmc.communication.crdt.*; +import us.ihmc.communication.packets.ExecutionMode; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.JSONTools; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition +{ + public static final double KICK_IMPULSE = 55.0; + public static final double KICK_TARGET_DISTANCE = 0.75; + public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; + public static final double HORIZONTAL_DISTANCE_FROM_HANDLE = 0.1; + public static final double STANCE_FOOT_WIDTH = 0.23; + + private final CRDTUnidirectionalDouble swingDuration; + private final CRDTUnidirectionalDouble transferDuration; + private final CRDTUnidirectionalEnumField executionMode; + private final CRDTUnidirectionalString parentFrameName; + private final CRDTUnidirectionalRecyclingArrayList footsteps; + private final CRDTUnidirectionalPoint3D goalStancePoint; + private final CRDTUnidirectionalPoint3D goalFocalPoint; + private final SideDependentList goalFootstepToGoalXs; + private final SideDependentList goalFootstepToGoalYs; + private final SideDependentList goalFootstepToGoalYaws; + + private final CRDTUnidirectionalEnumField kickSide; + private final CRDTUnidirectionalDouble kickImpulse; + private final CRDTUnidirectionalDouble kickTargetDistance; + private final CRDTUnidirectionalDouble prekickWeightDistribution; + private final CRDTUnidirectionalDouble horizontalDistanceFromHandle; + private final CRDTUnidirectionalDouble stanceFootWidth; + + // On disk fields + private double onDiskSwingDuration; + private double onDiskTransferDuration; + private ExecutionMode onDiskExecutionMode; + private String onDiskParentFrameName; + private int onDiskNumberOfFootsteps; + private final Point3D onDiskGoalStancePoint = new Point3D(); + private final Point3D onDiskGoalFocalPoint = new Point3D(); + private final SideDependentList onDiskGoalFootstepToGoalXs = new SideDependentList<>(() -> 0.0); + private final SideDependentList onDiskGoalFootstepToGoalYs = new SideDependentList<>(() -> 0.0); + private final SideDependentList onDiskGoalFootstepToGoalYaws = new SideDependentList<>(() -> 0.0); + + public KickDoorApproachPlanActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) + { + super(crdtInfo, saveFileDirectory); + + swingDuration = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 1.2); + transferDuration = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.8); + executionMode = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, ExecutionMode.OVERRIDE); + parentFrameName = new CRDTUnidirectionalString(ROS2ActorDesignation.OPERATOR, crdtInfo, ReferenceFrame.getWorldFrame().getName()); + footsteps = new CRDTUnidirectionalRecyclingArrayList<>(ROS2ActorDesignation.OPERATOR, + crdtInfo, + () -> new RecyclingArrayList<>(() -> new FootstepPlanActionFootstepDefinition(crdtInfo))); + goalStancePoint = new CRDTUnidirectionalPoint3D(ROS2ActorDesignation.OPERATOR, crdtInfo); + goalFocalPoint = new CRDTUnidirectionalPoint3D(ROS2ActorDesignation.OPERATOR, crdtInfo); + goalFootstepToGoalXs = new SideDependentList<>(() -> new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.0)); + goalFootstepToGoalYs = new SideDependentList<>(() -> new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.0)); + goalFootstepToGoalYaws = new SideDependentList<>(() -> new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.0)); + + kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); + kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); + kickTargetDistance = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_TARGET_DISTANCE); + prekickWeightDistribution = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, PREKICK_WEIGHT_DISTRIBUTION); + horizontalDistanceFromHandle = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, HORIZONTAL_DISTANCE_FROM_HANDLE); + stanceFootWidth = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, STANCE_FOOT_WIDTH); + } + + @Override + public void saveToFile(ObjectNode jsonNode) + { + super.saveToFile(jsonNode); + + jsonNode.put("swingDuration", swingDuration.getValue()); + jsonNode.put("transferDuration", transferDuration.getValue()); + jsonNode.put("executionMode", executionMode.getValue().name()); + jsonNode.put("parentFrame", parentFrameName.getValue()); + + JSONTools.toJSON(jsonNode, "goalStancePoint", goalStancePoint.getValueReadOnly()); + JSONTools.toJSON(jsonNode, "goalFocalPoint", goalFocalPoint.getValueReadOnly()); + + for (RobotSide side : RobotSide.values) + { + ObjectNode goalFootNode = jsonNode.putObject(side.getCamelCaseName() + "GoalFootToGoal"); + goalFootNode.put("x", (float) MathTools.roundToPrecision(goalFootstepToGoalXs.get(side).getValue(), 0.0005)); + goalFootNode.put("y", (float) MathTools.roundToPrecision(goalFootstepToGoalYs.get(side).getValue(), 0.0005)); + goalFootNode.put("yawInDegrees", (float) MathTools.roundToPrecision(Math.toDegrees(goalFootstepToGoalYaws.get(side).getValue()), 0.02)); + } + } + + @Override + public void loadFromFile(JsonNode jsonNode) + { + super.loadFromFile(jsonNode); + + swingDuration.setValue(jsonNode.get("swingDuration").asDouble()); + transferDuration.setValue(jsonNode.get("transferDuration").asDouble()); + executionMode.setValue(ExecutionMode.valueOf(jsonNode.get("executionMode").textValue())); + parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); + + footsteps.getValue().clear(); + + JSONTools.toEuclid(jsonNode, "goalStancePoint", goalStancePoint.getValue()); + JSONTools.toEuclid(jsonNode, "goalFocalPoint", goalFocalPoint.getValue()); + + for (RobotSide side : RobotSide.values) + { + ObjectNode goalFootNode = (ObjectNode) jsonNode.get(side.getCamelCaseName() + "GoalFootToGoal"); + goalFootstepToGoalXs.get(side).setValue(goalFootNode.get("x").asDouble()); + goalFootstepToGoalYs.get(side).setValue(goalFootNode.get("y").asDouble()); + goalFootstepToGoalYaws.get(side).setValue(Math.toRadians(goalFootNode.get("yawInDegrees").asDouble())); + } + } + + @Override + public void setOnDiskFields() + { + super.setOnDiskFields(); + + onDiskSwingDuration = swingDuration.getValue(); + onDiskTransferDuration = transferDuration.getValue(); + onDiskExecutionMode = executionMode.getValue(); + onDiskParentFrameName = parentFrameName.getValue(); + onDiskNumberOfFootsteps = footsteps.getSize(); + onDiskGoalStancePoint.set(goalStancePoint.getValueReadOnly()); + onDiskGoalFocalPoint.set(goalFocalPoint.getValueReadOnly()); + for (RobotSide side : goalFootstepToGoalXs.sides()) + { + onDiskGoalFootstepToGoalXs.put(side, goalFootstepToGoalXs.get(side).getValue()); + onDiskGoalFootstepToGoalYs.put(side, goalFootstepToGoalYs.get(side).getValue()); + onDiskGoalFootstepToGoalYaws.put(side, goalFootstepToGoalYaws.get(side).getValue()); + } + + for (int i = 0; i < footsteps.getSize(); i++) + footsteps.getValueReadOnly(i).setOnDiskFields(); + } + + @Override + public void undoAllNontopologicalChanges() + { + super.undoAllNontopologicalChanges(); + + swingDuration.setValue(onDiskSwingDuration); + transferDuration.setValue(onDiskTransferDuration); + executionMode.setValue(onDiskExecutionMode); + parentFrameName.setValue(onDiskParentFrameName); + footsteps.getValue().clear(); + for (int i = 0; i < onDiskNumberOfFootsteps; i++) + footsteps.getValue().add(); + goalStancePoint.getValue().set(onDiskGoalStancePoint); + goalFocalPoint.getValue().set(onDiskGoalFocalPoint); + for (RobotSide side : onDiskGoalFootstepToGoalXs.sides()) + { + goalFootstepToGoalXs.get(side).setValue(onDiskGoalFootstepToGoalXs.get(side)); + goalFootstepToGoalYs.get(side).setValue(onDiskGoalFootstepToGoalYs.get(side)); + goalFootstepToGoalYaws.get(side).setValue(onDiskGoalFootstepToGoalYaws.get(side)); + } + + for (int i = 0; i < footsteps.getSize(); i++) + footsteps.getValue().get(i).undoAllNontopologicalChanges(); + } + + @Override + public boolean hasChanges() + { + boolean unchanged = !super.hasChanges(); + + unchanged &= swingDuration.getValue() == onDiskSwingDuration; + unchanged &= transferDuration.getValue() == onDiskTransferDuration; + unchanged &= executionMode.getValue() == onDiskExecutionMode; + unchanged &= parentFrameName.getValue().equals(onDiskParentFrameName); + unchanged &= goalStancePoint.getValueReadOnly().equals(onDiskGoalStancePoint); + unchanged &= goalFocalPoint.getValueReadOnly().equals(onDiskGoalFocalPoint); + for (RobotSide side : goalFootstepToGoalXs.sides()) + { + unchanged &= goalFootstepToGoalXs.get(side).getValue() == onDiskGoalFootstepToGoalXs.get(side); + unchanged &= goalFootstepToGoalYs.get(side).getValue() == onDiskGoalFootstepToGoalYs.get(side); + unchanged &= goalFootstepToGoalYaws.get(side).getValue() == onDiskGoalFootstepToGoalYaws.get(side); + } + + boolean sameNumberOfFootsteps = footsteps.getSize() == onDiskNumberOfFootsteps; + unchanged &= sameNumberOfFootsteps; + + if (sameNumberOfFootsteps) + for (int i = 0; i < footsteps.getSize(); i++) + unchanged &= !footsteps.getValueReadOnly(i).hasChanges(); + + return !unchanged; + } + + public void toMessage(KickDoorApproachPlanDefinitionMessage message) + { + super.toMessage(message.getDefinition()); + + message.setSwingDuration(swingDuration.toMessage()); + message.setTransferDuration(transferDuration.toMessage()); + message.setExecutionMode(executionMode.toMessageOrdinal()); + message.setParentFrameName(parentFrameName.toMessage()); + + +// goalStancePoint.toMessage(message.getGoalStancePoint()); +// goalFocalPoint.toMessage(message.getGoalFocalPoint()); +// message.setLeftGoalFootXToGizmo(goalFootstepToGoalXs.get(RobotSide.LEFT).toMessage()); +// message.setLeftGoalFootYToGizmo(goalFootstepToGoalYs.get(RobotSide.LEFT).toMessage()); +// message.setLeftGoalFootYawToGizmo(goalFootstepToGoalYaws.get(RobotSide.LEFT).toMessage()); +// message.setRightGoalFootXToGizmo(goalFootstepToGoalXs.get(RobotSide.RIGHT).toMessage()); +// message.setRightGoalFootYToGizmo(goalFootstepToGoalYs.get(RobotSide.RIGHT).toMessage()); +// message.setRightGoalFootYawToGizmo(goalFootstepToGoalYaws.get(RobotSide.RIGHT).toMessage()); + } + + public void fromMessage(KickDoorApproachPlanDefinitionMessage message) + { + super.fromMessage(message.getDefinition()); + + swingDuration.fromMessage(message.getSwingDuration()); + transferDuration.fromMessage(message.getTransferDuration()); + executionMode.fromMessageOrdinal(message.getExecutionMode(), ExecutionMode.values); + parentFrameName.fromMessage(message.getParentFrameNameAsString()); + + +// goalStancePoint.fromMessage(message.getGoalStancePoint()); +// goalFocalPoint.fromMessage(message.getGoalFocalPoint()); +// goalFootstepToGoalXs.get(RobotSide.LEFT).fromMessage(message.getLeftGoalFootXToGizmo()); +// goalFootstepToGoalYs.get(RobotSide.LEFT).fromMessage(message.getLeftGoalFootYToGizmo()); +// goalFootstepToGoalYaws.get(RobotSide.LEFT).fromMessage(message.getLeftGoalFootYawToGizmo()); +// goalFootstepToGoalXs.get(RobotSide.RIGHT).fromMessage(message.getRightGoalFootXToGizmo()); +// goalFootstepToGoalYs.get(RobotSide.RIGHT).fromMessage(message.getRightGoalFootYToGizmo()); +// goalFootstepToGoalYaws.get(RobotSide.RIGHT).fromMessage(message.getRightGoalFootYawToGizmo()); + } + + public double getSwingDuration() + { + return swingDuration.getValue(); + } + + public void setSwingDuration(double swingDuration) + { + this.swingDuration.setValue(swingDuration); + } + + public double getTransferDuration() + { + return transferDuration.getValue(); + } + + public void setTransferDuration(double transferDuration) + { + this.transferDuration.setValue(transferDuration); + } + + public CRDTUnidirectionalEnumField getExecutionMode() + { + return executionMode; + } + + public String getParentFrameName() + { + return parentFrameName.getValue(); + } + + public void setParentFrameName(String parentFrameName) + { + this.parentFrameName.setValue(parentFrameName); + } + + public CRDTUnidirectionalString getCRDTParentFrameName() + { + return parentFrameName; + } + + public CRDTUnidirectionalEnumField getKickSide() + { + return kickSide; + } + + public CRDTUnidirectionalDouble getKickImpulse() + { + return kickImpulse; + } + + public CRDTUnidirectionalDouble getKickTargetDistance() + { + return kickTargetDistance; + } + + public CRDTUnidirectionalDouble getPrekickWeightDistribution() + { + return prekickWeightDistribution; + } + + public CRDTUnidirectionalDouble getHorizontalDistanceFromHandle() + { + return horizontalDistanceFromHandle; + } + + public CRDTUnidirectionalDouble getStanceFootWidth() + { + return stanceFootWidth; + } + + + public CRDTUnidirectionalRecyclingArrayList getFootsteps() + { + return footsteps; + } + +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java new file mode 100644 index 000000000000..2d1c87486fea --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -0,0 +1,474 @@ +package us.ihmc.behaviors.sequence.actions; + +import controller_msgs.msg.dds.FootstepDataListMessage; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.behaviors.sequence.ActionNodeExecutor; +import us.ihmc.behaviors.sequence.TaskspaceTrajectoryTrackingErrorCalculator; +import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickDynamicPlanner; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickInputParameters; +import us.ihmc.commonWalkingControlModules.donkeyKick.KickParameters; +import us.ihmc.commons.Conversions; +import us.ihmc.commons.FormattingTools; +import us.ihmc.commons.exception.DefaultExceptionHandler; +import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.euclid.Axis3D; +import us.ihmc.euclid.geometry.Plane3D; +import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; +import us.ihmc.euclid.matrix.RotationMatrix; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.footstepPlanning.*; +import us.ihmc.footstepPlanning.graphSearch.graph.visualization.BipedalFootstepPlannerNodeRejectionReason; +import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; +import us.ihmc.footstepPlanning.log.FootstepPlannerLogger; +import us.ihmc.footstepPlanning.tools.FootstepPlannerRejectionReasonReport; +import us.ihmc.footstepPlanning.tools.PlannerTools; +import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; +import us.ihmc.robotics.referenceFrames.PoseReferenceFrame; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.referenceFrames.ZUpFrame; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.WorkspaceResourceDirectory; +import us.ihmc.tools.thread.MissingThreadTools; +import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; + +import java.util.UUID; + +public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor +{ + public static final double POSITION_TOLERANCE = 0.15; + public static final double ORIENTATION_TOLERANCE = Math.toRadians(10.0); + + private final KickDoorApproachPlanActionState state; + private final KickDoorApproachPlanActionDefinition definition; + private final ROS2ControllerHelper ros2ControllerHelper; + private final ROS2SyncedRobotModel syncedRobot; + private final ControllerStatusTracker controllerStatusTracker; + private final WalkingControllerParameters walkingControllerParameters; + private final SideDependentList commandedGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final SideDependentList syncedFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final SideDependentList indexOfLastFoot = new SideDependentList<>(); + private double nominalExecutionDuration; + private final SideDependentList trackingCalculators = new SideDependentList<>( + TaskspaceTrajectoryTrackingErrorCalculator::new); + private final FootstepPlan footstepPlanToExecute = new FootstepPlan(); + private final FootstepPlanningModule footstepPlanner; + private final FootstepPlannerParametersBasics footstepPlannerParameters; + private final ResettableExceptionHandlingExecutorService footstepPlanningThread = MissingThreadTools.newSingleThreadExecutor("FootstepPlanning", true, 1); + private final TypedNotification footstepPlanNotification = new TypedNotification<>(); + private final SideDependentList liveGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final SideDependentList startFootPosesForThread = new SideDependentList<>(new FramePose3D(), new FramePose3D()); + private final SideDependentList goalFootPosesForThread = new SideDependentList<>(new FramePose3D(), new FramePose3D()); + + private final KickParameters kickParameters = new KickParameters(); + private final KickInputParameters kickInputParameters = new KickInputParameters(null); + private final SideDependentList soleFramesForPlanning = new SideDependentList<>(); + private final PoseReferenceFrame centerOfMassControlFrameForPlanning = new PoseReferenceFrame("CenterOfMassControlFrameForPlanning", ReferenceFrame.getWorldFrame()); + private static final double gravityZ = -9.81; + private final KickDynamicPlanner kickDynamicPlanner; + + private final ZUpFrame stateParentZUpFrame; + + public KickDoorApproachPlanActionExecutor(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + ROS2ControllerHelper ros2ControllerHelper, + ROS2SyncedRobotModel syncedRobot, + ControllerStatusTracker controllerStatusTracker, + ReferenceFrameLibrary referenceFrameLibrary, + WalkingControllerParameters walkingControllerParameters, + FootstepPlanningModule footstepPlanner, + FootstepPlannerParametersBasics footstepPlannerParameters) + { + super(new KickDoorApproachPlanActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + this.ros2ControllerHelper = ros2ControllerHelper; + this.syncedRobot = syncedRobot; + this.controllerStatusTracker = controllerStatusTracker; + this.walkingControllerParameters = walkingControllerParameters; + this.footstepPlanner = footstepPlanner; + this.footstepPlannerParameters = footstepPlannerParameters; + + stateParentZUpFrame = new ZUpFrame(state.getParentFrame(), "StateParentZUpFrame"); + + for (RobotSide robotSide : RobotSide.values) + { + PoseReferenceFrame soleFrameForPlanning = new PoseReferenceFrame(robotSide.getLowerCaseName() + "SoleFrameForPlanning", ReferenceFrame.getWorldFrame()); + soleFramesForPlanning.put(robotSide, soleFrameForPlanning); + } + this.kickDynamicPlanner = new KickDynamicPlanner(kickParameters, + soleFramesForPlanning, + centerOfMassControlFrameForPlanning, + gravityZ, + syncedRobot.getFullRobotModel().getTotalMass(), + null); + } + + @Override + public void update() + { + super.update(); + + boolean invalidDefinition = false; + + stateParentZUpFrame.update(); + + double kickImpulse = definition.getKickImpulse().getValue(); + double kickTargetDistance = definition.getKickTargetDistance().getValue(); + double prekickWeightDistribution = definition.getPrekickWeightDistribution().getValue(); + double kickStanceWidth = definition.getStanceFootWidth().getValue(); + double kickOffsetFromHandle = definition.getHorizontalDistanceFromHandle().getValue(); + + if (Double.isNaN(kickImpulse) || kickImpulse < 0.0) + { + state.getLogger().error("Kick impulse must be greater than 0.0"); + invalidDefinition = true; + } + + if (Double.isNaN(kickTargetDistance) || kickTargetDistance < 0.0) + { + state.getLogger().error("Kick target distance must be greater than 0.0"); + invalidDefinition = true; + } + + if (Double.isNaN(prekickWeightDistribution) || prekickWeightDistribution < 0.4) + { + state.getLogger().error("Pre kick weight distribution must be greater than 0.4"); + invalidDefinition = true; + } + + if (Double.isNaN(kickStanceWidth) || kickStanceWidth < 0.15) + { + state.getLogger().error("Kick stance width must be greater than 0.15"); + invalidDefinition = true; + } + + if (Double.isNaN(kickOffsetFromHandle) || kickOffsetFromHandle < 0.0) + { + state.getLogger().error("Kick offset from handle must be greater than 0.0"); + invalidDefinition = true; + } + + state.setCanExecute(state.areFramesInWorld() && !invalidDefinition); + if (state.getCanExecute()) + { + computeGoalFootPosesForKick(); + } + + for (RobotSide side : RobotSide.values) + { + trackingCalculators.get(side).update(Conversions.nanosecondsToSeconds(syncedRobot.getTimestamp())); + syncedFeetPoses.get(side).setFromReferenceFrame(syncedRobot.getReferenceFrames().getSoleFrame(side)); + } + } + + private void computeGoalFootPosesForKick() + { + // update the input parameters from the definition + kickInputParameters.setKickImpulse(definition.getKickImpulse().getValue()); + kickInputParameters.setKickTargetDistance(definition.getKickTargetDistance().getValue()); + kickInputParameters.setPrekickWeightDistribution(definition.getPrekickWeightDistribution().getValue()); + kickInputParameters.setKickFootSide(definition.getKickSide().getValue()); + + computeSquaredUpGoalFootPosesFromDoorHandle(); + + // call compute here to get the initial kick foot pose + kickDynamicPlanner.compute(kickInputParameters); + + updateGoalPosesFromDynamicPlanner(); + } + + private void computeSquaredUpGoalFootPosesFromDoorHandle() + { + RobotSide kickSide = definition.getKickSide().getValue(); + + FramePose3D kickFootPose = new FramePose3D(); + kickFootPose.setToZero(stateParentZUpFrame); + kickFootPose.setX(-definition.getKickTargetDistance().getValue()); + kickFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); + kickFootPose.getOrientation().setToYawOrientation(Math.PI); + kickFootPose.changeFrame(ReferenceFrame.getWorldFrame()); + kickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + + FramePose3D stanceFootPose = new FramePose3D(); + stanceFootPose.setToZero(stateParentZUpFrame); + stanceFootPose.setX(-definition.getKickTargetDistance().getValue()); + stanceFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); + stanceFootPose.getOrientation().setToYawOrientation(Math.PI); + stanceFootPose.changeFrame(ReferenceFrame.getWorldFrame()); + stanceFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + + FramePose3D centerOfMassPose = new FramePose3D(); + centerOfMassPose.interpolate(kickFootPose, stanceFootPose, 0.5); + + soleFramesForPlanning.get(kickSide).setPoseAndUpdate(kickFootPose); + soleFramesForPlanning.get(kickSide.getOppositeSide()).setPoseAndUpdate(stanceFootPose); + + centerOfMassControlFrameForPlanning.setPoseAndUpdate(centerOfMassPose); + } + + private void updateGoalPosesFromDynamicPlanner() + { + RobotSide kickSide = definition.getKickSide().getValue(); + RobotSide stanceSide = kickSide.getOppositeSide(); + + FramePose3D stanceGoalPose = new FramePose3D(soleFramesForPlanning.get(stanceSide)); + stanceGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); + + liveGoalFeetPoses.get(stanceSide).setIncludingFrame(stanceGoalPose); + + FramePose3D kickStartPose = new FramePose3D(soleFramesForPlanning.get(kickSide)); + kickStartPose.changeFrame(soleFramesForPlanning.get(stanceSide)); + kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); + kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); + + liveGoalFeetPoses.get(kickSide).setIncludingFrame(kickStartPose); + } + + @Override + public void triggerActionExecution() + { + super.triggerActionExecution(); + + // Reset state + state.setTotalNumberOfFootsteps(0); + state.setNumberOfIncompleteFootsteps(0); + for (RobotSide side : RobotSide.values) + { + state.getCurrentFootPoses().get(side).getValue().set(syncedFeetPoses.get(side)); + state.getDesiredFootPoses().get(side).getValue().clear(); + } + state.getLeftFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.LEFT)); + state.getRightFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.RIGHT)); + + state.setPositionDistanceToGoalTolerance(POSITION_TOLERANCE); + state.setOrientationDistanceToGoalTolerance(ORIENTATION_TOLERANCE); + + if (state.areFramesInWorld()) + { + startFootstepPlanningAsync(); + state.getExecutionState().setValue(FootstepPlanActionExecutionState.FOOTSTEP_PLANNING); + } + else + { + state.getLogger().error("Cannot execute. Frame is not a child of World frame."); + } + } + + @Override + public void updateCurrentlyExecuting() + { + switch (state.getExecutionState().getValue()) + { + case FOOTSTEP_PLANNING -> + { + state.setIsExecuting(true); + // TODO: Maybe report planning elapsed time or something + if (footstepPlanNotification.poll()) + { + footstepPlanToExecute.clear(); + footstepPlanToExecute.set(footstepPlanNotification.read()); + if (footstepPlanToExecute.isEmpty()) + { + state.getExecutionState().setValue(FootstepPlanActionExecutionState.PLANNING_FAILED); + } + else + { + state.getExecutionState().setValue(FootstepPlanActionExecutionState.PLANNING_SUCCEEDED); + } + } + } + case PLANNING_FAILED -> + { + state.getLogger().error("No planned steps to execute!"); + state.setIsExecuting(false); + state.setFailed(true); + } + case PLANNING_SUCCEEDED -> + { + state.setIsExecuting(true); + buildAndSendCommandAndSetDesiredState(); + state.getExecutionState().setValue(FootstepPlanActionExecutionState.PLAN_COMMANDED); + } + case PLAN_COMMANDED -> + { + updateProgress(); + } + } + } + + private void startFootstepPlanningAsync() + { + for (RobotSide side : RobotSide.values) + { + startFootPosesForThread.get(side).setFromReferenceFrame(syncedRobot.getReferenceFrames().getSoleFrame(side)); + goalFootPosesForThread.get(side).set(liveGoalFeetPoses.get(side)); + } + + footstepPlanNotification.poll(); // Make sure it's cleared + footstepPlanningThread.execute(() -> + { + footstepPlannerParameters.setFinalTurnProximity(1.0); + + FootstepPlannerRequest footstepPlannerRequest = new FootstepPlannerRequest(); + footstepPlannerRequest.setPlanBodyPath(false); + footstepPlannerRequest.setStartFootPoses(startFootPosesForThread.get(RobotSide.LEFT), startFootPosesForThread.get(RobotSide.RIGHT)); + // TODO: Set start footholds!! + for (RobotSide side : RobotSide.values) + { + footstepPlannerRequest.setGoalFootPose(side, goalFootPosesForThread.get(side)); + } + + footstepPlannerRequest.setAssumeFlatGround(true); // TODO: Incorporate height map + + footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); + double idealFootstepLength = 0.5; + footstepPlanner.getFootstepPlannerParameters().setIdealFootstepLength(idealFootstepLength); + footstepPlanner.getFootstepPlannerParameters().setMaximumStepReach(idealFootstepLength); + state.getLogger().info("Planning footsteps..."); + FootstepPlannerOutput footstepPlannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); + FootstepPlan footstepPlan = footstepPlannerOutput.getFootstepPlan(); + state.getLogger().info("Footstep planner completed with {}, {} step(s)", footstepPlannerOutput.getFootstepPlanningResult(), footstepPlan.getNumberOfSteps()); + + if (footstepPlan.getNumberOfSteps() < 1) // failed + { + FootstepPlannerRejectionReasonReport rejectionReasonReport = new FootstepPlannerRejectionReasonReport(footstepPlanner); + rejectionReasonReport.update(); + for (BipedalFootstepPlannerNodeRejectionReason reason : rejectionReasonReport.getSortedReasons()) + { + double rejectionPercentage = rejectionReasonReport.getRejectionReasonPercentage(reason); + state.getLogger().info("Rejection {}%: {}", FormattingTools.getFormattedToSignificantFigures(rejectionPercentage, 3), reason); + } + state.getLogger().info("Footstep planning failure..."); + footstepPlanNotification.set(new FootstepPlan()); + } + else + { + for (int i = 0; i < footstepPlan.getNumberOfSteps(); i++) + { + if (i == 0) + footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration() / 2.0); + else + footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration()); + + footstepPlan.getFootstep(i).setSwingDuration(getDefinition().getSwingDuration()); + } + footstepPlanNotification.set(new FootstepPlan(footstepPlan)); // Copy of the output to be safe + } + + FootstepPlannerLogger footstepPlannerLogger = new FootstepPlannerLogger(footstepPlanner); + footstepPlannerLogger.logSession(); + FootstepPlannerLogger.deleteOldLogs(); + }, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE); + } + + private void buildAndSendCommandAndSetDesiredState() + { + FootstepDataListMessage footstepDataListMessage = FootstepDataMessageConverter.createFootstepDataListFromPlan(footstepPlanToExecute, + definition.getSwingDuration(), + definition.getTransferDuration()); + double finalTransferDuration = 0.01; // We don't want any unecessary pauses at the end; but it can't be 0 + footstepDataListMessage.setFinalTransferDuration(finalTransferDuration); + footstepDataListMessage.getQueueingProperties().setExecutionMode(definition.getExecutionMode().getValue().toByte()); + footstepDataListMessage.getQueueingProperties().setMessageId(UUID.randomUUID().getLeastSignificantBits()); + state.getLogger().info("Commanding {} footsteps", footstepDataListMessage.getFootstepDataList().size()); + ros2ControllerHelper.publishToController(footstepDataListMessage); + for (RobotSide side : RobotSide.values) + { + trackingCalculators.get(side).reset(); + } + + nominalExecutionDuration = PlannerTools.calculateNominalTotalPlanExecutionDuration(footstepPlanToExecute, + definition.getSwingDuration(), + walkingControllerParameters.getDefaultInitialTransferTime(), + definition.getTransferDuration(), + finalTransferDuration); + for (RobotSide side : RobotSide.values) + { + indexOfLastFoot.put(side, -1); + } + for (int i = 0; i < footstepPlanToExecute.getNumberOfSteps(); i++) + { + indexOfLastFoot.put(footstepPlanToExecute.getFootstep(i).getRobotSide(), i); + } + + for (RobotSide side : RobotSide.values) + { + int indexOfLastFootSide = indexOfLastFoot.get(side); + if (indexOfLastFootSide >= 0) + { + commandedGoalFeetPoses.get(side).setIncludingFrame(footstepPlanToExecute.getFootstep(indexOfLastFootSide).getFootstepPose()); + } + else + { + commandedGoalFeetPoses.get(side).setIncludingFrame(syncedFeetPoses.get(side)); + } + + state.getDesiredFootPoses().get(side).getValue().clear(); + state.getDesiredFootPoses().get(side).addTrajectoryPoint(syncedFeetPoses.get(side), 0.0); + } + + for (int i = 0; i < footstepPlanToExecute.getNumberOfSteps(); i++) + { + PlannedFootstep footstep = footstepPlanToExecute.getFootstep(i); + double stepCompletionTime = PlannerTools.calculateFootstepCompletionTime(footstepPlanToExecute, + definition.getSwingDuration(), + walkingControllerParameters.getDefaultInitialTransferTime(), + definition.getTransferDuration(), + walkingControllerParameters.getDefaultFinalTransferTime(), + i + 1); + state.getDesiredFootPoses().get(footstep.getRobotSide()).addTrajectoryPoint(footstep.getFootstepPose(), stepCompletionTime); + } + } + + private void updateProgress() + { + boolean hitTimeLimit = false; + boolean meetsDesiredCompletionCriteria = true; + + for (RobotSide side : RobotSide.values) + { + trackingCalculators.get(side).computeExecutionTimings(nominalExecutionDuration); + trackingCalculators.get(side).computePoseTrackingData(commandedGoalFeetPoses.get(side), syncedFeetPoses.get(side)); + trackingCalculators.get(side).factorInR3Errors(POSITION_TOLERANCE); + trackingCalculators.get(side).factoryInSO3Errors(ORIENTATION_TOLERANCE); + meetsDesiredCompletionCriteria &= trackingCalculators.get(side).isWithinPositionTolerance(); + meetsDesiredCompletionCriteria &= trackingCalculators.get(side).getTimeIsUp(); + hitTimeLimit |= trackingCalculators.get(side).getHitTimeLimit(); + } + + int incompleteFootsteps = controllerStatusTracker.getFootstepTracker().getNumberOfIncompleteFootsteps(); + boolean isWalking = controllerStatusTracker.isWalking(); + meetsDesiredCompletionCriteria &= incompleteFootsteps == 0; + meetsDesiredCompletionCriteria &= !isWalking; + + if (meetsDesiredCompletionCriteria || hitTimeLimit) + { + state.setIsExecuting(false); + } + if (hitTimeLimit) + { + state.setFailed(true); + state.getLogger().info("Walking failed. (time limit)"); + } + state.setNominalExecutionDuration(nominalExecutionDuration); + state.setElapsedExecutionTime(trackingCalculators.get(RobotSide.LEFT).getElapsedTime()); + state.setTotalNumberOfFootsteps(footstepPlanToExecute.getNumberOfSteps()); + state.setNumberOfIncompleteFootsteps(incompleteFootsteps); + for (RobotSide side : RobotSide.values) + { + state.getCurrentFootPoses().get(side).getValue().set(syncedFeetPoses.get(side)); + } + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java new file mode 100644 index 000000000000..b138e0c476d2 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java @@ -0,0 +1,186 @@ +package us.ihmc.behaviors.sequence.actions; + +import behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessage; +import behavior_msgs.msg.dds.FootstepPlanActionStateMessage; +import behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage; +import us.ihmc.behaviors.sequence.ActionNodeState; +import us.ihmc.commons.lists.RecyclingArrayList; +import us.ihmc.communication.crdt.*; +import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.robotics.lists.RecyclingArrayListTools; +import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class KickDoorApproachPlanActionState extends ActionNodeState +{ + private final KickDoorApproachPlanActionDefinition definition; + private final ReferenceFrameLibrary referenceFrameLibrary; + private int numberOfAllocatedFootsteps = 0; + private final RecyclingArrayList footsteps; + private final CRDTUnidirectionalPose3D leftFootGoalPose; + private final CRDTUnidirectionalPose3D rightFootGoalPose; + private final SideDependentList goalFootstepToGoalTransforms = new SideDependentList<>(() -> new RigidBodyTransform()); + private final ReferenceFrame parentFrame; + private final CRDTUnidirectionalInteger totalNumberOfFootsteps; + private final CRDTUnidirectionalInteger numberOfIncompleteFootsteps; + private final SideDependentList desiredFootPoses = new SideDependentList<>(); + private final SideDependentList currentFootPoses = new SideDependentList<>(); + private final CRDTUnidirectionalEnumField executionState; + + public KickDoorApproachPlanActionState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, ReferenceFrameLibrary referenceFrameLibrary) + { + super(id, new KickDoorApproachPlanActionDefinition(crdtInfo, saveFileDirectory), crdtInfo); + + definition = getDefinition(); + + this.referenceFrameLibrary = referenceFrameLibrary; + + parentFrame = referenceFrameLibrary.findFrameByName(definition.getParentFrameName()); + + leftFootGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); + rightFootGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); + footsteps = new RecyclingArrayList<>(() -> + new FootstepPlanActionFootstepState(referenceFrameLibrary, + definition.getCRDTParentFrameName(), + RecyclingArrayListTools.getUnsafe(definition.getFootsteps().getValueUnsafe(), numberOfAllocatedFootsteps++))); + totalNumberOfFootsteps = new CRDTUnidirectionalInteger(ROS2ActorDesignation.ROBOT, crdtInfo, 0); + numberOfIncompleteFootsteps = new CRDTUnidirectionalInteger(ROS2ActorDesignation.ROBOT, crdtInfo, 0); + for (RobotSide side : RobotSide.values) + { + desiredFootPoses.set(side, new CRDTUnidirectionalSE3Trajectory(ROS2ActorDesignation.ROBOT, crdtInfo)); + currentFootPoses.set(side, new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo)); + } + executionState = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.ROBOT, crdtInfo, FootstepPlanActionExecutionState.PLANNING_SUCCEEDED); + } + + @Override + public void update() + { + for (RobotSide side : RobotSide.values) + { + goalFootstepToGoalTransforms.get(side).getTranslation().setZ(0.0); + goalFootstepToGoalTransforms.get(side).getRotation().setYawPitchRoll(goalFootstepToGoalTransforms.get(side).getRotation().getYaw(), 0.0, 0.0); + } + + RecyclingArrayListTools.synchronizeSize(footsteps, definition.getFootsteps().getSize()); + + for (int i = 0; i < footsteps.size(); i++) + { + footsteps.get(i).setIndex(i); + footsteps.get(i).update(); + } + } + + public void toMessage(KickDoorApproachPlanStateMessage message) + { + definition.toMessage(message.getDefinition()); + + super.toMessage(message.getState()); + + leftFootGoalPose.toMessage(message.getLeftFootGoalPose()); + rightFootGoalPose.toMessage(message.getRightFootGoalPose()); + message.setTotalNumberOfFootsteps(totalNumberOfFootsteps.toMessage()); + message.setNumberOfIncompleteFootsteps(numberOfIncompleteFootsteps.toMessage()); + desiredFootPoses.get(RobotSide.LEFT).toMessage(message.getDesiredLeftFootsteps()); + desiredFootPoses.get(RobotSide.RIGHT).toMessage(message.getDesiredRightFootsteps()); + currentFootPoses.get(RobotSide.LEFT).toMessage(message.getCurrentLeftFootPose()); + currentFootPoses.get(RobotSide.RIGHT).toMessage(message.getCurrentRightFootPose()); + + message.getFootsteps().clear(); + for (FootstepPlanActionFootstepState footstep : footsteps) + { + footstep.toMessage(message.getFootsteps().add()); + } + + message.setExecutionState(executionState.toMessage().toByte()); + } + + public void fromMessage(KickDoorApproachPlanStateMessage message) + { + super.fromMessage(message.getState()); + + definition.fromMessage(message.getDefinition()); + + leftFootGoalPose.fromMessage(message.getLeftFootGoalPose()); + rightFootGoalPose.fromMessage(message.getRightFootGoalPose()); + totalNumberOfFootsteps.fromMessage(message.getTotalNumberOfFootsteps()); + numberOfIncompleteFootsteps.fromMessage(message.getNumberOfIncompleteFootsteps()); + desiredFootPoses.get(RobotSide.LEFT).fromMessage(message.getDesiredLeftFootsteps()); + desiredFootPoses.get(RobotSide.RIGHT).fromMessage(message.getDesiredRightFootsteps()); + currentFootPoses.get(RobotSide.LEFT).fromMessage(message.getCurrentLeftFootPose()); + currentFootPoses.get(RobotSide.RIGHT).fromMessage(message.getCurrentRightFootPose()); + + footsteps.clear(); + for (FootstepPlanActionFootstepStateMessage footstep : message.getFootsteps()) + { + footsteps.add().fromMessage(footstep); + } + + executionState.fromMessage(FootstepPlanActionExecutionState.fromByte(message.getExecutionState())); + } + + public boolean areFramesInWorld() + { + return referenceFrameLibrary.containsFrame(definition.getParentFrameName()) && parentFrame.getRootFrame() == ReferenceFrame.getWorldFrame(); + } + + public ReferenceFrame getParentFrame() + { + return parentFrame; + } + + public RecyclingArrayList getFootsteps() + { + return footsteps; + } + + public int getTotalNumberOfFootsteps() + { + return totalNumberOfFootsteps.getValue(); + } + + public void setTotalNumberOfFootsteps(int totalNumberOfFootsteps) + { + this.totalNumberOfFootsteps.setValue(totalNumberOfFootsteps); + } + + public int getNumberOfIncompleteFootsteps() + { + return numberOfIncompleteFootsteps.getValue(); + } + + public void setNumberOfIncompleteFootsteps(int numberOfIncompleteFootsteps) + { + this.numberOfIncompleteFootsteps.setValue(numberOfIncompleteFootsteps); + } + + public CRDTUnidirectionalPose3D getLeftFootGoalPose() + { + return leftFootGoalPose; + } + + public CRDTUnidirectionalPose3D getRightFootGoalPose() + { + return rightFootGoalPose; + } + + public SideDependentList getDesiredFootPoses() + { + return desiredFootPoses; + } + + public SideDependentList getCurrentFootPoses() + { + return currentFootPoses; + } + + public CRDTUnidirectionalEnumField getExecutionState() + { + return executionState; + } +} diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl new file mode 100644 index 000000000000..3800d73ceaca --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage_.idl @@ -0,0 +1,65 @@ +#ifndef __behavior_msgs__msg__KickDoorApproachPlanDefinitionMessage__idl__ +#define __behavior_msgs__msg__KickDoorApproachPlanDefinitionMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeDefinitionMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorApproachPlanDefinitionMessage_") + struct KickDoorApproachPlanDefinitionMessage + { + /** + * Parent definition fields + */ + behavior_msgs::msg::dds::ActionNodeDefinitionMessage definition; + /** + * Name of the parent frame the footsteps are expressed in + */ + string parent_frame_name; + /** + * Swing duration + */ + double swing_duration; + /** + * Transfer duration + */ + double transfer_duration; + /** + * OVERRIDE (0) or QUEUE (1) + */ + long execution_mode; + /** + * Specifies the side of the robot that will execute the kick. + */ + @defaultValue(value=255) + octet robot_side; + /** + * The impulse with which the kick should be executed. + */ + double kick_impulse; + /** + * The target distance from the robot to where the kick should be aimed. + */ + double kick_target_distance; + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + double horizontal_distance_from_handle; + /** + * The stance foot width. + */ + double stance_foot_width; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + double prekick_weight_distribution; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl new file mode 100644 index 000000000000..c30dde9e8f82 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl @@ -0,0 +1,84 @@ +#ifndef __behavior_msgs__msg__KickDoorApproachPlanStateMessage__idl__ +#define __behavior_msgs__msg__KickDoorApproachPlanStateMessage__idl__ + +#include "behavior_msgs/msg/./ActionNodeStateMessage_.idl" +#include "behavior_msgs/msg/./FootstepPlanActionFootstepStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorApproachPlanDefinitionMessage_.idl" +#include "geometry_msgs/msg/./Pose_.idl" +#include "ihmc_common_msgs/msg/./SE3TrajectoryPointMessage_.idl" +module behavior_msgs +{ + module msg + { + module dds + { + const octet FOOTSTEP_PLANNING = + 0; + + const octet PLANNING_FAILED = + 1; + + const octet PLANNING_SUCCEEDED = + 2; + + const octet PLAN_COMMANDED = + 3; + + + @TypeCode(type="behavior_msgs::msg::dds_::KickDoorApproachPlanStateMessage_") + struct KickDoorApproachPlanStateMessage + { + /** + * Parent state fields + */ + behavior_msgs::msg::dds::ActionNodeStateMessage state; + /** + * Definition + */ + behavior_msgs::msg::dds::KickDoorApproachPlanDefinitionMessage definition; + /** + * The footsteps, with a maximum of 50 + */ + sequence footsteps; + /** + * Left foot goal pose + */ + geometry_msgs::msg::dds::Pose left_foot_goal_pose; + /** + * Right foot goal pose + */ + geometry_msgs::msg::dds::Pose right_foot_goal_pose; + /** + * Execution state + */ + octet execution_state; + /** + * Total number of footsteps; used for walking actions + */ + unsigned short total_number_of_footsteps; + /** + * Incomplete footsteps; used for walking actions + */ + unsigned short number_of_incomplete_footsteps; + /** + * Desired left footsteps + */ + sequence desired_left_footsteps; + /** + * Desired right footsteps + */ + sequence desired_right_footsteps; + /** + * Current left pose + */ + geometry_msgs::msg::dds::Pose current_left_foot_pose; + /** + * Current right pose + */ + geometry_msgs::msg::dds::Pose current_right_foot_pose; + }; + }; + }; +}; + +#endif 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 50e180542d9e..bce2360c9019 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 "658c0b8666c7972f8457a0083ef39b27b5553665acf3a9c02a2e7f422de7596a"; + return "2c899d203e51bc91b3593b5f091683e6bb453c1db1ed5ad17ef43ad8feb09db3"; } @Override @@ -95,6 +95,9 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) { current_alignment += behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} return current_alignment - initial_alignment; } @@ -179,6 +182,11 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeS { current_alignment += behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType.getCdrSerializedSize(data.getWaitDurationActions().get(i0), current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getKickDoorActions().size(); ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getCdrSerializedSize(data.getKickDoorActions().get(i0), current_alignment);} + return current_alignment - initial_alignment; } @@ -242,6 +250,10 @@ public static void write(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us cdr.write_type_e(data.getWaitDurationActions());else throw new RuntimeException("wait_duration_actions field exceeds the maximum length"); + if(data.getKickDoorActions().size() <= 200) + cdr.write_type_e(data.getKickDoorActions());else + throw new RuntimeException("kick_door_actions field exceeds the maximum length"); + } public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us.ihmc.idl.CDR cdr) @@ -264,6 +276,7 @@ public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us. cdr.read_type_e(data.getScrewPrimitiveActions()); cdr.read_type_e(data.getPelvisHeightActions()); cdr.read_type_e(data.getWaitDurationActions()); + cdr.read_type_e(data.getKickDoorActions()); } @@ -287,6 +300,7 @@ public final void serialize(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, ser.write_type_e("screw_primitive_actions", data.getScrewPrimitiveActions()); ser.write_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.write_type_e("wait_duration_actions", data.getWaitDurationActions()); + ser.write_type_e("kick_door_actions", data.getKickDoorActions()); } @Override @@ -309,6 +323,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_e("screw_primitive_actions", data.getScrewPrimitiveActions()); ser.read_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.read_type_e("wait_duration_actions", data.getWaitDurationActions()); + ser.read_type_e("kick_door_actions", data.getKickDoorActions()); } public static void staticCopy(behavior_msgs.msg.dds.BehaviorTreeStateMessage src, behavior_msgs.msg.dds.BehaviorTreeStateMessage dest) diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java index 3fcb1cfcdb4a..ce269d937d04 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionStateMessagePubSubType.java @@ -15,7 +15,7 @@ public class KickDoorActionStateMessagePubSubType implements us.ihmc.pubsub.Topi @Override public final java.lang.String getDefinitionChecksum() { - return "3a0ebd8fb98c287c4550e03a76a0419036befcca6abf564b0e87ba5fee59ecee"; + return "5bf66e03941840be435c2aa181ac18fb05885d90f6de4f50985c4d6bc276d789"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java new file mode 100644 index 000000000000..4fd51dd57cc2 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessage.java @@ -0,0 +1,370 @@ +package behavior_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; + +public class KickDoorApproachPlanDefinitionMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage definition_; + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.StringBuilder parent_frame_name_; + /** + * Swing duration + */ + public double swing_duration_; + /** + * Transfer duration + */ + public double transfer_duration_; + /** + * OVERRIDE (0) or QUEUE (1) + */ + public int execution_mode_; + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte robot_side_ = (byte) 255; + /** + * The impulse with which the kick should be executed. + */ + public double kick_impulse_; + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double kick_target_distance_; + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + public double horizontal_distance_from_handle_; + /** + * The stance foot width. + */ + public double stance_foot_width_; + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double prekick_weight_distribution_; + + public KickDoorApproachPlanDefinitionMessage() + { + definition_ = new behavior_msgs.msg.dds.ActionNodeDefinitionMessage(); + parent_frame_name_ = new java.lang.StringBuilder(255); + } + + public KickDoorApproachPlanDefinitionMessage(KickDoorApproachPlanDefinitionMessage other) + { + this(); + set(other); + } + + public void set(KickDoorApproachPlanDefinitionMessage other) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + parent_frame_name_.setLength(0); + parent_frame_name_.append(other.parent_frame_name_); + + swing_duration_ = other.swing_duration_; + + transfer_duration_ = other.transfer_duration_; + + execution_mode_ = other.execution_mode_; + + robot_side_ = other.robot_side_; + + kick_impulse_ = other.kick_impulse_; + + kick_target_distance_ = other.kick_target_distance_; + + horizontal_distance_from_handle_ = other.horizontal_distance_from_handle_; + + stance_foot_width_ = other.stance_foot_width_; + + prekick_weight_distribution_ = other.prekick_weight_distribution_; + + } + + + /** + * Parent definition fields + */ + public behavior_msgs.msg.dds.ActionNodeDefinitionMessage getDefinition() + { + return definition_; + } + + /** + * Name of the parent frame the footsteps are expressed in + */ + public void setParentFrameName(java.lang.String parent_frame_name) + { + parent_frame_name_.setLength(0); + parent_frame_name_.append(parent_frame_name); + } + + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.String getParentFrameNameAsString() + { + return getParentFrameName().toString(); + } + /** + * Name of the parent frame the footsteps are expressed in + */ + public java.lang.StringBuilder getParentFrameName() + { + return parent_frame_name_; + } + + /** + * Swing duration + */ + public void setSwingDuration(double swing_duration) + { + swing_duration_ = swing_duration; + } + /** + * Swing duration + */ + public double getSwingDuration() + { + return swing_duration_; + } + + /** + * Transfer duration + */ + public void setTransferDuration(double transfer_duration) + { + transfer_duration_ = transfer_duration; + } + /** + * Transfer duration + */ + public double getTransferDuration() + { + return transfer_duration_; + } + + /** + * OVERRIDE (0) or QUEUE (1) + */ + public void setExecutionMode(int execution_mode) + { + execution_mode_ = execution_mode; + } + /** + * OVERRIDE (0) or QUEUE (1) + */ + public int getExecutionMode() + { + return execution_mode_; + } + + /** + * Specifies the side of the robot that will execute the kick. + */ + public void setRobotSide(byte robot_side) + { + robot_side_ = robot_side; + } + /** + * Specifies the side of the robot that will execute the kick. + */ + public byte getRobotSide() + { + return robot_side_; + } + + /** + * The impulse with which the kick should be executed. + */ + public void setKickImpulse(double kick_impulse) + { + kick_impulse_ = kick_impulse; + } + /** + * The impulse with which the kick should be executed. + */ + public double getKickImpulse() + { + return kick_impulse_; + } + + /** + * The target distance from the robot to where the kick should be aimed. + */ + public void setKickTargetDistance(double kick_target_distance) + { + kick_target_distance_ = kick_target_distance; + } + /** + * The target distance from the robot to where the kick should be aimed. + */ + public double getKickTargetDistance() + { + return kick_target_distance_; + } + + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + public void setHorizontalDistanceFromHandle(double horizontal_distance_from_handle) + { + horizontal_distance_from_handle_ = horizontal_distance_from_handle; + } + /** + * The distance towards the inside of the door from where the kick foot should be aligned. + */ + public double getHorizontalDistanceFromHandle() + { + return horizontal_distance_from_handle_; + } + + /** + * The stance foot width. + */ + public void setStanceFootWidth(double stance_foot_width) + { + stance_foot_width_ = stance_foot_width; + } + /** + * The stance foot width. + */ + public double getStanceFootWidth() + { + return stance_foot_width_; + } + + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public void setPrekickWeightDistribution(double prekick_weight_distribution) + { + prekick_weight_distribution_ = prekick_weight_distribution; + } + /** + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ + public double getPrekickWeightDistribution() + { + return prekick_weight_distribution_; + } + + + public static Supplier getPubSubType() + { + return KickDoorApproachPlanDefinitionMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorApproachPlanDefinitionMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorApproachPlanDefinitionMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.parent_frame_name_, other.parent_frame_name_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.swing_duration_, other.swing_duration_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.transfer_duration_, other.transfer_duration_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_mode_, other.execution_mode_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.horizontal_distance_from_handle_, other.horizontal_distance_from_handle_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.stance_foot_width_, other.stance_foot_width_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) return false; + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorApproachPlanDefinitionMessage)) return false; + + KickDoorApproachPlanDefinitionMessage otherMyClass = (KickDoorApproachPlanDefinitionMessage) other; + + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if (!us.ihmc.idl.IDLTools.equals(this.parent_frame_name_, otherMyClass.parent_frame_name_)) return false; + + if(this.swing_duration_ != otherMyClass.swing_duration_) return false; + + if(this.transfer_duration_ != otherMyClass.transfer_duration_) return false; + + if(this.execution_mode_ != otherMyClass.execution_mode_) return false; + + if(this.robot_side_ != otherMyClass.robot_side_) return false; + + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; + + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; + + if(this.horizontal_distance_from_handle_ != otherMyClass.horizontal_distance_from_handle_) return false; + + if(this.stance_foot_width_ != otherMyClass.stance_foot_width_) return false; + + if(this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) return false; + + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorApproachPlanDefinitionMessage {"); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("parent_frame_name="); + builder.append(this.parent_frame_name_); builder.append(", "); + builder.append("swing_duration="); + builder.append(this.swing_duration_); builder.append(", "); + builder.append("transfer_duration="); + builder.append(this.transfer_duration_); builder.append(", "); + builder.append("execution_mode="); + builder.append(this.execution_mode_); builder.append(", "); + builder.append("robot_side="); + builder.append(this.robot_side_); builder.append(", "); + builder.append("kick_impulse="); + builder.append(this.kick_impulse_); builder.append(", "); + builder.append("kick_target_distance="); + builder.append(this.kick_target_distance_); builder.append(", "); + builder.append("horizontal_distance_from_handle="); + builder.append(this.horizontal_distance_from_handle_); builder.append(", "); + builder.append("stance_foot_width="); + builder.append(this.stance_foot_width_); builder.append(", "); + builder.append("prekick_weight_distribution="); + builder.append(this.prekick_weight_distribution_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java new file mode 100644 index 000000000000..fb871b51f104 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanDefinitionMessagePubSubType.java @@ -0,0 +1,252 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorApproachPlanDefinitionMessage" defined in "KickDoorApproachPlanDefinitionMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorApproachPlanDefinitionMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorApproachPlanDefinitionMessage_.idl instead. +* +*/ +public class KickDoorApproachPlanDefinitionMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorApproachPlanDefinitionMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "6cd05c5ccc35db9123d75e869e60679411dfaf2259ac9cbad039b64e1655ea08"; + } + + @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(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage 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, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage 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 += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1; + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + 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(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getParentFrameName().length() + 1; + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8); + + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + 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(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + if(data.getParentFrameName().length() <= 255) + cdr.write_type_d(data.getParentFrameName());else + throw new RuntimeException("parent_frame_name field exceeds the maximum length"); + + cdr.write_type_6(data.getSwingDuration()); + + cdr.write_type_6(data.getTransferDuration()); + + cdr.write_type_2(data.getExecutionMode()); + + cdr.write_type_9(data.getRobotSide()); + + cdr.write_type_6(data.getKickImpulse()); + + cdr.write_type_6(data.getKickTargetDistance()); + + cdr.write_type_6(data.getHorizontalDistanceFromHandle()); + + cdr.write_type_6(data.getStanceFootWidth()); + + cdr.write_type_6(data.getPrekickWeightDistribution()); + + } + + public static void read(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + cdr.read_type_d(data.getParentFrameName()); + data.setSwingDuration(cdr.read_type_6()); + + data.setTransferDuration(cdr.read_type_6()); + + data.setExecutionMode(cdr.read_type_2()); + + data.setRobotSide(cdr.read_type_9()); + + data.setKickImpulse(cdr.read_type_6()); + + data.setKickTargetDistance(cdr.read_type_6()); + + data.setHorizontalDistanceFromHandle(cdr.read_type_6()); + + data.setStanceFootWidth(cdr.read_type_6()); + + data.setPrekickWeightDistribution(cdr.read_type_6()); + + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_d("parent_frame_name", data.getParentFrameName()); + ser.write_type_6("swing_duration", data.getSwingDuration()); + ser.write_type_6("transfer_duration", data.getTransferDuration()); + ser.write_type_2("execution_mode", data.getExecutionMode()); + ser.write_type_9("robot_side", data.getRobotSide()); + ser.write_type_6("kick_impulse", data.getKickImpulse()); + ser.write_type_6("kick_target_distance", data.getKickTargetDistance()); + ser.write_type_6("horizontal_distance_from_handle", data.getHorizontalDistanceFromHandle()); + ser.write_type_6("stance_foot_width", data.getStanceFootWidth()); + ser.write_type_6("prekick_weight_distribution", data.getPrekickWeightDistribution()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data) + { + ser.read_type_a("definition", new behavior_msgs.msg.dds.ActionNodeDefinitionMessagePubSubType(), data.getDefinition()); + + ser.read_type_d("parent_frame_name", data.getParentFrameName()); + data.setSwingDuration(ser.read_type_6("swing_duration")); + data.setTransferDuration(ser.read_type_6("transfer_duration")); + data.setExecutionMode(ser.read_type_2("execution_mode")); + data.setRobotSide(ser.read_type_9("robot_side")); + data.setKickImpulse(ser.read_type_6("kick_impulse")); + data.setKickTargetDistance(ser.read_type_6("kick_target_distance")); + data.setHorizontalDistanceFromHandle(ser.read_type_6("horizontal_distance_from_handle")); + data.setStanceFootWidth(ser.read_type_6("stance_foot_width")); + data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorApproachPlanDefinitionMessagePubSubType newInstance() + { + return new KickDoorApproachPlanDefinitionMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java new file mode 100644 index 000000000000..e24f78639d45 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java @@ -0,0 +1,345 @@ +package behavior_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; + +public class KickDoorApproachPlanStateMessage extends Packet implements Settable, EpsilonComparable +{ + public static final byte FOOTSTEP_PLANNING = (byte) 0; + public static final byte PLANNING_FAILED = (byte) 1; + public static final byte PLANNING_SUCCEEDED = (byte) 2; + public static final byte PLAN_COMMANDED = (byte) 3; + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage state_; + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage definition_; + /** + * The footsteps, with a maximum of 50 + */ + public us.ihmc.idl.IDLSequence.Object footsteps_; + /** + * Left foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D left_foot_goal_pose_; + /** + * Right foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D right_foot_goal_pose_; + /** + * Execution state + */ + public byte execution_state_; + /** + * Total number of footsteps; used for walking actions + */ + public int total_number_of_footsteps_; + /** + * Incomplete footsteps; used for walking actions + */ + public int number_of_incomplete_footsteps_; + /** + * Desired left footsteps + */ + public us.ihmc.idl.IDLSequence.Object desired_left_footsteps_; + /** + * Desired right footsteps + */ + public us.ihmc.idl.IDLSequence.Object desired_right_footsteps_; + /** + * Current left pose + */ + public us.ihmc.euclid.geometry.Pose3D current_left_foot_pose_; + /** + * Current right pose + */ + public us.ihmc.euclid.geometry.Pose3D current_right_foot_pose_; + + public KickDoorApproachPlanStateMessage() + { + state_ = new behavior_msgs.msg.dds.ActionNodeStateMessage(); + definition_ = new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage(); + footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessagePubSubType()); + left_foot_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + right_foot_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + desired_left_footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType()); + desired_right_footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType()); + current_left_foot_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + current_right_foot_pose_ = new us.ihmc.euclid.geometry.Pose3D(); + + } + + public KickDoorApproachPlanStateMessage(KickDoorApproachPlanStateMessage other) + { + this(); + set(other); + } + + public void set(KickDoorApproachPlanStateMessage other) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.staticCopy(other.state_, state_); + behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + footsteps_.set(other.footsteps_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.left_foot_goal_pose_, left_foot_goal_pose_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.right_foot_goal_pose_, right_foot_goal_pose_); + execution_state_ = other.execution_state_; + + total_number_of_footsteps_ = other.total_number_of_footsteps_; + + number_of_incomplete_footsteps_ = other.number_of_incomplete_footsteps_; + + desired_left_footsteps_.set(other.desired_left_footsteps_); + desired_right_footsteps_.set(other.desired_right_footsteps_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.current_left_foot_pose_, current_left_foot_pose_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.current_right_foot_pose_, current_right_foot_pose_); + } + + + /** + * Parent state fields + */ + public behavior_msgs.msg.dds.ActionNodeStateMessage getState() + { + return state_; + } + + + /** + * Definition + */ + public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage getDefinition() + { + return definition_; + } + + + /** + * The footsteps, with a maximum of 50 + */ + public us.ihmc.idl.IDLSequence.Object getFootsteps() + { + return footsteps_; + } + + + /** + * Left foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D getLeftFootGoalPose() + { + return left_foot_goal_pose_; + } + + + /** + * Right foot goal pose + */ + public us.ihmc.euclid.geometry.Pose3D getRightFootGoalPose() + { + return right_foot_goal_pose_; + } + + /** + * Execution state + */ + public void setExecutionState(byte execution_state) + { + execution_state_ = execution_state; + } + /** + * Execution state + */ + public byte getExecutionState() + { + return execution_state_; + } + + /** + * Total number of footsteps; used for walking actions + */ + public void setTotalNumberOfFootsteps(int total_number_of_footsteps) + { + total_number_of_footsteps_ = total_number_of_footsteps; + } + /** + * Total number of footsteps; used for walking actions + */ + public int getTotalNumberOfFootsteps() + { + return total_number_of_footsteps_; + } + + /** + * Incomplete footsteps; used for walking actions + */ + public void setNumberOfIncompleteFootsteps(int number_of_incomplete_footsteps) + { + number_of_incomplete_footsteps_ = number_of_incomplete_footsteps; + } + /** + * Incomplete footsteps; used for walking actions + */ + public int getNumberOfIncompleteFootsteps() + { + return number_of_incomplete_footsteps_; + } + + + /** + * Desired left footsteps + */ + public us.ihmc.idl.IDLSequence.Object getDesiredLeftFootsteps() + { + return desired_left_footsteps_; + } + + + /** + * Desired right footsteps + */ + public us.ihmc.idl.IDLSequence.Object getDesiredRightFootsteps() + { + return desired_right_footsteps_; + } + + + /** + * Current left pose + */ + public us.ihmc.euclid.geometry.Pose3D getCurrentLeftFootPose() + { + return current_left_foot_pose_; + } + + + /** + * Current right pose + */ + public us.ihmc.euclid.geometry.Pose3D getCurrentRightFootPose() + { + return current_right_foot_pose_; + } + + + public static Supplier getPubSubType() + { + return KickDoorApproachPlanStateMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return KickDoorApproachPlanStateMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(KickDoorApproachPlanStateMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; + if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (this.footsteps_.size() != other.footsteps_.size()) { return false; } + else + { + for (int i = 0; i < this.footsteps_.size(); i++) + { if (!this.footsteps_.get(i).epsilonEquals(other.footsteps_.get(i), epsilon)) return false; } + } + + if (!this.left_foot_goal_pose_.epsilonEquals(other.left_foot_goal_pose_, epsilon)) return false; + if (!this.right_foot_goal_pose_.epsilonEquals(other.right_foot_goal_pose_, epsilon)) return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_state_, other.execution_state_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.total_number_of_footsteps_, other.total_number_of_footsteps_, epsilon)) return false; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.number_of_incomplete_footsteps_, other.number_of_incomplete_footsteps_, epsilon)) return false; + + if (this.desired_left_footsteps_.size() != other.desired_left_footsteps_.size()) { return false; } + else + { + for (int i = 0; i < this.desired_left_footsteps_.size(); i++) + { if (!this.desired_left_footsteps_.get(i).epsilonEquals(other.desired_left_footsteps_.get(i), epsilon)) return false; } + } + + if (this.desired_right_footsteps_.size() != other.desired_right_footsteps_.size()) { return false; } + else + { + for (int i = 0; i < this.desired_right_footsteps_.size(); i++) + { if (!this.desired_right_footsteps_.get(i).epsilonEquals(other.desired_right_footsteps_.get(i), epsilon)) return false; } + } + + if (!this.current_left_foot_pose_.epsilonEquals(other.current_left_foot_pose_, epsilon)) return false; + if (!this.current_right_foot_pose_.epsilonEquals(other.current_right_foot_pose_, epsilon)) return false; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof KickDoorApproachPlanStateMessage)) return false; + + KickDoorApproachPlanStateMessage otherMyClass = (KickDoorApproachPlanStateMessage) other; + + if (!this.state_.equals(otherMyClass.state_)) return false; + if (!this.definition_.equals(otherMyClass.definition_)) return false; + if (!this.footsteps_.equals(otherMyClass.footsteps_)) return false; + if (!this.left_foot_goal_pose_.equals(otherMyClass.left_foot_goal_pose_)) return false; + if (!this.right_foot_goal_pose_.equals(otherMyClass.right_foot_goal_pose_)) return false; + if(this.execution_state_ != otherMyClass.execution_state_) return false; + + if(this.total_number_of_footsteps_ != otherMyClass.total_number_of_footsteps_) return false; + + if(this.number_of_incomplete_footsteps_ != otherMyClass.number_of_incomplete_footsteps_) return false; + + if (!this.desired_left_footsteps_.equals(otherMyClass.desired_left_footsteps_)) return false; + if (!this.desired_right_footsteps_.equals(otherMyClass.desired_right_footsteps_)) return false; + if (!this.current_left_foot_pose_.equals(otherMyClass.current_left_foot_pose_)) return false; + if (!this.current_right_foot_pose_.equals(otherMyClass.current_right_foot_pose_)) return false; + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("KickDoorApproachPlanStateMessage {"); + builder.append("state="); + builder.append(this.state_); builder.append(", "); + builder.append("definition="); + builder.append(this.definition_); builder.append(", "); + builder.append("footsteps="); + builder.append(this.footsteps_); builder.append(", "); + builder.append("left_foot_goal_pose="); + builder.append(this.left_foot_goal_pose_); builder.append(", "); + builder.append("right_foot_goal_pose="); + builder.append(this.right_foot_goal_pose_); builder.append(", "); + builder.append("execution_state="); + builder.append(this.execution_state_); builder.append(", "); + builder.append("total_number_of_footsteps="); + builder.append(this.total_number_of_footsteps_); builder.append(", "); + builder.append("number_of_incomplete_footsteps="); + builder.append(this.number_of_incomplete_footsteps_); builder.append(", "); + builder.append("desired_left_footsteps="); + builder.append(this.desired_left_footsteps_); builder.append(", "); + builder.append("desired_right_footsteps="); + builder.append(this.desired_right_footsteps_); builder.append(", "); + builder.append("current_left_foot_pose="); + builder.append(this.current_left_foot_pose_); builder.append(", "); + builder.append("current_right_foot_pose="); + builder.append(this.current_right_foot_pose_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java new file mode 100644 index 000000000000..cae570dc9d08 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java @@ -0,0 +1,271 @@ +package behavior_msgs.msg.dds; + +/** +* +* Topic data type of the struct "KickDoorApproachPlanStateMessage" defined in "KickDoorApproachPlanStateMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from KickDoorApproachPlanStateMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorApproachPlanStateMessage_.idl instead. +* +*/ +public class KickDoorApproachPlanStateMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "behavior_msgs::msg::dds_::KickDoorApproachPlanStateMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "02cbee0a296afe41ffd8e462465f1c0a8c85d3293d8ba89f7a7d4d59ba918691"; + } + + @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(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage 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, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage 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 += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0) + { + current_alignment += behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.getCdrSerializedSize(data.getState(), current_alignment); + + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getFootsteps().size(); ++i0) + { + current_alignment += behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessagePubSubType.getCdrSerializedSize(data.getFootsteps().get(i0), current_alignment);} + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getLeftFootGoalPose(), current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getRightFootGoalPose(), current_alignment); + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + + current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); + + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getDesiredLeftFootsteps().size(); ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getDesiredLeftFootsteps().get(i0), current_alignment);} + + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getDesiredRightFootsteps().size(); ++i0) + { + current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getDesiredRightFootsteps().get(i0), current_alignment);} + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getCurrentLeftFootPose(), current_alignment); + + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getCurrentRightFootPose(), current_alignment); + + + return current_alignment - initial_alignment; + } + + public static void write(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + if(data.getFootsteps().size() <= 100) + cdr.write_type_e(data.getFootsteps());else + throw new RuntimeException("footsteps field exceeds the maximum length"); + + geometry_msgs.msg.dds.PosePubSubType.write(data.getLeftFootGoalPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getRightFootGoalPose(), cdr); + cdr.write_type_9(data.getExecutionState()); + + cdr.write_type_3(data.getTotalNumberOfFootsteps()); + + cdr.write_type_3(data.getNumberOfIncompleteFootsteps()); + + if(data.getDesiredLeftFootsteps().size() <= 100) + cdr.write_type_e(data.getDesiredLeftFootsteps());else + throw new RuntimeException("desired_left_footsteps field exceeds the maximum length"); + + if(data.getDesiredRightFootsteps().size() <= 100) + cdr.write_type_e(data.getDesiredRightFootsteps());else + throw new RuntimeException("desired_right_footsteps field exceeds the maximum length"); + + geometry_msgs.msg.dds.PosePubSubType.write(data.getCurrentLeftFootPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getCurrentRightFootPose(), cdr); + } + + public static void read(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); + behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + cdr.read_type_e(data.getFootsteps()); + geometry_msgs.msg.dds.PosePubSubType.read(data.getLeftFootGoalPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getRightFootGoalPose(), cdr); + data.setExecutionState(cdr.read_type_9()); + + data.setTotalNumberOfFootsteps(cdr.read_type_3()); + + data.setNumberOfIncompleteFootsteps(cdr.read_type_3()); + + cdr.read_type_e(data.getDesiredLeftFootsteps()); + cdr.read_type_e(data.getDesiredRightFootsteps()); + geometry_msgs.msg.dds.PosePubSubType.read(data.getCurrentLeftFootPose(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getCurrentRightFootPose(), cdr); + + } + + @Override + public final void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); + + ser.write_type_e("footsteps", data.getFootsteps()); + ser.write_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); + + ser.write_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); + + ser.write_type_9("execution_state", data.getExecutionState()); + ser.write_type_3("total_number_of_footsteps", data.getTotalNumberOfFootsteps()); + ser.write_type_3("number_of_incomplete_footsteps", data.getNumberOfIncompleteFootsteps()); + ser.write_type_e("desired_left_footsteps", data.getDesiredLeftFootsteps()); + ser.write_type_e("desired_right_footsteps", data.getDesiredRightFootsteps()); + ser.write_type_a("current_left_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentLeftFootPose()); + + ser.write_type_a("current_right_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentRightFootPose()); + + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data) + { + ser.read_type_a("state", new behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType(), data.getState()); + + ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); + + ser.read_type_e("footsteps", data.getFootsteps()); + ser.read_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); + + ser.read_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); + + data.setExecutionState(ser.read_type_9("execution_state")); + data.setTotalNumberOfFootsteps(ser.read_type_3("total_number_of_footsteps")); + data.setNumberOfIncompleteFootsteps(ser.read_type_3("number_of_incomplete_footsteps")); + ser.read_type_e("desired_left_footsteps", data.getDesiredLeftFootsteps()); + ser.read_type_e("desired_right_footsteps", data.getDesiredRightFootsteps()); + ser.read_type_a("current_left_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentLeftFootPose()); + + ser.read_type_a("current_right_foot_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getCurrentRightFootPose()); + + } + + public static void staticCopy(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage dest) + { + dest.set(src); + } + + @Override + public behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage createData() + { + return new behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage src, behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage dest) + { + staticCopy(src, dest); + } + + @Override + public KickDoorApproachPlanStateMessagePubSubType newInstance() + { + return new KickDoorApproachPlanStateMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java index da7fc1038d40..806003ca4545 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java @@ -3,46 +3,44 @@ 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 message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. - */ + * This message is part of the IHMC whole-body controller API. + * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. + */ public class TriggerKickMessage extends Packet implements Settable, EpsilonComparable { public static final byte ROBOT_SIDE_LEFT = (byte) 0; public static final byte ROBOT_SIDE_RIGHT = (byte) 1; /** - * Unique ID used to identify this message, should preferably be consecutively increasing. - */ + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ public long sequence_id_; /** - * Specifies the side of the robot that will execute the kick. - */ + * Specifies the side of the robot that will execute the kick. + */ public byte robot_side_; /** - * The height at which the kick should be targeted. - */ + * The height at which the kick should be targeted. + */ public double kick_height_; /** - * The impulse with which the kick should be executed. - */ + * The impulse with which the kick should be executed. + */ public double kick_impulse_; /** - * The target distance from the robot to where the kick should be aimed. - */ + * The target distance from the robot to where the kick should be aimed. + */ public double kick_target_distance_; /** - * A boolean for tracking whether a kick has been requested. - */ + * A boolean for tracking whether a kick has been requested. + */ public boolean trigger_kick_request_; /** - * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. - */ + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ public double prekick_weight_distribution_; public TriggerKickMessage() @@ -70,120 +68,115 @@ public void set(TriggerKickMessage other) trigger_kick_request_ = other.trigger_kick_request_; prekick_weight_distribution_ = other.prekick_weight_distribution_; + } /** - * Unique ID used to identify this message, should preferably be consecutively increasing. - */ + * 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. - */ + * Unique ID used to identify this message, should preferably be consecutively increasing. + */ public long getSequenceId() { return sequence_id_; } /** - * Specifies the side of the robot that will execute the kick. - */ + * Specifies the side of the robot that will execute the kick. + */ public void setRobotSide(byte robot_side) { robot_side_ = robot_side; } - /** - * Specifies the side of the robot that will execute the kick. - */ + * Specifies the side of the robot that will execute the kick. + */ public byte getRobotSide() { return robot_side_; } /** - * The height at which the kick should be targeted. - */ + * The height at which the kick should be targeted. + */ public void setKickHeight(double kick_height) { kick_height_ = kick_height; } - /** - * The height at which the kick should be targeted. - */ + * The height at which the kick should be targeted. + */ public double getKickHeight() { return kick_height_; } /** - * The impulse with which the kick should be executed. - */ + * The impulse with which the kick should be executed. + */ public void setKickImpulse(double kick_impulse) { kick_impulse_ = kick_impulse; } - /** - * The impulse with which the kick should be executed. - */ + * The impulse with which the kick should be executed. + */ public double getKickImpulse() { return kick_impulse_; } /** - * The target distance from the robot to where the kick should be aimed. - */ + * The target distance from the robot to where the kick should be aimed. + */ public void setKickTargetDistance(double kick_target_distance) { kick_target_distance_ = kick_target_distance; } - /** - * The target distance from the robot to where the kick should be aimed. - */ + * The target distance from the robot to where the kick should be aimed. + */ public double getKickTargetDistance() { return kick_target_distance_; } /** - * A boolean for tracking whether a kick has been requested. - */ + * A boolean for tracking whether a kick has been requested. + */ public void setTriggerKickRequest(boolean trigger_kick_request) { trigger_kick_request_ = trigger_kick_request; } - /** - * A boolean for tracking whether a kick has been requested. - */ + * A boolean for tracking whether a kick has been requested. + */ public boolean getTriggerKickRequest() { return trigger_kick_request_; } /** - * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. - */ + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ public void setPrekickWeightDistribution(double prekick_weight_distribution) { prekick_weight_distribution_ = prekick_weight_distribution; } - /** - * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. - */ + * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. + */ public double getPrekickWeightDistribution() { return prekick_weight_distribution_; } + public static Supplier getPubSubType() { return TriggerKickMessagePubSubType::new; @@ -198,31 +191,23 @@ public Supplier getPubSubTypePacket() @Override public boolean epsilonEquals(TriggerKickMessage other, double epsilon) { - if (other == null) - return false; - if (other == this) - return true; + 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.sequence_id_, other.sequence_id_, epsilon)) - return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) - return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_height_, other.kick_height_, epsilon)) - return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_impulse_, other.kick_impulse_, epsilon)) - return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.kick_target_distance_, other.kick_target_distance_, epsilon)) - return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.trigger_kick_request_, other.trigger_kick_request_, epsilon)) - return false; + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prekick_weight_distribution_, other.prekick_weight_distribution_, epsilon)) - return false; return true; } @@ -230,35 +215,26 @@ public boolean epsilonEquals(TriggerKickMessage other, double epsilon) @Override public boolean equals(Object other) { - if (other == null) - return false; - if (other == this) - return true; - if (!(other instanceof TriggerKickMessage)) - return false; + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof TriggerKickMessage)) return false; TriggerKickMessage otherMyClass = (TriggerKickMessage) other; - if (this.sequence_id_ != otherMyClass.sequence_id_) - return false; + if(this.sequence_id_ != otherMyClass.sequence_id_) return false; + + if(this.robot_side_ != otherMyClass.robot_side_) return false; - if (this.robot_side_ != otherMyClass.robot_side_) - return false; + if(this.kick_height_ != otherMyClass.kick_height_) return false; - if (this.kick_height_ != otherMyClass.kick_height_) - return false; + if(this.kick_impulse_ != otherMyClass.kick_impulse_) return false; - if (this.kick_impulse_ != otherMyClass.kick_impulse_) - return false; + if(this.kick_target_distance_ != otherMyClass.kick_target_distance_) return false; - if (this.kick_target_distance_ != otherMyClass.kick_target_distance_) - return false; + if(this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) return false; - if (this.trigger_kick_request_ != otherMyClass.trigger_kick_request_) - return false; + if(this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) return false; - if (this.prekick_weight_distribution_ != otherMyClass.prekick_weight_distribution_) - return false; return true; } @@ -270,23 +246,17 @@ public java.lang.String toString() builder.append("TriggerKickMessage {"); builder.append("sequence_id="); - builder.append(this.sequence_id_); - builder.append(", "); + builder.append(this.sequence_id_); builder.append(", "); builder.append("robot_side="); - builder.append(this.robot_side_); - builder.append(", "); + builder.append(this.robot_side_); builder.append(", "); builder.append("kick_height="); - builder.append(this.kick_height_); - builder.append(", "); + builder.append(this.kick_height_); builder.append(", "); builder.append("kick_impulse="); - builder.append(this.kick_impulse_); - builder.append(", "); + builder.append(this.kick_impulse_); builder.append(", "); builder.append("kick_target_distance="); - builder.append(this.kick_target_distance_); - builder.append(", "); + builder.append(this.kick_target_distance_); builder.append(", "); builder.append("trigger_kick_request="); - builder.append(this.trigger_kick_request_); - builder.append(", "); + builder.append(this.trigger_kick_request_); builder.append(", "); builder.append("prekick_weight_distribution="); builder.append(this.prekick_weight_distribution_); builder.append("}"); diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg new file mode 100644 index 000000000000..f7b4f5934b7c --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg @@ -0,0 +1,32 @@ +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Name of the parent frame the footsteps are expressed in +string parent_frame_name + +# Swing duration +float64 swing_duration + +# Transfer duration +float64 transfer_duration + +# OVERRIDE (0) or QUEUE (1) +int32 execution_mode + +# Specifies the side of the robot that will execute the kick. +byte robot_side 255 + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# The distance towards the inside of the door from where the kick foot should be aligned. +float64 horizontal_distance_from_handle + +# The stance foot width. +float64 stance_foot_width + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg new file mode 100644 index 000000000000..1f2519989f94 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -0,0 +1,40 @@ +byte FOOTSTEP_PLANNING = 0 +byte PLANNING_FAILED = 1 +byte PLANNING_SUCCEEDED = 2 +byte PLAN_COMMANDED = 3 + +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorApproachPlanDefinitionMessage definition + +# The footsteps, with a maximum of 50 +behavior_msgs/FootstepPlanActionFootstepStateMessage[] footsteps + +# Left foot goal pose +geometry_msgs/Pose left_foot_goal_pose + +# Right foot goal pose +geometry_msgs/Pose right_foot_goal_pose + +# Execution state +int8 execution_state + +# Total number of footsteps; used for walking actions +uint16 total_number_of_footsteps + +# Incomplete footsteps; used for walking actions +uint16 number_of_incomplete_footsteps + +# Desired left footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_left_footsteps + +# Desired right footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_right_footsteps + +# Current left pose +geometry_msgs/Pose current_left_foot_pose + +# Current right pose +geometry_msgs/Pose current_right_foot_pose diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg new file mode 100644 index 000000000000..80c545330c15 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg @@ -0,0 +1,36 @@ + +# Parent definition fields +behavior_msgs/ActionNodeDefinitionMessage definition + +# Name of the parent frame the footsteps are expressed in +string parent_frame_name + +# Swing duration +float64 swing_duration + +# Transfer duration +float64 transfer_duration + +# OVERRIDE (0) or QUEUE (1) +int32 execution_mode + +# Specifies the side of the robot that will execute the kick. +# Field default value 255 +int8 robot_side + +# The impulse with which the kick should be executed. +float64 kick_impulse + +# The target distance from the robot to where the kick should be aimed. +float64 kick_target_distance + +# The distance towards the inside of the door from where the kick foot should be aligned. +float64 horizontal_distance_from_handle + +# The stance foot width. +float64 stance_foot_width + +# Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. +float64 prekick_weight_distribution + + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg new file mode 100644 index 000000000000..91ac6b0cb414 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -0,0 +1,46 @@ + +int8 FOOTSTEP_PLANNING=0 + +int8 PLANNING_FAILED=1 + +int8 PLANNING_SUCCEEDED=2 + +int8 PLAN_COMMANDED=3 + +# Parent state fields +behavior_msgs/ActionNodeStateMessage state + +# Definition +behavior_msgs/KickDoorApproachPlanDefinitionMessage definition + +# The footsteps, with a maximum of 50 +behavior_msgs/FootstepPlanActionFootstepStateMessage[] footsteps + +# Left foot goal pose +geometry_msgs/Pose left_foot_goal_pose + +# Right foot goal pose +geometry_msgs/Pose right_foot_goal_pose + +# Execution state +int8 execution_state + +# Total number of footsteps; used for walking actions +uint16 total_number_of_footsteps + +# Incomplete footsteps; used for walking actions +uint16 number_of_incomplete_footsteps + +# Desired left footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_left_footsteps + +# Desired right footsteps +ihmc_common_msgs/SE3TrajectoryPointMessage[] desired_right_footsteps + +# Current left pose +geometry_msgs/Pose current_left_foot_pose + +# Current right pose +geometry_msgs/Pose current_right_foot_pose + + From f8cba8c345350be6d4f35cc75efdc84fe5f87de8 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Wed, 10 Apr 2024 00:04:30 -0500 Subject: [PATCH 12/32] set up the rdx values and did some set ups --- .../RDXKickDoorApproachPlanAction.java | 199 ++++++++++++++++++ .../BehaviorTreeDefinitionBuilder.java | 13 +- .../BehaviorTreeExecutorNodeBuilder.java | 13 ++ .../KickDoorApproachPlanActionDefinition.java | 168 +++++---------- .../KickDoorApproachPlanActionExecutor.java | 7 - .../KickDoorApproachPlanActionState.java | 42 ---- .../msg/KickDoorApproachPlanStateMessage_.idl | 5 - .../dds/KickDoorApproachPlanStateMessage.java | 25 --- ...oorApproachPlanStateMessagePubSubType.java | 17 +- .../msg/KickDoorApproachPlanStateMessage.msg | 3 - .../msg/KickDoorApproachPlanStateMessage.msg | 3 - 11 files changed, 271 insertions(+), 224 deletions(-) create mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java new file mode 100644 index 000000000000..84af92b0ce67 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java @@ -0,0 +1,199 @@ +package us.ihmc.rdx.ui.behavior.actions; + +import com.badlogic.gdx.graphics.Color; +import com.badlogic.gdx.graphics.g3d.Renderable; +import com.badlogic.gdx.utils.Array; +import com.badlogic.gdx.utils.Pool; +import imgui.ImGui; +import imgui.type.ImBoolean; +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.behaviors.sequence.actions.*; +import us.ihmc.commons.lists.RecyclingArrayList; +import us.ihmc.commons.thread.Notification; +import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.packets.ExecutionMode; +import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; +import us.ihmc.rdx.imgui.ImBooleanWrapper; +import us.ihmc.rdx.imgui.ImDoubleWrapper; +import us.ihmc.rdx.imgui.ImGuiReferenceFrameLibraryCombo; +import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.rdx.input.ImGui3DViewInput; +import us.ihmc.rdx.mesh.RDXMutableArrowModel; +import us.ihmc.rdx.ui.RDX3DPanelTooltip; +import us.ihmc.rdx.ui.RDXBaseUI; +import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode; +import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo; +import us.ihmc.rdx.ui.gizmo.RDXSelectablePose3DGizmo; +import us.ihmc.rdx.ui.graphics.RDXFootstepGraphic; +import us.ihmc.rdx.ui.widgets.ImGuiFootstepsWidget; +import us.ihmc.rdx.vr.RDXVRContext; +import us.ihmc.robotics.lists.RecyclingArrayListTools; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.tools.io.WorkspaceResourceDirectory; + +public class RDXKickDoorApproachPlanAction extends RDXActionNode +{ + private final DRCRobotModel robotModel; + private final ROS2SyncedRobotModel syncedRobot; + private final KickDoorApproachPlanActionState state; + private final KickDoorApproachPlanActionDefinition definition; + private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final ImBoolean editManuallyPlacedSteps = new ImBoolean(); + private final ImDoubleWrapper swingDurationWidget; + private final ImDoubleWrapper transferDurationWidget; + private int numberOfAllocatedFootsteps = 0; + private final SideDependentList goalFeetGraphics = new SideDependentList<>(); + private final RDX3DPanelTooltip tooltip; + private final ImGuiFootstepsWidget footstepsWidget = new ImGuiFootstepsWidget(); + + public RDXKickDoorApproachPlanAction(long id, + CRDTInfo crdtInfo, + WorkspaceResourceDirectory saveFileDirectory, + RDXBaseUI baseUI, + DRCRobotModel robotModel, + ROS2SyncedRobotModel syncedRobot, + ReferenceFrameLibrary referenceFrameLibrary, + FootstepPlannerParametersBasics footstepPlannerParameters) + { + super(new KickDoorApproachPlanActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); + + state = getState(); + definition = getDefinition(); + + this.robotModel = robotModel; + this.syncedRobot = syncedRobot; + + definition.setName("Footstep plan"); + + + swingDurationWidget = new ImDoubleWrapper(definition::getSwingDuration, + definition::setSwingDuration, + imDouble -> ImGui.inputDouble(labels.get("Swing duration"), imDouble)); + transferDurationWidget = new ImDoubleWrapper(definition::getTransferDuration, + definition::setTransferDuration, + imDouble -> ImGui.inputDouble(labels.get("Transfer duration"), imDouble)); + + + for (RobotSide side : RobotSide.values) + { + RDXFootstepGraphic goalFootGraphic = new RDXFootstepGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints(), side); + goalFootGraphic.create(); + goalFeetGraphics.put(side, goalFootGraphic); + } + + tooltip = new RDX3DPanelTooltip(baseUI.getPrimary3DPanel()); + baseUI.getPrimary3DPanel().addImGuiOverlayAddition(this::render3DPanelImGuiOverlays); + } + + @Override + public void update() + { + super.update(); + + + if (state.areFramesInWorld()) + { + // Add a footstep to the action data only + + + // Update arrow graphic geometry + + goalFeetGraphics.get(RobotSide.LEFT).setPose(state.getLeftFootGoalPose().getValue()); + goalFeetGraphics.get(RobotSide.RIGHT).setPose(state.getRightFootGoalPose().getValue()); + + } + } + + @Override + public void renderTreeViewIconArea() + { + super.renderTreeViewIconArea(); + + footstepsWidget.render(ImGui.getFrameHeight()); + ImGui.sameLine(); + } + + @Override + public void calculateVRPick(RDXVRContext vrContext) + { + } + + @Override + public void processVRInput(RDXVRContext vrContext) + { + } + + @Override + public void calculate3DViewPick(ImGui3DViewInput input) + { + } + + @Override + public void process3DViewInput(ImGui3DViewInput input) + { + + } + + @Override + protected void renderImGuiWidgetsInternal() + { + ImGui.pushItemWidth(80.0f); + swingDurationWidget.renderImGuiWidget(); + transferDurationWidget.renderImGuiWidget(); + ImGui.popItemWidth(); + ImGui.text("Execution mode:"); + ImGui.sameLine(); + if (ImGui.radioButton(labels.get("Override"), definition.getExecutionMode().getValue() == ExecutionMode.OVERRIDE)) + definition.getExecutionMode().setValue(ExecutionMode.OVERRIDE); + ImGui.sameLine(); + if (ImGui.radioButton(labels.get("Queue"), definition.getExecutionMode().getValue() == ExecutionMode.QUEUE)) + definition.getExecutionMode().setValue(ExecutionMode.QUEUE); + + } + + @Override + public void deselectGizmos() + { + editManuallyPlacedSteps.set(false); + } + + public void render3DPanelImGuiOverlays() + { + + } + + @Override + public void getRenderables(Array renderables, Pool pool) + { + if (state.areFramesInWorld()) + { + + + for (RobotSide side : RobotSide.values) + { + goalFeetGraphics.get(side).setHighlighted(footstepsWidget.getIsHovered().get(side)); + goalFeetGraphics.get(side).getRenderables(renderables, pool); + } + } + } + + @Override + public String getActionTypeTitle() + { + return "Kick Door Approach Plan"; + } + + public ImBoolean getEditManuallyPlacedSteps() + { + return editManuallyPlacedSteps; + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java index 8b3020424093..8e6cb2d32b84 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionBuilder.java @@ -2,14 +2,7 @@ import us.ihmc.behaviors.door.DoorTraversalDefinition; import us.ihmc.behaviors.sequence.ActionSequenceDefinition; -import us.ihmc.behaviors.sequence.actions.ChestOrientationActionDefinition; -import us.ihmc.behaviors.sequence.actions.FootstepPlanActionDefinition; -import us.ihmc.behaviors.sequence.actions.HandPoseActionDefinition; -import us.ihmc.behaviors.sequence.actions.HandWrenchActionDefinition; -import us.ihmc.behaviors.sequence.actions.PelvisHeightPitchActionDefinition; -import us.ihmc.behaviors.sequence.actions.SakeHandCommandActionDefinition; -import us.ihmc.behaviors.sequence.actions.ScrewPrimitiveActionDefinition; -import us.ihmc.behaviors.sequence.actions.WaitDurationActionDefinition; +import us.ihmc.behaviors.sequence.actions.*; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.tools.io.WorkspaceResourceDirectory; @@ -37,6 +30,10 @@ public static BehaviorTreeNodeDefinition createNode(Class definitionType, CRD { return new FootstepPlanActionDefinition(crdtInfo, saveFileDirectory); } + if (definitionType == KickDoorApproachPlanActionDefinition.class) + { + return new KickDoorApproachPlanActionDefinition(crdtInfo, saveFileDirectory); + } if (definitionType == HandPoseActionDefinition.class) { return new HandPoseActionDefinition(crdtInfo, saveFileDirectory); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java index 6ca533a7e2a8..a8b31da22e35 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java @@ -84,6 +84,19 @@ public BehaviorTreeExecutorNodeBuilder(DRCRobotModel robotModel, footstepPlanner, footstepPlannerParameters); } + if (nodeType == KickDoorApproachPlanActionDefinition.class) + { + return new KickDoorApproachPlanActionExecutor(id, + crdtInfo, + saveFileDirectory, + ros2ControllerHelper, + syncedRobot, + controllerStatusTracker, + referenceFrameLibrary, + walkingControllerParameters, + footstepPlanner, + footstepPlannerParameters); + } if (nodeType == HandPoseActionDefinition.class) { return new HandPoseActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, referenceFrameLibrary, robotModel, syncedRobot); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java index d29dacd9ca64..0e2d45d32542 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -2,19 +2,16 @@ import behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage; import com.fasterxml.jackson.databind.JsonNode; -import com.fasterxml.jackson.databind.node.ArrayNode; import com.fasterxml.jackson.databind.node.ObjectNode; import us.ihmc.behaviors.sequence.ActionNodeDefinition; -import us.ihmc.commons.MathTools; -import us.ihmc.commons.lists.RecyclingArrayList; -import us.ihmc.communication.crdt.*; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.crdt.CRDTUnidirectionalDouble; +import us.ihmc.communication.crdt.CRDTUnidirectionalEnumField; +import us.ihmc.communication.crdt.CRDTUnidirectionalString; import us.ihmc.communication.packets.ExecutionMode; import us.ihmc.communication.ros2.ROS2ActorDesignation; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.tools.io.JSONTools; import us.ihmc.tools.io.WorkspaceResourceDirectory; public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition @@ -29,12 +26,6 @@ public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition private final CRDTUnidirectionalDouble transferDuration; private final CRDTUnidirectionalEnumField executionMode; private final CRDTUnidirectionalString parentFrameName; - private final CRDTUnidirectionalRecyclingArrayList footsteps; - private final CRDTUnidirectionalPoint3D goalStancePoint; - private final CRDTUnidirectionalPoint3D goalFocalPoint; - private final SideDependentList goalFootstepToGoalXs; - private final SideDependentList goalFootstepToGoalYs; - private final SideDependentList goalFootstepToGoalYaws; private final CRDTUnidirectionalEnumField kickSide; private final CRDTUnidirectionalDouble kickImpulse; @@ -48,12 +39,13 @@ public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition private double onDiskTransferDuration; private ExecutionMode onDiskExecutionMode; private String onDiskParentFrameName; - private int onDiskNumberOfFootsteps; - private final Point3D onDiskGoalStancePoint = new Point3D(); - private final Point3D onDiskGoalFocalPoint = new Point3D(); - private final SideDependentList onDiskGoalFootstepToGoalXs = new SideDependentList<>(() -> 0.0); - private final SideDependentList onDiskGoalFootstepToGoalYs = new SideDependentList<>(() -> 0.0); - private final SideDependentList onDiskGoalFootstepToGoalYaws = new SideDependentList<>(() -> 0.0); + private RobotSide onDiskKickSide; + private double onDiskKickImpulse; + private double onDiskKickTargetDistance; + private double onDiskPrekickWeightDistribution; + private double onDiskHorizontalDistanceFromHandle; + private double onDiskStanceFootWidth; + public KickDoorApproachPlanActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) { @@ -63,14 +55,6 @@ public KickDoorApproachPlanActionDefinition(CRDTInfo crdtInfo, WorkspaceResource transferDuration = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.8); executionMode = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, ExecutionMode.OVERRIDE); parentFrameName = new CRDTUnidirectionalString(ROS2ActorDesignation.OPERATOR, crdtInfo, ReferenceFrame.getWorldFrame().getName()); - footsteps = new CRDTUnidirectionalRecyclingArrayList<>(ROS2ActorDesignation.OPERATOR, - crdtInfo, - () -> new RecyclingArrayList<>(() -> new FootstepPlanActionFootstepDefinition(crdtInfo))); - goalStancePoint = new CRDTUnidirectionalPoint3D(ROS2ActorDesignation.OPERATOR, crdtInfo); - goalFocalPoint = new CRDTUnidirectionalPoint3D(ROS2ActorDesignation.OPERATOR, crdtInfo); - goalFootstepToGoalXs = new SideDependentList<>(() -> new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.0)); - goalFootstepToGoalYs = new SideDependentList<>(() -> new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.0)); - goalFootstepToGoalYaws = new SideDependentList<>(() -> new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, 0.0)); kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); @@ -90,16 +74,12 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("executionMode", executionMode.getValue().name()); jsonNode.put("parentFrame", parentFrameName.getValue()); - JSONTools.toJSON(jsonNode, "goalStancePoint", goalStancePoint.getValueReadOnly()); - JSONTools.toJSON(jsonNode, "goalFocalPoint", goalFocalPoint.getValueReadOnly()); - - for (RobotSide side : RobotSide.values) - { - ObjectNode goalFootNode = jsonNode.putObject(side.getCamelCaseName() + "GoalFootToGoal"); - goalFootNode.put("x", (float) MathTools.roundToPrecision(goalFootstepToGoalXs.get(side).getValue(), 0.0005)); - goalFootNode.put("y", (float) MathTools.roundToPrecision(goalFootstepToGoalYs.get(side).getValue(), 0.0005)); - goalFootNode.put("yawInDegrees", (float) MathTools.roundToPrecision(Math.toDegrees(goalFootstepToGoalYaws.get(side).getValue()), 0.02)); - } + jsonNode.put("kickSide", kickSide.getValue().toString()); + jsonNode.put("kickImpulse", kickImpulse.getValue()); + jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); + jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); + jsonNode.put("horizontalDistanceFromHandle", horizontalDistanceFromHandle.getValue()); + jsonNode.put("stanceFootWidth", stanceFootWidth.getValue()); } @Override @@ -112,18 +92,12 @@ public void loadFromFile(JsonNode jsonNode) executionMode.setValue(ExecutionMode.valueOf(jsonNode.get("executionMode").textValue())); parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); - footsteps.getValue().clear(); - - JSONTools.toEuclid(jsonNode, "goalStancePoint", goalStancePoint.getValue()); - JSONTools.toEuclid(jsonNode, "goalFocalPoint", goalFocalPoint.getValue()); - - for (RobotSide side : RobotSide.values) - { - ObjectNode goalFootNode = (ObjectNode) jsonNode.get(side.getCamelCaseName() + "GoalFootToGoal"); - goalFootstepToGoalXs.get(side).setValue(goalFootNode.get("x").asDouble()); - goalFootstepToGoalYs.get(side).setValue(goalFootNode.get("y").asDouble()); - goalFootstepToGoalYaws.get(side).setValue(Math.toRadians(goalFootNode.get("yawInDegrees").asDouble())); - } + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").asText())); + kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); + kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); + prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); + horizontalDistanceFromHandle.setValue(jsonNode.get("horizontalDistanceFromHandle").asDouble()); + stanceFootWidth.setValue(jsonNode.get("stanceFootWidth").asDouble()); } @Override @@ -135,18 +109,12 @@ public void setOnDiskFields() onDiskTransferDuration = transferDuration.getValue(); onDiskExecutionMode = executionMode.getValue(); onDiskParentFrameName = parentFrameName.getValue(); - onDiskNumberOfFootsteps = footsteps.getSize(); - onDiskGoalStancePoint.set(goalStancePoint.getValueReadOnly()); - onDiskGoalFocalPoint.set(goalFocalPoint.getValueReadOnly()); - for (RobotSide side : goalFootstepToGoalXs.sides()) - { - onDiskGoalFootstepToGoalXs.put(side, goalFootstepToGoalXs.get(side).getValue()); - onDiskGoalFootstepToGoalYs.put(side, goalFootstepToGoalYs.get(side).getValue()); - onDiskGoalFootstepToGoalYaws.put(side, goalFootstepToGoalYaws.get(side).getValue()); - } - - for (int i = 0; i < footsteps.getSize(); i++) - footsteps.getValueReadOnly(i).setOnDiskFields(); + onDiskKickSide = kickSide.getValue(); + onDiskKickImpulse = kickImpulse.getValue(); + onDiskKickTargetDistance = kickTargetDistance.getValue(); + onDiskPrekickWeightDistribution = prekickWeightDistribution.getValue(); + onDiskHorizontalDistanceFromHandle = horizontalDistanceFromHandle.getValue(); + onDiskStanceFootWidth = stanceFootWidth.getValue(); } @Override @@ -158,20 +126,12 @@ public void undoAllNontopologicalChanges() transferDuration.setValue(onDiskTransferDuration); executionMode.setValue(onDiskExecutionMode); parentFrameName.setValue(onDiskParentFrameName); - footsteps.getValue().clear(); - for (int i = 0; i < onDiskNumberOfFootsteps; i++) - footsteps.getValue().add(); - goalStancePoint.getValue().set(onDiskGoalStancePoint); - goalFocalPoint.getValue().set(onDiskGoalFocalPoint); - for (RobotSide side : onDiskGoalFootstepToGoalXs.sides()) - { - goalFootstepToGoalXs.get(side).setValue(onDiskGoalFootstepToGoalXs.get(side)); - goalFootstepToGoalYs.get(side).setValue(onDiskGoalFootstepToGoalYs.get(side)); - goalFootstepToGoalYaws.get(side).setValue(onDiskGoalFootstepToGoalYaws.get(side)); - } - - for (int i = 0; i < footsteps.getSize(); i++) - footsteps.getValue().get(i).undoAllNontopologicalChanges(); + kickSide.setValue(onDiskKickSide); + kickImpulse.setValue(onDiskKickImpulse); + kickTargetDistance.setValue(onDiskKickTargetDistance); + prekickWeightDistribution.setValue(onDiskPrekickWeightDistribution); + horizontalDistanceFromHandle.setValue(onDiskHorizontalDistanceFromHandle); + stanceFootWidth.setValue(onDiskStanceFootWidth); } @Override @@ -183,21 +143,12 @@ public boolean hasChanges() unchanged &= transferDuration.getValue() == onDiskTransferDuration; unchanged &= executionMode.getValue() == onDiskExecutionMode; unchanged &= parentFrameName.getValue().equals(onDiskParentFrameName); - unchanged &= goalStancePoint.getValueReadOnly().equals(onDiskGoalStancePoint); - unchanged &= goalFocalPoint.getValueReadOnly().equals(onDiskGoalFocalPoint); - for (RobotSide side : goalFootstepToGoalXs.sides()) - { - unchanged &= goalFootstepToGoalXs.get(side).getValue() == onDiskGoalFootstepToGoalXs.get(side); - unchanged &= goalFootstepToGoalYs.get(side).getValue() == onDiskGoalFootstepToGoalYs.get(side); - unchanged &= goalFootstepToGoalYaws.get(side).getValue() == onDiskGoalFootstepToGoalYaws.get(side); - } - - boolean sameNumberOfFootsteps = footsteps.getSize() == onDiskNumberOfFootsteps; - unchanged &= sameNumberOfFootsteps; - - if (sameNumberOfFootsteps) - for (int i = 0; i < footsteps.getSize(); i++) - unchanged &= !footsteps.getValueReadOnly(i).hasChanges(); + unchanged &= kickSide.getValue() == onDiskKickSide; + unchanged &= kickImpulse.getValue() == onDiskKickImpulse; + unchanged &= kickTargetDistance.getValue() == onDiskKickTargetDistance; + unchanged &= prekickWeightDistribution.getValue() == onDiskPrekickWeightDistribution; + unchanged &= horizontalDistanceFromHandle.getValue() == onDiskHorizontalDistanceFromHandle; + unchanged &= stanceFootWidth.getValue() == onDiskStanceFootWidth; return !unchanged; } @@ -211,15 +162,12 @@ public void toMessage(KickDoorApproachPlanDefinitionMessage message) message.setExecutionMode(executionMode.toMessageOrdinal()); message.setParentFrameName(parentFrameName.toMessage()); - -// goalStancePoint.toMessage(message.getGoalStancePoint()); -// goalFocalPoint.toMessage(message.getGoalFocalPoint()); -// message.setLeftGoalFootXToGizmo(goalFootstepToGoalXs.get(RobotSide.LEFT).toMessage()); -// message.setLeftGoalFootYToGizmo(goalFootstepToGoalYs.get(RobotSide.LEFT).toMessage()); -// message.setLeftGoalFootYawToGizmo(goalFootstepToGoalYaws.get(RobotSide.LEFT).toMessage()); -// message.setRightGoalFootXToGizmo(goalFootstepToGoalXs.get(RobotSide.RIGHT).toMessage()); -// message.setRightGoalFootYToGizmo(goalFootstepToGoalYs.get(RobotSide.RIGHT).toMessage()); -// message.setRightGoalFootYawToGizmo(goalFootstepToGoalYaws.get(RobotSide.RIGHT).toMessage()); + message.setRobotSide(kickSide.toMessage().toByte()); + message.setKickImpulse(kickImpulse.toMessage()); + message.setKickTargetDistance(kickTargetDistance.toMessage()); + message.setPrekickWeightDistribution(prekickWeightDistribution.toMessage()); + message.setHorizontalDistanceFromHandle(horizontalDistanceFromHandle.toMessage()); + message.setStanceFootWidth(stanceFootWidth.toMessage()); } public void fromMessage(KickDoorApproachPlanDefinitionMessage message) @@ -231,15 +179,12 @@ public void fromMessage(KickDoorApproachPlanDefinitionMessage message) executionMode.fromMessageOrdinal(message.getExecutionMode(), ExecutionMode.values); parentFrameName.fromMessage(message.getParentFrameNameAsString()); - -// goalStancePoint.fromMessage(message.getGoalStancePoint()); -// goalFocalPoint.fromMessage(message.getGoalFocalPoint()); -// goalFootstepToGoalXs.get(RobotSide.LEFT).fromMessage(message.getLeftGoalFootXToGizmo()); -// goalFootstepToGoalYs.get(RobotSide.LEFT).fromMessage(message.getLeftGoalFootYToGizmo()); -// goalFootstepToGoalYaws.get(RobotSide.LEFT).fromMessage(message.getLeftGoalFootYawToGizmo()); -// goalFootstepToGoalXs.get(RobotSide.RIGHT).fromMessage(message.getRightGoalFootXToGizmo()); -// goalFootstepToGoalYs.get(RobotSide.RIGHT).fromMessage(message.getRightGoalFootYToGizmo()); -// goalFootstepToGoalYaws.get(RobotSide.RIGHT).fromMessage(message.getRightGoalFootYawToGizmo()); + kickSide.setValue(RobotSide.fromByte(message.getRobotSide())); + kickImpulse.setValue(message.getKickImpulse()); + kickTargetDistance.setValue(message.getKickTargetDistance()); + prekickWeightDistribution.setValue(message.getPrekickWeightDistribution()); + horizontalDistanceFromHandle.setValue(message.getHorizontalDistanceFromHandle()); + stanceFootWidth.setValue(message.getStanceFootWidth()); } public double getSwingDuration() @@ -311,11 +256,4 @@ public CRDTUnidirectionalDouble getStanceFootWidth() { return stanceFootWidth; } - - - public CRDTUnidirectionalRecyclingArrayList getFootsteps() - { - return footsteps; - } - } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 2d1c87486fea..83cca9b81248 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -15,15 +15,8 @@ import us.ihmc.commons.exception.DefaultExceptionHandler; import us.ihmc.commons.thread.TypedNotification; import us.ihmc.communication.crdt.CRDTInfo; -import us.ihmc.euclid.Axis3D; -import us.ihmc.euclid.geometry.Plane3D; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.matrix.RotationMatrix; -import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.footstepPlanning.*; import us.ihmc.footstepPlanning.graphSearch.graph.visualization.BipedalFootstepPlannerNodeRejectionReason; import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java index b138e0c476d2..da921454d236 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java @@ -1,16 +1,10 @@ package us.ihmc.behaviors.sequence.actions; -import behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessage; -import behavior_msgs.msg.dds.FootstepPlanActionStateMessage; import behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage; import us.ihmc.behaviors.sequence.ActionNodeState; -import us.ihmc.commons.lists.RecyclingArrayList; import us.ihmc.communication.crdt.*; import us.ihmc.communication.ros2.ROS2ActorDesignation; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.robotics.lists.RecyclingArrayListTools; -import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; @@ -20,11 +14,8 @@ public class KickDoorApproachPlanActionState extends ActionNodeState footsteps; private final CRDTUnidirectionalPose3D leftFootGoalPose; private final CRDTUnidirectionalPose3D rightFootGoalPose; - private final SideDependentList goalFootstepToGoalTransforms = new SideDependentList<>(() -> new RigidBodyTransform()); private final ReferenceFrame parentFrame; private final CRDTUnidirectionalInteger totalNumberOfFootsteps; private final CRDTUnidirectionalInteger numberOfIncompleteFootsteps; @@ -44,10 +35,6 @@ public KickDoorApproachPlanActionState(long id, CRDTInfo crdtInfo, WorkspaceReso leftFootGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); rightFootGoalPose = new CRDTUnidirectionalPose3D(ROS2ActorDesignation.ROBOT, crdtInfo); - footsteps = new RecyclingArrayList<>(() -> - new FootstepPlanActionFootstepState(referenceFrameLibrary, - definition.getCRDTParentFrameName(), - RecyclingArrayListTools.getUnsafe(definition.getFootsteps().getValueUnsafe(), numberOfAllocatedFootsteps++))); totalNumberOfFootsteps = new CRDTUnidirectionalInteger(ROS2ActorDesignation.ROBOT, crdtInfo, 0); numberOfIncompleteFootsteps = new CRDTUnidirectionalInteger(ROS2ActorDesignation.ROBOT, crdtInfo, 0); for (RobotSide side : RobotSide.values) @@ -61,19 +48,6 @@ public KickDoorApproachPlanActionState(long id, CRDTInfo crdtInfo, WorkspaceReso @Override public void update() { - for (RobotSide side : RobotSide.values) - { - goalFootstepToGoalTransforms.get(side).getTranslation().setZ(0.0); - goalFootstepToGoalTransforms.get(side).getRotation().setYawPitchRoll(goalFootstepToGoalTransforms.get(side).getRotation().getYaw(), 0.0, 0.0); - } - - RecyclingArrayListTools.synchronizeSize(footsteps, definition.getFootsteps().getSize()); - - for (int i = 0; i < footsteps.size(); i++) - { - footsteps.get(i).setIndex(i); - footsteps.get(i).update(); - } } public void toMessage(KickDoorApproachPlanStateMessage message) @@ -91,11 +65,6 @@ public void toMessage(KickDoorApproachPlanStateMessage message) currentFootPoses.get(RobotSide.LEFT).toMessage(message.getCurrentLeftFootPose()); currentFootPoses.get(RobotSide.RIGHT).toMessage(message.getCurrentRightFootPose()); - message.getFootsteps().clear(); - for (FootstepPlanActionFootstepState footstep : footsteps) - { - footstep.toMessage(message.getFootsteps().add()); - } message.setExecutionState(executionState.toMessage().toByte()); } @@ -115,12 +84,6 @@ public void fromMessage(KickDoorApproachPlanStateMessage message) currentFootPoses.get(RobotSide.LEFT).fromMessage(message.getCurrentLeftFootPose()); currentFootPoses.get(RobotSide.RIGHT).fromMessage(message.getCurrentRightFootPose()); - footsteps.clear(); - for (FootstepPlanActionFootstepStateMessage footstep : message.getFootsteps()) - { - footsteps.add().fromMessage(footstep); - } - executionState.fromMessage(FootstepPlanActionExecutionState.fromByte(message.getExecutionState())); } @@ -134,11 +97,6 @@ public ReferenceFrame getParentFrame() return parentFrame; } - public RecyclingArrayList getFootsteps() - { - return footsteps; - } - public int getTotalNumberOfFootsteps() { return totalNumberOfFootsteps.getValue(); diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl index c30dde9e8f82..463a1b768bc8 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorApproachPlanStateMessage_.idl @@ -2,7 +2,6 @@ #define __behavior_msgs__msg__KickDoorApproachPlanStateMessage__idl__ #include "behavior_msgs/msg/./ActionNodeStateMessage_.idl" -#include "behavior_msgs/msg/./FootstepPlanActionFootstepStateMessage_.idl" #include "behavior_msgs/msg/./KickDoorApproachPlanDefinitionMessage_.idl" #include "geometry_msgs/msg/./Pose_.idl" #include "ihmc_common_msgs/msg/./SE3TrajectoryPointMessage_.idl" @@ -36,10 +35,6 @@ module behavior_msgs * Definition */ behavior_msgs::msg::dds::KickDoorApproachPlanDefinitionMessage definition; - /** - * The footsteps, with a maximum of 50 - */ - sequence footsteps; /** * Left foot goal pose */ diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java index e24f78639d45..6f314a57e708 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessage.java @@ -20,10 +20,6 @@ public class KickDoorApproachPlanStateMessage extends Packet footsteps_; /** * Left foot goal pose */ @@ -65,7 +61,6 @@ public KickDoorApproachPlanStateMessage() { state_ = new behavior_msgs.msg.dds.ActionNodeStateMessage(); definition_ = new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage(); - footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessagePubSubType()); left_foot_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); right_foot_goal_pose_ = new us.ihmc.euclid.geometry.Pose3D(); desired_left_footsteps_ = new us.ihmc.idl.IDLSequence.Object (100, new ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType()); @@ -85,7 +80,6 @@ public void set(KickDoorApproachPlanStateMessage other) { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.staticCopy(other.state_, state_); behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); - footsteps_.set(other.footsteps_); geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.left_foot_goal_pose_, left_foot_goal_pose_); geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.right_foot_goal_pose_, right_foot_goal_pose_); execution_state_ = other.execution_state_; @@ -119,15 +113,6 @@ public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage getDefinition } - /** - * The footsteps, with a maximum of 50 - */ - public us.ihmc.idl.IDLSequence.Object getFootsteps() - { - return footsteps_; - } - - /** * Left foot goal pose */ @@ -246,13 +231,6 @@ public boolean epsilonEquals(KickDoorApproachPlanStateMessage other, double epsi if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; - if (this.footsteps_.size() != other.footsteps_.size()) { return false; } - else - { - for (int i = 0; i < this.footsteps_.size(); i++) - { if (!this.footsteps_.get(i).epsilonEquals(other.footsteps_.get(i), epsilon)) return false; } - } - if (!this.left_foot_goal_pose_.epsilonEquals(other.left_foot_goal_pose_, epsilon)) return false; if (!this.right_foot_goal_pose_.epsilonEquals(other.right_foot_goal_pose_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_state_, other.execution_state_, epsilon)) return false; @@ -292,7 +270,6 @@ public boolean equals(Object other) if (!this.state_.equals(otherMyClass.state_)) return false; if (!this.definition_.equals(otherMyClass.definition_)) return false; - if (!this.footsteps_.equals(otherMyClass.footsteps_)) return false; if (!this.left_foot_goal_pose_.equals(otherMyClass.left_foot_goal_pose_)) return false; if (!this.right_foot_goal_pose_.equals(otherMyClass.right_foot_goal_pose_)) return false; if(this.execution_state_ != otherMyClass.execution_state_) return false; @@ -319,8 +296,6 @@ public java.lang.String toString() builder.append(this.state_); builder.append(", "); builder.append("definition="); builder.append(this.definition_); builder.append(", "); - builder.append("footsteps="); - builder.append(this.footsteps_); builder.append(", "); builder.append("left_foot_goal_pose="); builder.append(this.left_foot_goal_pose_); builder.append(", "); builder.append("right_foot_goal_pose="); diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java index cae570dc9d08..fe90aab72acf 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java @@ -15,7 +15,7 @@ public class KickDoorApproachPlanStateMessagePubSubType implements us.ihmc.pubsu @Override public final java.lang.String getDefinitionChecksum() { - return "02cbee0a296afe41ffd8e462465f1c0a8c85d3293d8ba89f7a7d4d59ba918691"; + return "61a537c9444792482ecd68daa073eccc3b8255b17eee37ec3f1792e6a77ef49f"; } @Override @@ -56,9 +56,6 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0) - { - current_alignment += behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); @@ -96,11 +93,6 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorAppro current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); - for(int i0 = 0; i0 < data.getFootsteps().size(); ++i0) - { - current_alignment += behavior_msgs.msg.dds.FootstepPlanActionFootstepStateMessagePubSubType.getCdrSerializedSize(data.getFootsteps().get(i0), current_alignment);} - current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getLeftFootGoalPose(), current_alignment); current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getRightFootGoalPose(), current_alignment); @@ -136,10 +128,6 @@ public static void write(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.write(data.getDefinition(), cdr); - if(data.getFootsteps().size() <= 100) - cdr.write_type_e(data.getFootsteps());else - throw new RuntimeException("footsteps field exceeds the maximum length"); - geometry_msgs.msg.dds.PosePubSubType.write(data.getLeftFootGoalPose(), cdr); geometry_msgs.msg.dds.PosePubSubType.write(data.getRightFootGoalPose(), cdr); cdr.write_type_9(data.getExecutionState()); @@ -164,7 +152,6 @@ public static void read(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage d { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.read(data.getDefinition(), cdr); - cdr.read_type_e(data.getFootsteps()); geometry_msgs.msg.dds.PosePubSubType.read(data.getLeftFootGoalPose(), cdr); geometry_msgs.msg.dds.PosePubSubType.read(data.getRightFootGoalPose(), cdr); data.setExecutionState(cdr.read_type_9()); @@ -187,7 +174,6 @@ public final void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessa ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); - ser.write_type_e("footsteps", data.getFootsteps()); ser.write_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); ser.write_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); @@ -210,7 +196,6 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); - ser.read_type_e("footsteps", data.getFootsteps()); ser.read_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); ser.read_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg index 1f2519989f94..6e05fda31a06 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -9,9 +9,6 @@ behavior_msgs/ActionNodeStateMessage state # Definition behavior_msgs/KickDoorApproachPlanDefinitionMessage definition -# The footsteps, with a maximum of 50 -behavior_msgs/FootstepPlanActionFootstepStateMessage[] footsteps - # Left foot goal pose geometry_msgs/Pose left_foot_goal_pose diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg index 91ac6b0cb414..bf00c294f3a9 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -13,9 +13,6 @@ behavior_msgs/ActionNodeStateMessage state # Definition behavior_msgs/KickDoorApproachPlanDefinitionMessage definition -# The footsteps, with a maximum of 50 -behavior_msgs/FootstepPlanActionFootstepStateMessage[] footsteps - # Left foot goal pose geometry_msgs/Pose left_foot_goal_pose From da31411a3323a964444bc44b475209625deb90e6 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Wed, 10 Apr 2024 00:25:22 -0500 Subject: [PATCH 13/32] got the behavior running, but not correctly --- .../RDXKickDoorApproachPlanAction.java | 4 ++-- .../tree/RDXBehaviorTreeNodeBuilder.java | 11 ++++++++++ .../BehaviorTreeDefinitionRegistry.java | 3 ++- .../actions/KickDoorActionDefinition.java | 2 +- .../KickDoorApproachPlanActionDefinition.java | 4 ++-- .../msg/BehaviorTreeStateMessage_.idl | 5 +++++ .../msg/dds/BehaviorTreeStateMessage.java | 22 ++++++++++++++++++- .../BehaviorTreeStateMessagePubSubType.java | 17 +++++++++++++- .../msg/BehaviorTreeStateMessage.msg | 3 +++ .../msg/BehaviorTreeStateMessage.msg | 4 ++++ 10 files changed, 67 insertions(+), 8 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java index 84af92b0ce67..66b80422aa34 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java @@ -107,8 +107,8 @@ public void update() // Update arrow graphic geometry - goalFeetGraphics.get(RobotSide.LEFT).setPose(state.getLeftFootGoalPose().getValue()); - goalFeetGraphics.get(RobotSide.RIGHT).setPose(state.getRightFootGoalPose().getValue()); + goalFeetGraphics.get(RobotSide.LEFT).setPose(state.getLeftFootGoalPose().getValueReadOnly()); + goalFeetGraphics.get(RobotSide.RIGHT).setPose(state.getRightFootGoalPose().getValueReadOnly()); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java index 559906411055..846074a98720 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java @@ -139,6 +139,17 @@ public RDXBehaviorTreeNodeBuilder(DRCRobotModel robotModel, referenceFrameLibrary, footstepPlannerParametersBasics); } + if (nodeType == KickDoorApproachPlanActionDefinition.class) + { + return new RDXKickDoorApproachPlanAction(id, + crdtInfo, + saveFileDirectory, + baseUI, + robotModel, + syncedRobot, + referenceFrameLibrary, + footstepPlannerParametersBasics); + } else { return null; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java index e4b7b8e3c3a3..69fae57d8196 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java @@ -23,7 +23,8 @@ private record RegistryRecord(Class typeClass, byte messageByte) { } new RegistryRecord(PelvisHeightPitchActionDefinition.class, BehaviorTreeStateMessage.PELVIS_HEIGHT_PITCH_ACTION), new RegistryRecord(SakeHandCommandActionDefinition.class, BehaviorTreeStateMessage.SAKE_HAND_COMMAND_ACTION), new RegistryRecord(WaitDurationActionDefinition.class, BehaviorTreeStateMessage.WAIT_DURATION_ACTION), - new RegistryRecord(KickDoorActionDefinition.class, BehaviorTreeStateMessage.KICK_DOOR_ACTION) + new RegistryRecord(KickDoorActionDefinition.class, BehaviorTreeStateMessage.KICK_DOOR_ACTION), + new RegistryRecord(KickDoorApproachPlanActionDefinition.class, BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION) }; public static Class getClassFromTypeName(String typeName) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java index ed712b3a8fb9..530d2bb7a46b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -59,7 +59,7 @@ public void loadFromFile(JsonNode jsonNode) { super.loadFromFile(jsonNode); - kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("side").asText())); + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").textValue())); kickHeight.setValue(jsonNode.get("kickHeight").asDouble()); kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java index 0e2d45d32542..3f148830290e 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -74,12 +74,12 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("executionMode", executionMode.getValue().name()); jsonNode.put("parentFrame", parentFrameName.getValue()); - jsonNode.put("kickSide", kickSide.getValue().toString()); jsonNode.put("kickImpulse", kickImpulse.getValue()); jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); jsonNode.put("horizontalDistanceFromHandle", horizontalDistanceFromHandle.getValue()); jsonNode.put("stanceFootWidth", stanceFootWidth.getValue()); + jsonNode.put("kickSide", kickSide.getValue().toString()); } @Override @@ -92,12 +92,12 @@ public void loadFromFile(JsonNode jsonNode) executionMode.setValue(ExecutionMode.valueOf(jsonNode.get("executionMode").textValue())); parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); - kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").asText())); kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); horizontalDistanceFromHandle.setValue(jsonNode.get("horizontalDistanceFromHandle").asDouble()); stanceFootWidth.setValue(jsonNode.get("stanceFootWidth").asDouble()); + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").textValue())); } @Override diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl index aed5593bcd36..3c4312ea267f 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl @@ -9,6 +9,7 @@ #include "behavior_msgs/msg/./HandPoseActionStateMessage_.idl" #include "behavior_msgs/msg/./HandWrenchActionStateMessage_.idl" #include "behavior_msgs/msg/./KickDoorActionStateMessage_.idl" +#include "behavior_msgs/msg/./KickDoorApproachPlanStateMessage_.idl" #include "behavior_msgs/msg/./PelvisHeightPitchActionStateMessage_.idl" #include "behavior_msgs/msg/./SakeHandCommandActionStateMessage_.idl" #include "behavior_msgs/msg/./ScrewPrimitiveActionStateMessage_.idl" @@ -56,6 +57,9 @@ module behavior_msgs const octet KICK_DOOR_ACTION = 18; + const octet KICK_DOOR_APPROACH_ACTION = + 19; + /** * Gives the current state of the complete collection of behavior tree nodes. @@ -100,6 +104,7 @@ module behavior_msgs sequence pelvis_height_actions; sequence wait_duration_actions; sequence kick_door_actions; + sequence kick_door_approach_actions; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java index 6489033cc036..92fb3ecf46da 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java @@ -26,6 +26,7 @@ public class BehaviorTreeStateMessage extends Packet i public static final byte PELVIS_HEIGHT_PITCH_ACTION = (byte) 16; public static final byte WAIT_DURATION_ACTION = (byte) 17; public static final byte KICK_DOOR_ACTION = (byte) 18; + public static final byte KICK_DOOR_APPROACH_ACTION = (byte) 19; /** * Monotonically increasing message ID that matches the CRDTInfo update number */ @@ -60,6 +61,7 @@ public class BehaviorTreeStateMessage extends Packet i public us.ihmc.idl.IDLSequence.Object pelvis_height_actions_; public us.ihmc.idl.IDLSequence.Object wait_duration_actions_; public us.ihmc.idl.IDLSequence.Object kick_door_actions_; + public us.ihmc.idl.IDLSequence.Object kick_door_approach_actions_; public BehaviorTreeStateMessage() { @@ -80,6 +82,7 @@ public BehaviorTreeStateMessage() pelvis_height_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.PelvisHeightPitchActionStateMessagePubSubType()); wait_duration_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.WaitDurationActionStateMessagePubSubType()); kick_door_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType()); + kick_door_approach_actions_ = new us.ihmc.idl.IDLSequence.Object (200, new behavior_msgs.msg.dds.KickDoorApproachPlanStateMessagePubSubType()); } @@ -110,6 +113,7 @@ public void set(BehaviorTreeStateMessage other) pelvis_height_actions_.set(other.pelvis_height_actions_); wait_duration_actions_.set(other.wait_duration_actions_); kick_door_actions_.set(other.kick_door_actions_); + kick_door_approach_actions_.set(other.kick_door_approach_actions_); } /** @@ -244,6 +248,12 @@ public us.ihmc.idl.IDLSequence.Object getKickDoorApproachActions() + { + return kick_door_approach_actions_; + } + + public static Supplier getPubSubType() { return BehaviorTreeStateMessagePubSubType::new; @@ -354,6 +364,13 @@ public boolean epsilonEquals(BehaviorTreeStateMessage other, double epsilon) { if (!this.kick_door_actions_.get(i).epsilonEquals(other.kick_door_actions_.get(i), epsilon)) return false; } } + if (this.kick_door_approach_actions_.size() != other.kick_door_approach_actions_.size()) { return false; } + else + { + for (int i = 0; i < this.kick_door_approach_actions_.size(); i++) + { if (!this.kick_door_approach_actions_.get(i).epsilonEquals(other.kick_door_approach_actions_.get(i), epsilon)) return false; } + } + return true; } @@ -386,6 +403,7 @@ public boolean equals(Object other) if (!this.pelvis_height_actions_.equals(otherMyClass.pelvis_height_actions_)) return false; if (!this.wait_duration_actions_.equals(otherMyClass.wait_duration_actions_)) return false; if (!this.kick_door_actions_.equals(otherMyClass.kick_door_actions_)) return false; + if (!this.kick_door_approach_actions_.equals(otherMyClass.kick_door_approach_actions_)) return false; return true; } @@ -429,7 +447,9 @@ public java.lang.String toString() builder.append("wait_duration_actions="); builder.append(this.wait_duration_actions_); builder.append(", "); builder.append("kick_door_actions="); - builder.append(this.kick_door_actions_); + builder.append(this.kick_door_actions_); builder.append(", "); + builder.append("kick_door_approach_actions="); + builder.append(this.kick_door_approach_actions_); builder.append("}"); return builder.toString(); } 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 bce2360c9019..d74c8f55127f 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 "2c899d203e51bc91b3593b5f091683e6bb453c1db1ed5ad17ef43ad8feb09db3"; + return "eb09f0c4d897abcb5ad3b672feea55721fb178e81516ba82174c14a29864bcb0"; } @Override @@ -98,6 +98,9 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) { current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 200; ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanStateMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} return current_alignment - initial_alignment; } @@ -187,6 +190,11 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeS { current_alignment += behavior_msgs.msg.dds.KickDoorActionStateMessagePubSubType.getCdrSerializedSize(data.getKickDoorActions().get(i0), current_alignment);} + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + for(int i0 = 0; i0 < data.getKickDoorApproachActions().size(); ++i0) + { + current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanStateMessagePubSubType.getCdrSerializedSize(data.getKickDoorApproachActions().get(i0), current_alignment);} + return current_alignment - initial_alignment; } @@ -254,6 +262,10 @@ public static void write(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us cdr.write_type_e(data.getKickDoorActions());else throw new RuntimeException("kick_door_actions field exceeds the maximum length"); + if(data.getKickDoorApproachActions().size() <= 200) + cdr.write_type_e(data.getKickDoorApproachActions());else + throw new RuntimeException("kick_door_approach_actions field exceeds the maximum length"); + } public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us.ihmc.idl.CDR cdr) @@ -277,6 +289,7 @@ public static void read(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, us. cdr.read_type_e(data.getPelvisHeightActions()); cdr.read_type_e(data.getWaitDurationActions()); cdr.read_type_e(data.getKickDoorActions()); + cdr.read_type_e(data.getKickDoorApproachActions()); } @@ -301,6 +314,7 @@ public final void serialize(behavior_msgs.msg.dds.BehaviorTreeStateMessage data, ser.write_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.write_type_e("wait_duration_actions", data.getWaitDurationActions()); ser.write_type_e("kick_door_actions", data.getKickDoorActions()); + ser.write_type_e("kick_door_approach_actions", data.getKickDoorApproachActions()); } @Override @@ -324,6 +338,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_e("pelvis_height_actions", data.getPelvisHeightActions()); ser.read_type_e("wait_duration_actions", data.getWaitDurationActions()); ser.read_type_e("kick_door_actions", data.getKickDoorActions()); + ser.read_type_e("kick_door_approach_actions", data.getKickDoorApproachActions()); } public static void staticCopy(behavior_msgs.msg.dds.BehaviorTreeStateMessage src, behavior_msgs.msg.dds.BehaviorTreeStateMessage dest) diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg index 9a47eee3f32b..10960bab3cff 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg @@ -16,6 +16,7 @@ byte SCREW_PRIMITIVE_ACTION = 15 byte PELVIS_HEIGHT_PITCH_ACTION = 16 byte WAIT_DURATION_ACTION = 17 byte KICK_DOOR_ACTION = 18 +byte KICK_DOOR_APPROACH_ACTION = 19 # Monotonically increasing message ID that matches the CRDTInfo update number uint32 sequence_id @@ -57,3 +58,5 @@ behavior_msgs/PelvisHeightPitchActionStateMessage[<=200] pelvis_height_actions behavior_msgs/WaitDurationActionStateMessage[<=200] wait_duration_actions behavior_msgs/KickDoorActionStateMessage[<=200] kick_door_actions + +behavior_msgs/KickDoorApproachPlanStateMessage[<=200] kick_door_approach_actions diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg index 229f247d5eca..19d53e666db0 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg @@ -27,6 +27,8 @@ int8 WAIT_DURATION_ACTION=17 int8 KICK_DOOR_ACTION=18 +int8 KICK_DOOR_APPROACH_ACTION=19 + # Monotonically increasing message ID that matches the CRDTInfo update number uint32 sequence_id @@ -68,4 +70,6 @@ behavior_msgs/WaitDurationActionStateMessage[] wait_duration_actions behavior_msgs/KickDoorActionStateMessage[] kick_door_actions +behavior_msgs/KickDoorApproachPlanStateMessage[] kick_door_approach_actions + From d3b4fcbc1388d9ea3dff80f79f825a3d4bacec36 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Wed, 10 Apr 2024 12:02:01 -0500 Subject: [PATCH 14/32] Added some missing changes to accommodate the new behavior. Still need to get the move action working and add the contents of the kick door action. --- .../ros2/ROS2BehaviorTreeMessageTools.java | 17 +++++++++ .../ROS2BehaviorTreeSubscriptionNode.java | 12 ++++++ .../sequence/ActionNodeInitialization.java | 25 ++++++++++++ .../actions/KickDoorActionDefinition.java | 30 +++++++++++++++ .../sequence/actions/KickDoorActionState.java | 21 +++++++++- .../KickDoorApproachPlanActionDefinition.java | 26 +++++++++---- .../KickDoorApproachPlanActionExecutor.java | 7 ++-- .../msg/KickDoorActionDefinitionMessage_.idl | 4 ++ .../BehaviorTreeStateMessagePubSubType.java | 2 +- .../dds/KickDoorActionDefinitionMessage.java | 38 +++++++++++++++++++ ...DoorActionDefinitionMessagePubSubType.java | 12 +++++- .../KickDoorActionStateMessagePubSubType.java | 2 +- .../msg/KickDoorActionDefinitionMessage.msg | 3 ++ .../msg/KickDoorActionDefinitionMessage.msg | 3 ++ 14 files changed, 188 insertions(+), 14 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index e7651564f746..aeed4d7a972c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -93,6 +93,12 @@ else if (nodeState instanceof WaitDurationActionState waitDurationActionState) treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getWaitDurationActions().size()); waitDurationActionState.toMessage(treeStateMessage.getWaitDurationActions().add()); } + else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) + { + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION); + treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); + kickDoorApproachPlanActionState.toMessage(treeStateMessage.getKickDoorApproachActions().add()); + } else if (nodeState instanceof KickDoorActionState kickDoorActionState) { treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_ACTION); @@ -151,6 +157,10 @@ else if (nodeState instanceof WaitDurationActionState waitDurationActionState) { waitDurationActionState.fromMessage(subscriptionNode.getWaitDurationActionStateMessage()); } + else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) + { + kickDoorApproachPlanActionState.fromMessage(subscriptionNode.getKickDoorApproachPlanStateMessage()); + } else if (nodeState instanceof KickDoorActionState kickDoorActionState) { kickDoorActionState.fromMessage(subscriptionNode.getKickDoorActionStateMessage()); @@ -244,6 +254,13 @@ public static void packSubscriptionNode(byte nodeType, subscriptionNode.setBehaviorTreeNodeStateMessage(waitDurationActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(waitDurationActionStateMessage.getDefinition().getDefinition().getDefinition()); } + case BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION -> + { + KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage = treeStateMessage.getKickDoorApproachActions().get(indexInTypesList); + subscriptionNode.setKickDoorApproachPlanStateMessage(kickDoorApproachPlanStateMessage); + subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorApproachPlanStateMessage.getState().getState()); + subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorApproachPlanStateMessage.getDefinition().getDefinition().getDefinition()); + } case BehaviorTreeStateMessage.KICK_DOOR_ACTION -> { KickDoorActionStateMessage kickDoorActionStateMessage = treeStateMessage.getKickDoorActions().get(indexInTypesList); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java index 0c2ab795ccfb..21fb6440f8f5 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java @@ -21,6 +21,7 @@ public class ROS2BehaviorTreeSubscriptionNode private ScrewPrimitiveActionStateMessage screwPrimitiveActionStateMessage; private PelvisHeightPitchActionStateMessage pelvisHeightPitchActionStateMessage; private WaitDurationActionStateMessage waitDurationActionStateMessage; + private KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage; private KickDoorActionStateMessage kickDoorActionStateMessage; private final List children = new ArrayList<>(); @@ -40,6 +41,7 @@ public void clear() screwPrimitiveActionStateMessage = null; pelvisHeightPitchActionStateMessage = null; waitDurationActionStateMessage = null; + kickDoorApproachPlanStateMessage = null; kickDoorActionStateMessage = null; children.clear(); } @@ -184,6 +186,16 @@ public void setWaitDurationActionStateMessage(WaitDurationActionStateMessage wai this.waitDurationActionStateMessage = waitDurationActionStateMessage; } + public KickDoorApproachPlanStateMessage getKickDoorApproachPlanStateMessage() + { + return kickDoorApproachPlanStateMessage; + } + + public void setKickDoorApproachPlanStateMessage(KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage) + { + this.kickDoorApproachPlanStateMessage = kickDoorApproachPlanStateMessage; + } + public KickDoorActionStateMessage getKickDoorActionStateMessage() { return kickDoorActionStateMessage; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java index ed80f0142e54..815d9d227ca9 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/ActionNodeInitialization.java @@ -52,8 +52,33 @@ else if (newAction instanceof ScrewPrimitiveActionState screwPrimitiveAction) .setObjectFrameName(findConvenientParentFrameName(actionSequence, HandPoseActionState.class, indexOfInsertion, sideOfNewAction)); screwPrimitiveAction.getState().update(); } + else if ( newAction instanceof KickDoorApproachPlanActionState kickDoorApproachPlanAction) + { + KickDoorApproachPlanActionState nextPreviousFootstepPlanAction = findNextPreviousAction(actionSequence, KickDoorApproachPlanActionState.class, indexOfInsertion, null); + if (nextPreviousFootstepPlanAction != null) + { + kickDoorApproachPlanAction.getDefinition().setParentFrameName(nextPreviousFootstepPlanAction.getDefinition().getParentFrameName()); + } + else + { + kickDoorApproachPlanAction.getDefinition().setParentFrameName(syncedRobot.getReferenceFrames().getMidFeetUnderPelvisFrame().getName()); + } + + kickDoorApproachPlanAction.getDefinition().setSide(sideOfNewAction); + kickDoorApproachPlanAction.update(); + } else if ( newAction instanceof KickDoorActionState kickDoorAction) { + KickDoorActionState nextPreviousFootstepPlanAction = findNextPreviousAction(actionSequence, KickDoorActionState.class, indexOfInsertion, null); + if (nextPreviousFootstepPlanAction != null) + { + kickDoorAction.getDefinition().setParentFrameName(nextPreviousFootstepPlanAction.getDefinition().getParentFrameName()); + } + else + { + kickDoorAction.getDefinition().setParentFrameName(syncedRobot.getReferenceFrames().getMidFeetUnderPelvisFrame().getName()); + } + kickDoorAction.getDefinition().setSide(sideOfNewAction); kickDoorAction.update(); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java index 530d2bb7a46b..23e6b579ac62 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -6,6 +6,7 @@ import us.ihmc.behaviors.sequence.ActionNodeDefinition; import us.ihmc.communication.crdt.*; import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SidedObject; import us.ihmc.tools.io.WorkspaceResourceDirectory; @@ -17,6 +18,8 @@ public class KickDoorActionDefinition extends ActionNodeDefinition implements Si public static final double KICK_TARGET_DISTANCE = 0.75; public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; + private final CRDTUnidirectionalString parentFrameName; + private final CRDTUnidirectionalEnumField kickSide; private final CRDTUnidirectionalDouble kickHeight; private final CRDTUnidirectionalDouble kickImpulse; @@ -24,6 +27,7 @@ public class KickDoorActionDefinition extends ActionNodeDefinition implements Si private final CRDTUnidirectionalDouble prekickWeightDistribution; // On disk fields + private String onDiskParentFrameName; private RobotSide onDiskSide; private double onDiskKickHeight; private double onDiskKickImpulse; @@ -34,6 +38,8 @@ public KickDoorActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory sa { super(crdtInfo, saveFileDirectory); + parentFrameName = new CRDTUnidirectionalString(ROS2ActorDesignation.OPERATOR, crdtInfo, ReferenceFrame.getWorldFrame().getName()); + kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); kickHeight = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_HEIGHT); kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); @@ -47,6 +53,8 @@ public void saveToFile(ObjectNode jsonNode) { super.saveToFile(jsonNode); + jsonNode.put("parentFrame", parentFrameName.getValue()); + jsonNode.put("side", kickSide.getValue().getLowerCaseName()); jsonNode.put("kickHeight", kickHeight.getValue()); jsonNode.put("kickImpulse", kickImpulse.getValue()); @@ -59,6 +67,8 @@ public void loadFromFile(JsonNode jsonNode) { super.loadFromFile(jsonNode); + parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); + kickSide.setValue(RobotSide.getSideFromString(jsonNode.get("kickSide").textValue())); kickHeight.setValue(jsonNode.get("kickHeight").asDouble()); kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); @@ -71,6 +81,8 @@ public void setOnDiskFields() { super.setOnDiskFields(); + onDiskParentFrameName = parentFrameName.getValue(); + onDiskSide = kickSide.getValue(); onDiskKickHeight = kickHeight.getValue(); onDiskKickImpulse = kickImpulse.getValue(); @@ -83,6 +95,8 @@ public void undoAllNontopologicalChanges() { super.undoAllNontopologicalChanges(); + parentFrameName.setValue(onDiskParentFrameName); + kickSide.setValue(onDiskSide); kickHeight.setValue(onDiskKickHeight); kickImpulse.setValue(onDiskKickImpulse); @@ -95,6 +109,8 @@ public boolean hasChanges() { boolean unchanged = !super.hasChanges(); + unchanged &= parentFrameName.getValue().equals(onDiskParentFrameName); + unchanged &= kickSide.getValue() == onDiskSide; unchanged &= kickHeight.getValue() == onDiskKickHeight; unchanged &= kickImpulse.getValue() == onDiskKickImpulse; @@ -108,6 +124,8 @@ public void toMessage(KickDoorActionDefinitionMessage message) { super.toMessage(message.getDefinition()); + message.setParentFrameName(parentFrameName.toMessage()); + message.setRobotSide(kickSide.toMessage().toByte()); message.setKickHeight(kickHeight.getValue()); message.setKickImpulse(kickImpulse.getValue()); @@ -119,6 +137,8 @@ public void fromMessage(KickDoorActionDefinitionMessage message) { super.fromMessage(message.getDefinition()); + parentFrameName.fromMessage(message.getParentFrameNameAsString()); + kickSide.fromMessage(RobotSide.fromByte(message.getRobotSide())); kickHeight.fromMessage(message.getKickHeight()); kickImpulse.fromMessage(message.getKickImpulse()); @@ -137,6 +157,16 @@ public void setSide(RobotSide kickSide) this.kickSide.setValue(kickSide); } + public String getParentFrameName() + { + return parentFrameName.getValue(); + } + + public void setParentFrameName(String parentFrameName) + { + this.parentFrameName.setValue(parentFrameName); + } + public double getKickHeight() { return kickHeight.getValue(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java index 4a4d331e7816..80c96d7bf08a 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionState.java @@ -5,18 +5,27 @@ import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.communication.crdt.CRDTUnidirectionalEnumField; import us.ihmc.communication.ros2.ROS2ActorDesignation; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.tools.io.WorkspaceResourceDirectory; public class KickDoorActionState extends ActionNodeState { - + private final ReferenceFrame parentFrame; + private final KickDoorActionDefinition definition; + private final ReferenceFrameLibrary referenceFrameLibrary; private CRDTUnidirectionalEnumField executionState; public KickDoorActionState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, ReferenceFrameLibrary referenceFrameLibrary) { super(id, new KickDoorActionDefinition(crdtInfo, saveFileDirectory), crdtInfo); + definition = getDefinition(); + + this.referenceFrameLibrary = referenceFrameLibrary; + + parentFrame = referenceFrameLibrary.findFrameByName(definition.getParentFrameName()); + executionState = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.ROBOT, crdtInfo, KickDoorActionExecutionState.STANDING); } @@ -43,6 +52,16 @@ public void fromMessage(KickDoorActionStateMessage message) executionState.fromMessage(KickDoorActionExecutionState.fromByte(message.getExecutionState())); } + public boolean areFramesInWorld() + { + return referenceFrameLibrary.containsFrame(definition.getParentFrameName()) && parentFrame.getRootFrame() == ReferenceFrame.getWorldFrame(); + } + + public ReferenceFrame getParentFrame() + { + return parentFrame; + } + public CRDTUnidirectionalEnumField getExecutionState() { return executionState; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java index 3f148830290e..c2a5b9086873 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -12,9 +12,10 @@ import us.ihmc.communication.ros2.ROS2ActorDesignation; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SidedObject; import us.ihmc.tools.io.WorkspaceResourceDirectory; -public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition +public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition implements SidedObject { public static final double KICK_IMPULSE = 55.0; public static final double KICK_TARGET_DISTANCE = 0.75; @@ -179,12 +180,23 @@ public void fromMessage(KickDoorApproachPlanDefinitionMessage message) executionMode.fromMessageOrdinal(message.getExecutionMode(), ExecutionMode.values); parentFrameName.fromMessage(message.getParentFrameNameAsString()); - kickSide.setValue(RobotSide.fromByte(message.getRobotSide())); - kickImpulse.setValue(message.getKickImpulse()); - kickTargetDistance.setValue(message.getKickTargetDistance()); - prekickWeightDistribution.setValue(message.getPrekickWeightDistribution()); - horizontalDistanceFromHandle.setValue(message.getHorizontalDistanceFromHandle()); - stanceFootWidth.setValue(message.getStanceFootWidth()); + kickSide.fromMessage(RobotSide.fromByte(message.getRobotSide())); + kickImpulse.fromMessage(message.getKickImpulse()); + kickTargetDistance.fromMessage(message.getKickTargetDistance()); + prekickWeightDistribution.fromMessage(message.getPrekickWeightDistribution()); + horizontalDistanceFromHandle.fromMessage(message.getHorizontalDistanceFromHandle()); + stanceFootWidth.fromMessage(message.getStanceFootWidth()); + } + + @Override + public RobotSide getSide() + { + return kickSide.getValue(); + } + + public void setSide(RobotSide kickSide) + { + this.kickSide.setValue(kickSide); } public double getSwingDuration() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 83cca9b81248..f766e1abf5fb 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -65,7 +65,7 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor soleFramesForPlanning = new SideDependentList<>(); private final PoseReferenceFrame centerOfMassControlFrameForPlanning = new PoseReferenceFrame("CenterOfMassControlFrameForPlanning", ReferenceFrame.getWorldFrame()); - private static final double gravityZ = -9.81; + private static final double gravityZ = 9.81; // This needs to be positive for the planner to work private final KickDynamicPlanner kickDynamicPlanner; private final ZUpFrame stateParentZUpFrame; @@ -222,9 +222,10 @@ private void updateGoalPosesFromDynamicPlanner() liveGoalFeetPoses.get(stanceSide).setIncludingFrame(stanceGoalPose); FramePose3D kickStartPose = new FramePose3D(soleFramesForPlanning.get(kickSide)); - kickStartPose.changeFrame(soleFramesForPlanning.get(stanceSide)); - kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); +// kickStartPose.changeFrame(soleFramesForPlanning.get(stanceSide)); kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); + kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); +// kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); liveGoalFeetPoses.get(kickSide).setIncludingFrame(kickStartPose); } diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl index 88796fc249f7..126ff5d40ec5 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl @@ -16,6 +16,10 @@ module behavior_msgs * Parent definition fields */ behavior_msgs::msg::dds::ActionNodeDefinitionMessage definition; + /** + * Name of the parent frame the footsteps are expressed in + */ + string parent_frame_name; /** * Specifies the side of the robot that will execute the kick. */ 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 d74c8f55127f..a9a3e32d379e 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 "eb09f0c4d897abcb5ad3b672feea55721fb178e81516ba82174c14a29864bcb0"; + return "3de5038739d17959c39e2a371518c7c58941264cca5c3afa6c173ef7d8b4db7f"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java index 3fb338cd8d3d..13d13e3c265a 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java @@ -12,6 +12,10 @@ public class KickDoorActionDefinitionMessage extends Packet Date: Wed, 10 Apr 2024 12:04:35 -0500 Subject: [PATCH 15/32] reconfigured some of the way the behavior tree calls were structured to make better use of enums, and added in the additional action type, which seems incredibly prone to user error and labor intensive --- .../tree/RDXBehaviorTreeNodeBuilder.java | 152 +++++++----------- .../BehaviorTreeDefinitionRegistry.java | 78 ++++++--- .../BehaviorTreeExecutorNodeBuilder.java | 2 +- .../ros2/ROS2BehaviorTreeMessageTools.java | 91 +++++++---- .../ROS2BehaviorTreeSubscriptionNode.java | 13 ++ .../KickDoorApproachPlanActionExecutor.java | 2 - 6 files changed, 180 insertions(+), 158 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java index 846074a98720..03285c650e92 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java @@ -2,6 +2,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.behaviors.behaviorTree.BehaviorTreeDefinitionRegistry; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeDefinition; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeStateBuilder; import us.ihmc.behaviors.door.DoorTraversalDefinition; @@ -10,6 +11,7 @@ import us.ihmc.behaviors.sequence.actions.*; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; +import us.ihmc.log.LogTools; import us.ihmc.rdx.ui.RDX3DPanel; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.behavior.actions.*; @@ -53,109 +55,65 @@ public RDXBehaviorTreeNodeBuilder(DRCRobotModel robotModel, @Override public RDXBehaviorTreeNode createNode(Class nodeType, long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) { - // Control nodes: - if (nodeType == BehaviorTreeNodeDefinition.class) + BehaviorTreeDefinitionRegistry nodeEnum = BehaviorTreeDefinitionRegistry.getTypeFromClass(nodeType); + if (nodeEnum == null) { - return new RDXBehaviorTreeNode<>(id, crdtInfo, saveFileDirectory); - } - if (nodeType == ActionSequenceDefinition.class) - { - return new RDXActionSequence(id, crdtInfo, saveFileDirectory); - } - if (nodeType == DoorTraversalDefinition.class) - { - return new RDXDoorTraversal(id, crdtInfo, saveFileDirectory, syncedRobot); + throw new RuntimeException("No known node type for class " + nodeType.getSimpleName()); } - // Actions: - if (nodeType == ChestOrientationActionDefinition.class) - { - return new RDXChestOrientationAction(id, - crdtInfo, - saveFileDirectory, - panel3D, - robotModel, - syncedRobot.getFullRobotModel(), - selectionCollisionModel, - referenceFrameLibrary); - } - if (nodeType == FootstepPlanActionDefinition.class) - { - return new RDXFootstepPlanAction(id, - crdtInfo, - saveFileDirectory, - baseUI, - robotModel, - syncedRobot, - referenceFrameLibrary, - footstepPlannerParametersBasics); - } - if (nodeType == HandPoseActionDefinition.class) - { - return new RDXHandPoseAction(id, - crdtInfo, - saveFileDirectory, - panel3D, - robotModel, - syncedRobot, - selectionCollisionModel, - referenceFrameLibrary); - } - if (nodeType == HandWrenchActionDefinition.class) - { - return new RDXHandWrenchAction(id, crdtInfo, saveFileDirectory); - } - if (nodeType == ScrewPrimitiveActionDefinition.class) - { - return new RDXScrewPrimitiveAction(id, crdtInfo, saveFileDirectory, panel3D, referenceFrameLibrary, syncedRobot); - } - if (nodeType == PelvisHeightPitchActionDefinition.class) + return switch (nodeEnum) { - return new RDXPelvisHeightPitchAction(id, - crdtInfo, - saveFileDirectory, - panel3D, - robotModel, - syncedRobot.getFullRobotModel(), - selectionCollisionModel, - referenceFrameLibrary); - } - if (nodeType == SakeHandCommandActionDefinition.class) - { - return new RDXSakeHandCommandAction(id, crdtInfo, saveFileDirectory); - } - if (nodeType == WaitDurationActionDefinition.class) - { - return new RDXWaitDurationAction(id, crdtInfo, saveFileDirectory); - } - if (nodeType == KickDoorActionDefinition.class) - { - return new RDXKickDoorAction(id, - crdtInfo, - saveFileDirectory, - baseUI, - robotModel, - syncedRobot, - referenceFrameLibrary, - footstepPlannerParametersBasics); - } - if (nodeType == KickDoorApproachPlanActionDefinition.class) - { - return new RDXKickDoorApproachPlanAction(id, - crdtInfo, - saveFileDirectory, - baseUI, - robotModel, - syncedRobot, - referenceFrameLibrary, - footstepPlannerParametersBasics); - } - else - { - return null; - } + // Control nodes: + case BASIC_NODE -> new RDXBehaviorTreeNode<>(id, crdtInfo, saveFileDirectory); + case ACTION_SEQUENCE -> new RDXActionSequence(id, crdtInfo, saveFileDirectory); + case DOOR_TRAVERSAL -> new RDXDoorTraversal(id, crdtInfo, saveFileDirectory, syncedRobot); + // Actions: + case CHEST_ORIENTATION_ACTION -> new RDXChestOrientationAction(id, + crdtInfo, + saveFileDirectory, + panel3D, + robotModel, + syncedRobot.getFullRobotModel(), + selectionCollisionModel, + referenceFrameLibrary); + case FOOTSTEP_PLAN_ACTION -> new RDXFootstepPlanAction(id, + crdtInfo, + saveFileDirectory, + baseUI, + robotModel, + syncedRobot, + referenceFrameLibrary, + footstepPlannerParametersBasics); + case HAND_POSE_ACTION -> + new RDXHandPoseAction(id, crdtInfo, saveFileDirectory, panel3D, robotModel, syncedRobot, selectionCollisionModel, referenceFrameLibrary); + case HAND_WRENCH_ACTION -> new RDXHandWrenchAction(id, crdtInfo, saveFileDirectory); + case SCREW_PRIMITIVE_ACTION -> new RDXScrewPrimitiveAction(id, crdtInfo, saveFileDirectory, panel3D, referenceFrameLibrary, syncedRobot); + case PELVIS_HEIGHT_PITCH_ACTION -> new RDXPelvisHeightPitchAction(id, + crdtInfo, + saveFileDirectory, + panel3D, + robotModel, + syncedRobot.getFullRobotModel(), + selectionCollisionModel, + referenceFrameLibrary); + case SAKE_HAND_COMMAND_ACTION -> new RDXSakeHandCommandAction(id, crdtInfo, saveFileDirectory); + case WAIT_DURATION_ACTION -> new RDXWaitDurationAction(id, crdtInfo, saveFileDirectory); + case KICK_DOOR_ACTION -> + new RDXKickDoorAction(id, crdtInfo, saveFileDirectory, baseUI, robotModel, syncedRobot, referenceFrameLibrary, footstepPlannerParametersBasics); + case KICK_DOOR_APPROACH_ACTION -> new RDXKickDoorApproachPlanAction(id, + crdtInfo, + saveFileDirectory, + baseUI, + robotModel, + syncedRobot, + referenceFrameLibrary, + footstepPlannerParametersBasics); + default -> throw new RuntimeException("No return type defined for class " + nodeType.getSimpleName()); + }; } + + // This method is in this class because we have a syncedRobot here. public void initializeActionNode(@Nullable RDXActionSequence actionSequence, RDXActionNode newAction, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java index 69fae57d8196..9890d287490b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeDefinitionRegistry.java @@ -1,51 +1,77 @@ package us.ihmc.behaviors.behaviorTree; import behavior_msgs.msg.dds.BehaviorTreeStateMessage; +import com.esotericsoftware.kryo.util.IntMap; import us.ihmc.behaviors.door.DoorTraversalDefinition; import us.ihmc.behaviors.sequence.ActionSequenceDefinition; import us.ihmc.behaviors.sequence.actions.*; -public class BehaviorTreeDefinitionRegistry +import java.util.HashMap; + +public enum BehaviorTreeDefinitionRegistry { - private record RegistryRecord(Class typeClass, byte messageByte) { } + BASIC_NODE(BehaviorTreeNodeDefinition.class), + ACTION_SEQUENCE(ActionSequenceDefinition.class), + DOOR_TRAVERSAL(DoorTraversalDefinition.class), + CHEST_ORIENTATION_ACTION(ChestOrientationActionDefinition.class), + FOOTSTEP_PLAN_ACTION(FootstepPlanActionDefinition.class), + HAND_POSE_ACTION(HandPoseActionDefinition.class), + HAND_WRENCH_ACTION(HandWrenchActionDefinition.class), + SCREW_PRIMITIVE_ACTION(ScrewPrimitiveActionDefinition.class), + PELVIS_HEIGHT_PITCH_ACTION(PelvisHeightPitchActionDefinition.class), + SAKE_HAND_COMMAND_ACTION(SakeHandCommandActionDefinition.class), + WAIT_DURATION_ACTION(WaitDurationActionDefinition.class), + KICK_DOOR_ACTION(KickDoorActionDefinition.class), + KICK_DOOR_APPROACH_ACTION(KickDoorApproachPlanActionDefinition.class); + + private final Class typeClass; - private static final RegistryRecord[] DEFINITIONS = new RegistryRecord[] + private BehaviorTreeDefinitionRegistry(Class typeClass) { - new RegistryRecord(BehaviorTreeNodeDefinition.class, BehaviorTreeStateMessage.BASIC_NODE), - new RegistryRecord(ActionSequenceDefinition.class, BehaviorTreeStateMessage.ACTION_SEQUENCE), - new RegistryRecord(DoorTraversalDefinition.class, BehaviorTreeStateMessage.DOOR_TRAVERSAL), - - new RegistryRecord(ChestOrientationActionDefinition.class, BehaviorTreeStateMessage.CHEST_ORIENTATION_ACTION), - new RegistryRecord(FootstepPlanActionDefinition.class, BehaviorTreeStateMessage.FOOTSTEP_PLAN_ACTION), - new RegistryRecord(HandPoseActionDefinition.class, BehaviorTreeStateMessage.HAND_POSE_ACTION), - new RegistryRecord(HandWrenchActionDefinition.class, BehaviorTreeStateMessage.HAND_WRENCH_ACTION), - new RegistryRecord(ScrewPrimitiveActionDefinition.class, BehaviorTreeStateMessage.SCREW_PRIMITIVE_ACTION), - new RegistryRecord(PelvisHeightPitchActionDefinition.class, BehaviorTreeStateMessage.PELVIS_HEIGHT_PITCH_ACTION), - new RegistryRecord(SakeHandCommandActionDefinition.class, BehaviorTreeStateMessage.SAKE_HAND_COMMAND_ACTION), - new RegistryRecord(WaitDurationActionDefinition.class, BehaviorTreeStateMessage.WAIT_DURATION_ACTION), - new RegistryRecord(KickDoorActionDefinition.class, BehaviorTreeStateMessage.KICK_DOOR_ACTION), - new RegistryRecord(KickDoorApproachPlanActionDefinition.class, BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION) - }; + this.typeClass = typeClass; + } - public static Class getClassFromTypeName(String typeName) + public static BehaviorTreeDefinitionRegistry[] values = values(); + + public static BehaviorTreeDefinitionRegistry fromByte(byte index) + { + return values[index]; + } + + public byte toByte() + { + return (byte) ordinal(); + } + + public Class getTypeClass() { - for (RegistryRecord definitionEntry : DEFINITIONS) + return typeClass; + } + + public static BehaviorTreeDefinitionRegistry getTypeFromClass(Class clazz) + { + for (BehaviorTreeDefinitionRegistry type : values) { - if (typeName.equals(definitionEntry.typeClass().getSimpleName())) - return definitionEntry.typeClass(); + if (type.getTypeClass().equals(clazz)) + return type; } return null; } - public static Class getNodeStateClass(byte nodeType) + public static Class getClassFromTypeName(String typeName) { - for (RegistryRecord definitionEntry : DEFINITIONS) + for (BehaviorTreeDefinitionRegistry definitionType : values) { - if (nodeType == definitionEntry.messageByte()) - return definitionEntry.typeClass(); + if (typeName.equals(definitionType.getTypeClass().getSimpleName())) + return definitionType.typeClass; } return null; } + + public static Class getNodeStateClass(byte nodeType) + { + return fromByte(nodeType).getTypeClass(); + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java index a8b31da22e35..ef58153daba7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java @@ -126,6 +126,6 @@ public BehaviorTreeExecutorNodeBuilder(DRCRobotModel robotModel, return new KickDoorActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, syncedRobot, controllerStatusTracker, referenceFrameLibrary, walkingControllerParameters); } - return null; + throw new RuntimeException("Node defintion type not found: " + nodeType.getSimpleName()); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index aeed4d7a972c..83086cd7f3ac 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -1,6 +1,8 @@ package us.ihmc.behaviors.behaviorTree.ros2; import behavior_msgs.msg.dds.*; +import us.ihmc.behaviors.behaviorTree.BehaviorTreeDefinitionRegistry; +import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeDefinition; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeState; import us.ihmc.behaviors.door.DoorTraversalState; import us.ihmc.behaviors.sequence.ActionSequenceState; @@ -29,67 +31,68 @@ public static void clearLists(BehaviorTreeStateMessage treeStateMessage) treeStateMessage.getSakeHandCommandActions().clear(); treeStateMessage.getWaitDurationActions().clear(); treeStateMessage.getKickDoorActions().clear(); + treeStateMessage.getKickDoorApproachActions().clear(); } public static void packMessage(BehaviorTreeNodeState nodeState, BehaviorTreeStateMessage treeStateMessage) { if (nodeState instanceof ActionSequenceState actionSequenceState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.ACTION_SEQUENCE); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.ACTION_SEQUENCE.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getActionSequences().size()); actionSequenceState.toMessage(treeStateMessage.getActionSequences().add()); } else if (nodeState instanceof DoorTraversalState doorTraversalState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.DOOR_TRAVERSAL); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.DOOR_TRAVERSAL.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getDoorTraversals().size()); doorTraversalState.toMessage(treeStateMessage.getDoorTraversals().add()); } else if (nodeState instanceof ChestOrientationActionState chestOrientationActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.CHEST_ORIENTATION_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.CHEST_ORIENTATION_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getChestOrientationActions().size()); chestOrientationActionState.toMessage(treeStateMessage.getChestOrientationActions().add()); } else if (nodeState instanceof FootstepPlanActionState footstepPlanActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.FOOTSTEP_PLAN_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.FOOTSTEP_PLAN_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getFootstepPlanActions().size()); footstepPlanActionState.toMessage(treeStateMessage.getFootstepPlanActions().add()); } else if (nodeState instanceof SakeHandCommandActionState sakeHandCommandActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.SAKE_HAND_COMMAND_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.SAKE_HAND_COMMAND_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getSakeHandCommandActions().size()); sakeHandCommandActionState.toMessage(treeStateMessage.getSakeHandCommandActions().add()); } else if (nodeState instanceof HandPoseActionState handPoseActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.HAND_POSE_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.HAND_POSE_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getHandPoseActions().size()); handPoseActionState.toMessage(treeStateMessage.getHandPoseActions().add()); } else if (nodeState instanceof HandWrenchActionState handWrenchActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.HAND_WRENCH_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.HAND_WRENCH_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getHandWrenchActions().size()); handWrenchActionState.toMessage(treeStateMessage.getHandWrenchActions().add()); } else if (nodeState instanceof ScrewPrimitiveActionState screwPrimitiveActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.SCREW_PRIMITIVE_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.SCREW_PRIMITIVE_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getScrewPrimitiveActions().size()); screwPrimitiveActionState.toMessage(treeStateMessage.getScrewPrimitiveActions().add()); } else if (nodeState instanceof PelvisHeightPitchActionState pelvisHeightActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.PELVIS_HEIGHT_PITCH_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.PELVIS_HEIGHT_PITCH_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getPelvisHeightActions().size()); pelvisHeightActionState.toMessage(treeStateMessage.getPelvisHeightActions().add()); } else if (nodeState instanceof WaitDurationActionState waitDurationActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.WAIT_DURATION_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.WAIT_DURATION_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getWaitDurationActions().size()); waitDurationActionState.toMessage(treeStateMessage.getWaitDurationActions().add()); } @@ -101,13 +104,19 @@ else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPl } else if (nodeState instanceof KickDoorActionState kickDoorActionState) { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_ACTION); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.KICK_DOOR_ACTION.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); kickDoorActionState.toMessage(treeStateMessage.getKickDoorActions().add()); } + else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) + { + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.KICK_DOOR_APPROACH_ACTION.toByte()); + treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); + kickDoorApproachPlanActionState.toMessage(treeStateMessage.getKickDoorApproachActions().add()); + } else { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.BASIC_NODE); + treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.BASIC_NODE.toByte()); treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getBasicNodes().size()); BasicNodeStateMessage basicNodeMessage = treeStateMessage.getBasicNodes().add(); nodeState.toMessage(basicNodeMessage.getState()); @@ -165,6 +174,10 @@ else if (nodeState instanceof KickDoorActionState kickDoorActionState) { kickDoorActionState.fromMessage(subscriptionNode.getKickDoorActionStateMessage()); } + else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) + { + kickDoorApproachPlanActionState.fromMessage(subscriptionNode.getKickDoorApproachPlanActionStateMessage()); + } else { nodeState.fromMessage(subscriptionNode.getBehaviorTreeNodeStateMessage()); @@ -176,98 +189,112 @@ public static void packSubscriptionNode(byte nodeType, BehaviorTreeStateMessage treeStateMessage, ROS2BehaviorTreeSubscriptionNode subscriptionNode) { - switch (nodeType) + packSubscriptionNode(BehaviorTreeDefinitionRegistry.fromByte(nodeType), indexInTypesList, treeStateMessage, subscriptionNode); + } + + public static void packSubscriptionNode(BehaviorTreeDefinitionRegistry nodeEnum, + int indexInTypesList, + BehaviorTreeStateMessage treeStateMessage, + ROS2BehaviorTreeSubscriptionNode subscriptionNode) + { + switch (nodeEnum) { - case BehaviorTreeStateMessage.BASIC_NODE -> + case BASIC_NODE -> { BasicNodeStateMessage basicNodeStateMessage = treeStateMessage.getBasicNodes().get(indexInTypesList); subscriptionNode.setBehaviorTreeNodeStateMessage(basicNodeStateMessage.getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(basicNodeStateMessage.getDefinition()); } - case BehaviorTreeStateMessage.ACTION_SEQUENCE -> + case ACTION_SEQUENCE -> { ActionSequenceStateMessage actionSequenceStateMessage = treeStateMessage.getActionSequences().get(indexInTypesList); subscriptionNode.setActionSequenceStateMessage(actionSequenceStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(actionSequenceStateMessage.getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(actionSequenceStateMessage.getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.DOOR_TRAVERSAL -> + case DOOR_TRAVERSAL -> { DoorTraversalStateMessage doorTraversalStateMessage = treeStateMessage.getDoorTraversals().get(indexInTypesList); subscriptionNode.setDoorTraversalStateMessage(doorTraversalStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(doorTraversalStateMessage.getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(doorTraversalStateMessage.getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.CHEST_ORIENTATION_ACTION -> + case CHEST_ORIENTATION_ACTION -> { ChestOrientationActionStateMessage chestOrientationActionStateMessage = treeStateMessage.getChestOrientationActions().get(indexInTypesList); subscriptionNode.setChestOrientationActionStateMessage(chestOrientationActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(chestOrientationActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(chestOrientationActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.FOOTSTEP_PLAN_ACTION -> + case FOOTSTEP_PLAN_ACTION -> { FootstepPlanActionStateMessage footstepPlanActionStateMessage = treeStateMessage.getFootstepPlanActions().get(indexInTypesList); subscriptionNode.setFootstepPlanActionStateMessage(footstepPlanActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(footstepPlanActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(footstepPlanActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.HAND_POSE_ACTION -> + case HAND_POSE_ACTION -> { HandPoseActionStateMessage handPoseActionStateMessage = treeStateMessage.getHandPoseActions().get(indexInTypesList); subscriptionNode.setHandPoseActionStateMessage(handPoseActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(handPoseActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(handPoseActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.HAND_WRENCH_ACTION -> + case HAND_WRENCH_ACTION -> { HandWrenchActionStateMessage handWrenchActionStateMessage = treeStateMessage.getHandWrenchActions().get(indexInTypesList); subscriptionNode.setHandWrenchActionStateMessage(handWrenchActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(handWrenchActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(handWrenchActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.SCREW_PRIMITIVE_ACTION -> + case SCREW_PRIMITIVE_ACTION -> { ScrewPrimitiveActionStateMessage screwPrimitiveActionStateMessage = treeStateMessage.getScrewPrimitiveActions().get(indexInTypesList); subscriptionNode.setScrewPrimitiveActionStateMessage(screwPrimitiveActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(screwPrimitiveActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(screwPrimitiveActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.PELVIS_HEIGHT_PITCH_ACTION -> + case PELVIS_HEIGHT_PITCH_ACTION -> { PelvisHeightPitchActionStateMessage pelvisHeightPitchActionStateMessage = treeStateMessage.getPelvisHeightActions().get(indexInTypesList); subscriptionNode.setPelvisHeightPitchActionStateMessage(pelvisHeightPitchActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(pelvisHeightPitchActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(pelvisHeightPitchActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.SAKE_HAND_COMMAND_ACTION -> + case SAKE_HAND_COMMAND_ACTION -> { SakeHandCommandActionStateMessage sakeHandCommandActionStateMessage = treeStateMessage.getSakeHandCommandActions().get(indexInTypesList); subscriptionNode.setSakeHandCommandActionStateMessage(sakeHandCommandActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(sakeHandCommandActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(sakeHandCommandActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.WAIT_DURATION_ACTION -> + case WAIT_DURATION_ACTION -> { WaitDurationActionStateMessage waitDurationActionStateMessage = treeStateMessage.getWaitDurationActions().get(indexInTypesList); subscriptionNode.setWaitDurationActionStateMessage(waitDurationActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(waitDurationActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(waitDurationActionStateMessage.getDefinition().getDefinition().getDefinition()); } - case BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION -> - { - KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage = treeStateMessage.getKickDoorApproachActions().get(indexInTypesList); - subscriptionNode.setKickDoorApproachPlanStateMessage(kickDoorApproachPlanStateMessage); - subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorApproachPlanStateMessage.getState().getState()); - subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorApproachPlanStateMessage.getDefinition().getDefinition().getDefinition()); - } - case BehaviorTreeStateMessage.KICK_DOOR_ACTION -> + case KICK_DOOR_ACTION -> { KickDoorActionStateMessage kickDoorActionStateMessage = treeStateMessage.getKickDoorActions().get(indexInTypesList); subscriptionNode.setKickDoorActionStateMessage(kickDoorActionStateMessage); subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorActionStateMessage.getState().getState()); subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorActionStateMessage.getDefinition().getDefinition().getDefinition()); } + case KICK_DOOR_APPROACH_ACTION -> + { + KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage = treeStateMessage.getKickDoorApproachActions().get(indexInTypesList); + subscriptionNode.setKickDoorApproachPlanActionStateMessage(kickDoorApproachPlanActionStateMessage); + subscriptionNode.setBehaviorTreeNodeStateMessage(kickDoorApproachPlanActionStateMessage.getState().getState()); + subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorApproachPlanActionStateMessage.getDefinition().getDefinition().getDefinition()); + } + default -> + { + Class behaviorClass = nodeEnum.getTypeClass(); + String name = behaviorClass != null ? behaviorClass.getName() : "Undefined"; + throw new RuntimeException("Undefined behavior tree state message type " + name); + } } } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java index 21fb6440f8f5..b4166c6cfd7e 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java @@ -23,6 +23,7 @@ public class ROS2BehaviorTreeSubscriptionNode private WaitDurationActionStateMessage waitDurationActionStateMessage; private KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage; private KickDoorActionStateMessage kickDoorActionStateMessage; + private KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage; private final List children = new ArrayList<>(); public void clear() @@ -43,6 +44,7 @@ public void clear() waitDurationActionStateMessage = null; kickDoorApproachPlanStateMessage = null; kickDoorActionStateMessage = null; + kickDoorApproachPlanActionStateMessage = null; children.clear(); } @@ -206,6 +208,17 @@ public void setKickDoorActionStateMessage(KickDoorActionStateMessage kickDoorAct this.kickDoorActionStateMessage = kickDoorActionStateMessage; } + public KickDoorApproachPlanStateMessage getKickDoorApproachPlanActionStateMessage() + { + return kickDoorApproachPlanActionStateMessage; + } + + public void setKickDoorApproachPlanActionStateMessage(KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage) + { + this.kickDoorApproachPlanActionStateMessage = kickDoorApproachPlanActionStateMessage; + } + + public List getChildren() { return children; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index f766e1abf5fb..022439fc4c4f 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -222,10 +222,8 @@ private void updateGoalPosesFromDynamicPlanner() liveGoalFeetPoses.get(stanceSide).setIncludingFrame(stanceGoalPose); FramePose3D kickStartPose = new FramePose3D(soleFramesForPlanning.get(kickSide)); -// kickStartPose.changeFrame(soleFramesForPlanning.get(stanceSide)); kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); -// kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); liveGoalFeetPoses.get(kickSide).setIncludingFrame(kickStartPose); } From 1d7f671cb61fea50cc6c340568c03e8356c15014 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 10 Apr 2024 13:45:24 -0500 Subject: [PATCH 16/32] fixed a bunch of non-conflicting merge conflicts --- .../msg/BehaviorTreeStateMessage_.idl | 20 +++++++++---------- .../msg/dds/BehaviorTreeStateMessage.java | 20 +++++++++---------- .../BehaviorTreeStateMessagePubSubType.java | 2 +- .../msg/BehaviorTreeStateMessage.msg | 20 +++++++++---------- .../msg/BehaviorTreeStateMessage.msg | 20 +++++++++---------- 5 files changed, 41 insertions(+), 41 deletions(-) diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl index 3c4312ea267f..5e404269514d 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeStateMessage_.idl @@ -31,34 +31,34 @@ module behavior_msgs 2; const octet CHEST_ORIENTATION_ACTION = - 10; + 3; const octet FOOTSTEP_PLAN_ACTION = - 11; + 4; const octet SAKE_HAND_COMMAND_ACTION = - 12; + 5; const octet HAND_POSE_ACTION = - 13; + 6; const octet HAND_WRENCH_ACTION = - 14; + 7; const octet SCREW_PRIMITIVE_ACTION = - 15; + 8; const octet PELVIS_HEIGHT_PITCH_ACTION = - 16; + 9; const octet WAIT_DURATION_ACTION = - 17; + 10; const octet KICK_DOOR_ACTION = - 18; + 11; const octet KICK_DOOR_APPROACH_ACTION = - 19; + 12; /** diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java index 92fb3ecf46da..851245f6b3ba 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessage.java @@ -17,16 +17,16 @@ public class BehaviorTreeStateMessage extends Packet i public static final byte BASIC_NODE = (byte) 0; public static final byte ACTION_SEQUENCE = (byte) 1; public static final byte DOOR_TRAVERSAL = (byte) 2; - public static final byte CHEST_ORIENTATION_ACTION = (byte) 10; - public static final byte FOOTSTEP_PLAN_ACTION = (byte) 11; - public static final byte SAKE_HAND_COMMAND_ACTION = (byte) 12; - public static final byte HAND_POSE_ACTION = (byte) 13; - public static final byte HAND_WRENCH_ACTION = (byte) 14; - public static final byte SCREW_PRIMITIVE_ACTION = (byte) 15; - public static final byte PELVIS_HEIGHT_PITCH_ACTION = (byte) 16; - public static final byte WAIT_DURATION_ACTION = (byte) 17; - public static final byte KICK_DOOR_ACTION = (byte) 18; - public static final byte KICK_DOOR_APPROACH_ACTION = (byte) 19; + public static final byte CHEST_ORIENTATION_ACTION = (byte) 3; + public static final byte FOOTSTEP_PLAN_ACTION = (byte) 4; + public static final byte SAKE_HAND_COMMAND_ACTION = (byte) 5; + public static final byte HAND_POSE_ACTION = (byte) 6; + public static final byte HAND_WRENCH_ACTION = (byte) 7; + public static final byte SCREW_PRIMITIVE_ACTION = (byte) 8; + public static final byte PELVIS_HEIGHT_PITCH_ACTION = (byte) 9; + public static final byte WAIT_DURATION_ACTION = (byte) 10; + public static final byte KICK_DOOR_ACTION = (byte) 11; + public static final byte KICK_DOOR_APPROACH_ACTION = (byte) 12; /** * Monotonically increasing message ID that matches the CRDTInfo update number */ 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 a9a3e32d379e..d8045be25b8d 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 "3de5038739d17959c39e2a371518c7c58941264cca5c3afa6c173ef7d8b4db7f"; + return "8b4742aa570fa963b0a3a1fba451a820783f1f160012d67ea8163e1bc340daf6"; } @Override diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg index 10960bab3cff..61dd1bf369f9 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeStateMessage.msg @@ -7,16 +7,16 @@ byte BASIC_NODE = 0 byte ACTION_SEQUENCE = 1 byte DOOR_TRAVERSAL = 2 -byte CHEST_ORIENTATION_ACTION = 10 -byte FOOTSTEP_PLAN_ACTION = 11 -byte SAKE_HAND_COMMAND_ACTION = 12 -byte HAND_POSE_ACTION = 13 -byte HAND_WRENCH_ACTION = 14 -byte SCREW_PRIMITIVE_ACTION = 15 -byte PELVIS_HEIGHT_PITCH_ACTION = 16 -byte WAIT_DURATION_ACTION = 17 -byte KICK_DOOR_ACTION = 18 -byte KICK_DOOR_APPROACH_ACTION = 19 +byte CHEST_ORIENTATION_ACTION = 3 +byte FOOTSTEP_PLAN_ACTION = 4 +byte SAKE_HAND_COMMAND_ACTION = 5 +byte HAND_POSE_ACTION = 6 +byte HAND_WRENCH_ACTION = 7 +byte SCREW_PRIMITIVE_ACTION = 8 +byte PELVIS_HEIGHT_PITCH_ACTION = 9 +byte WAIT_DURATION_ACTION = 10 +byte KICK_DOOR_ACTION = 11 +byte KICK_DOOR_APPROACH_ACTION = 12 # Monotonically increasing message ID that matches the CRDTInfo update number uint32 sequence_id diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg index 19d53e666db0..c6b8a1e724e7 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeStateMessage.msg @@ -9,25 +9,25 @@ int8 ACTION_SEQUENCE=1 int8 DOOR_TRAVERSAL=2 -int8 CHEST_ORIENTATION_ACTION=10 +int8 CHEST_ORIENTATION_ACTION=3 -int8 FOOTSTEP_PLAN_ACTION=11 +int8 FOOTSTEP_PLAN_ACTION=4 -int8 SAKE_HAND_COMMAND_ACTION=12 +int8 SAKE_HAND_COMMAND_ACTION=5 -int8 HAND_POSE_ACTION=13 +int8 HAND_POSE_ACTION=6 -int8 HAND_WRENCH_ACTION=14 +int8 HAND_WRENCH_ACTION=7 -int8 SCREW_PRIMITIVE_ACTION=15 +int8 SCREW_PRIMITIVE_ACTION=8 -int8 PELVIS_HEIGHT_PITCH_ACTION=16 +int8 PELVIS_HEIGHT_PITCH_ACTION=9 -int8 WAIT_DURATION_ACTION=17 +int8 WAIT_DURATION_ACTION=10 -int8 KICK_DOOR_ACTION=18 +int8 KICK_DOOR_ACTION=11 -int8 KICK_DOOR_APPROACH_ACTION=19 +int8 KICK_DOOR_APPROACH_ACTION=12 # Monotonically increasing message ID that matches the CRDTInfo update number uint32 sequence_id From 8069309a9c933952cdd909191252d037fe1451f3 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 10 Apr 2024 13:51:03 -0500 Subject: [PATCH 17/32] fixed a few other conflicts --- .../donkeyKick/KickDynamicPlanner.java | 2 +- .../ros2/ROS2BehaviorTreeMessageTools.java | 12 +----------- .../ros2/ROS2BehaviorTreeSubscriptionNode.java | 12 ------------ 3 files changed, 2 insertions(+), 24 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java index 99aa3a5c1427..3041621d2bde 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java @@ -71,7 +71,7 @@ public KickDynamicPlanner(KickParameters kickParameters, this.centerOfMassControlFrame = centerOfMassControlFrame; this.soleFrames = soleFrames; // figure out the touchdown duration after the fact. - this.gravityZ = gravityZ; + this.gravityZ = Math.abs(gravityZ); this.totalMass = totalMass; if (parentRegistry != null) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index 83086cd7f3ac..0497da32f703 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -96,12 +96,6 @@ else if (nodeState instanceof WaitDurationActionState waitDurationActionState) treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getWaitDurationActions().size()); waitDurationActionState.toMessage(treeStateMessage.getWaitDurationActions().add()); } - else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) - { - treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeStateMessage.KICK_DOOR_APPROACH_ACTION); - treeStateMessage.getBehaviorTreeIndices().add(treeStateMessage.getKickDoorActions().size()); - kickDoorApproachPlanActionState.toMessage(treeStateMessage.getKickDoorApproachActions().add()); - } else if (nodeState instanceof KickDoorActionState kickDoorActionState) { treeStateMessage.getBehaviorTreeTypes().add(BehaviorTreeDefinitionRegistry.KICK_DOOR_ACTION.toByte()); @@ -168,16 +162,12 @@ else if (nodeState instanceof WaitDurationActionState waitDurationActionState) } else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) { - kickDoorApproachPlanActionState.fromMessage(subscriptionNode.getKickDoorApproachPlanStateMessage()); + kickDoorApproachPlanActionState.fromMessage(subscriptionNode.getKickDoorApproachPlanActionStateMessage()); } else if (nodeState instanceof KickDoorActionState kickDoorActionState) { kickDoorActionState.fromMessage(subscriptionNode.getKickDoorActionStateMessage()); } - else if (nodeState instanceof KickDoorApproachPlanActionState kickDoorApproachPlanActionState) - { - kickDoorApproachPlanActionState.fromMessage(subscriptionNode.getKickDoorApproachPlanActionStateMessage()); - } else { nodeState.fromMessage(subscriptionNode.getBehaviorTreeNodeStateMessage()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java index b4166c6cfd7e..7ec65b563287 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeSubscriptionNode.java @@ -21,7 +21,6 @@ public class ROS2BehaviorTreeSubscriptionNode private ScrewPrimitiveActionStateMessage screwPrimitiveActionStateMessage; private PelvisHeightPitchActionStateMessage pelvisHeightPitchActionStateMessage; private WaitDurationActionStateMessage waitDurationActionStateMessage; - private KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage; private KickDoorActionStateMessage kickDoorActionStateMessage; private KickDoorApproachPlanStateMessage kickDoorApproachPlanActionStateMessage; private final List children = new ArrayList<>(); @@ -42,7 +41,6 @@ public void clear() screwPrimitiveActionStateMessage = null; pelvisHeightPitchActionStateMessage = null; waitDurationActionStateMessage = null; - kickDoorApproachPlanStateMessage = null; kickDoorActionStateMessage = null; kickDoorApproachPlanActionStateMessage = null; children.clear(); @@ -188,16 +186,6 @@ public void setWaitDurationActionStateMessage(WaitDurationActionStateMessage wai this.waitDurationActionStateMessage = waitDurationActionStateMessage; } - public KickDoorApproachPlanStateMessage getKickDoorApproachPlanStateMessage() - { - return kickDoorApproachPlanStateMessage; - } - - public void setKickDoorApproachPlanStateMessage(KickDoorApproachPlanStateMessage kickDoorApproachPlanStateMessage) - { - this.kickDoorApproachPlanStateMessage = kickDoorApproachPlanStateMessage; - } - public KickDoorActionStateMessage getKickDoorActionStateMessage() { return kickDoorActionStateMessage; From 16ce7e57284b65e635bb87a0f615beb119eeee7e Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Wed, 10 Apr 2024 15:11:18 -0500 Subject: [PATCH 18/32] Temp fix to get the robot in the right spot. --- .../KickDoorApproachPlanActionExecutor.java | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 022439fc4c4f..4aae6f6f1da9 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -49,6 +49,7 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor commandedGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); private final SideDependentList syncedFeetPoses = new SideDependentList<>(() -> new FramePose3D()); private final SideDependentList indexOfLastFoot = new SideDependentList<>(); + private final ReferenceFrameLibrary referenceFrameLibrary; private double nominalExecutionDuration; private final SideDependentList trackingCalculators = new SideDependentList<>( TaskspaceTrajectoryTrackingErrorCalculator::new); @@ -86,6 +87,7 @@ public KickDoorApproachPlanActionExecutor(long id, state = getState(); definition = getDefinition(); + this.referenceFrameLibrary = referenceFrameLibrary; this.ros2ControllerHelper = ros2ControllerHelper; this.syncedRobot = syncedRobot; this.controllerStatusTracker = controllerStatusTracker; @@ -93,7 +95,7 @@ public KickDoorApproachPlanActionExecutor(long id, this.footstepPlanner = footstepPlanner; this.footstepPlannerParameters = footstepPlannerParameters; - stateParentZUpFrame = new ZUpFrame(state.getParentFrame(), "StateParentZUpFrame"); + stateParentZUpFrame = new ZUpFrame(referenceFrameLibrary.findFrameByName("RightDoorLeverHandle"), "StateParentZUpFrame"); for (RobotSide robotSide : RobotSide.values) { @@ -188,17 +190,25 @@ private void computeSquaredUpGoalFootPosesFromDoorHandle() FramePose3D kickFootPose = new FramePose3D(); kickFootPose.setToZero(stateParentZUpFrame); - kickFootPose.setX(-definition.getKickTargetDistance().getValue()); - kickFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); - kickFootPose.getOrientation().setToYawOrientation(Math.PI); + //FIXME: This is a temp fix until we get stateParentZUpFrame to be the correct frame. +// kickFootPose.setX(-definition.getKickTargetDistance().getValue()); +// kickFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); +// kickFootPose.getOrientation().setToYawOrientation(Math.PI); + kickFootPose.setX(definition.getKickTargetDistance().getValue()); + kickFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); +// kickFootPose.getOrientation().setToYawOrientation(Math.PI); kickFootPose.changeFrame(ReferenceFrame.getWorldFrame()); kickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); FramePose3D stanceFootPose = new FramePose3D(); stanceFootPose.setToZero(stateParentZUpFrame); - stanceFootPose.setX(-definition.getKickTargetDistance().getValue()); - stanceFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); - stanceFootPose.getOrientation().setToYawOrientation(Math.PI); + //FIXME: This is a temp fix until we get stateParentZUpFrame to be the correct frame. +// stanceFootPose.setX(-definition.getKickTargetDistance().getValue()); +// stanceFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); +// stanceFootPose.getOrientation().setToYawOrientation(Math.PI); + stanceFootPose.setX(definition.getKickTargetDistance().getValue()); + stanceFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); +// stanceFootPose.getOrientation().setToYawOrientation(Math.PI); stanceFootPose.changeFrame(ReferenceFrame.getWorldFrame()); stanceFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); @@ -218,12 +228,18 @@ private void updateGoalPosesFromDynamicPlanner() FramePose3D stanceGoalPose = new FramePose3D(soleFramesForPlanning.get(stanceSide)); stanceGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); +// stanceGoalPose.setIncludingFrame(stateParentZUpFrame, stanceGoalPose.getPosition(), stanceGoalPose.getOrientation()); +// stanceGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); +// stanceGoalPose.setZ(0.0); liveGoalFeetPoses.get(stanceSide).setIncludingFrame(stanceGoalPose); FramePose3D kickStartPose = new FramePose3D(soleFramesForPlanning.get(kickSide)); kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); +// kickStartPose.setIncludingFrame(stateParentZUpFrame, kickStartPose.getPosition(), kickStartPose.getOrientation()); +// kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); +// kickStartPose.setZ(0.0); liveGoalFeetPoses.get(kickSide).setIncludingFrame(kickStartPose); } From cafc234e07ffee5e5769b9452abbaff9edba684f Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 10 Apr 2024 14:02:43 -0500 Subject: [PATCH 19/32] set the parent frame up to be mutable, but things are still wrong --- .../KickDoorApproachPlanActionExecutor.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 4aae6f6f1da9..f0e28ce11051 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -69,7 +69,7 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor Date: Wed, 10 Apr 2024 17:17:54 -0500 Subject: [PATCH 20/32] piped through the weight distribution from the input parameters for the kick --- .../donkeyKick/KickDynamicPlanner.java | 1 + .../donkeyKick/KickParameters.java | 7 ++- .../RDXKickDoorApproachPlanAction.java | 49 ++++++++++++------- .../KickDoorApproachPlanActionExecutor.java | 8 +-- 4 files changed, 42 insertions(+), 23 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java index 3041621d2bde..7e169eba764b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java @@ -152,6 +152,7 @@ public void compute(KickInputParameters inputParameters) { double touchdownDuration = inputParameters.getKickHeight() / kickParameters.getTouchdownHeightSpeed(); estimatedTouchdownDuration.set(touchdownDuration); + kickParameters.setPreShiftWeightDistribution(inputParameters.getPrekickWeightDistribution()); RobotSide supportSide = inputParameters.getKickFootSide().getOppositeSide(); double omega = Math.sqrt(gravityZ / kickParameters.getDesiredCoMHeight()); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java index dc25fa7d9cd8..05f641c0876b 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickParameters.java @@ -31,7 +31,7 @@ public class KickParameters private static final double desiredTouchdownPositionRelativeToTarget = -0.15; // m private static final double touchdownHeightSpeed = 1.0; // m /s - private static final double preShiftWeightDistribution = 0.75; // percentage + private double preShiftWeightDistribution = 0.75; // percentage private static final double preShiftDuration = 2.5; // s private static final double shiftDuration = 0.5; // s // TODO make this a computed value private static final double chamberDuration = 0.25; // s @@ -54,6 +54,11 @@ public class KickParameters private final KickingWBCCParameters fastWalkingWBCCParameters = new KickingWBCCParameters(); + public void setPreShiftWeightDistribution(double preShiftWeightDistribution) + { + this.preShiftWeightDistribution = preShiftWeightDistribution; + } + public double getDesiredCoMHeight() { return desiredCoMHeight; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java index 66b80422aa34..bf71704ed992 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java @@ -47,9 +47,13 @@ public class RDXKickDoorApproachPlanAction extends RDXActionNode goalFeetGraphics = new SideDependentList<>(); private final RDX3DPanelTooltip tooltip; @@ -74,7 +78,6 @@ public RDXKickDoorApproachPlanAction(long id, definition.setName("Footstep plan"); - swingDurationWidget = new ImDoubleWrapper(definition::getSwingDuration, definition::setSwingDuration, imDouble -> ImGui.inputDouble(labels.get("Swing duration"), imDouble)); @@ -82,6 +85,21 @@ public RDXKickDoorApproachPlanAction(long id, definition::setTransferDuration, imDouble -> ImGui.inputDouble(labels.get("Transfer duration"), imDouble)); + kickImpulseWidget = new ImDoubleWrapper(definition.getKickImpulse()::getValue, + definition.getKickImpulse()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Kick impulse"), imDouble)); + kickTargetDistanceWidget = new ImDoubleWrapper(definition.getKickTargetDistance()::getValue, + definition.getKickTargetDistance()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Kick Target Distance"), imDouble)); + horizontalDistanceFromHandleWidget = new ImDoubleWrapper(definition.getHorizontalDistanceFromHandle()::getValue, + definition.getHorizontalDistanceFromHandle()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Horizontal distance from handle"), imDouble)); + preKickWeightDistributionWidget = new ImDoubleWrapper(definition.getPrekickWeightDistribution()::getValue, + definition.getPrekickWeightDistribution()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Prekick weight distribution"), imDouble)); + stanceFootWidthWidget = new ImDoubleWrapper(definition.getStanceFootWidth()::getValue, + definition.getStanceFootWidth()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Stance foot width"), imDouble)); for (RobotSide side : RobotSide.values) { @@ -99,17 +117,14 @@ public void update() { super.update(); - if (state.areFramesInWorld()) { // Add a footstep to the action data only - // Update arrow graphic geometry goalFeetGraphics.get(RobotSide.LEFT).setPose(state.getLeftFootGoalPose().getValueReadOnly()); goalFeetGraphics.get(RobotSide.RIGHT).setPose(state.getRightFootGoalPose().getValueReadOnly()); - } } @@ -149,6 +164,11 @@ protected void renderImGuiWidgetsInternal() ImGui.pushItemWidth(80.0f); swingDurationWidget.renderImGuiWidget(); transferDurationWidget.renderImGuiWidget(); + kickImpulseWidget.renderImGuiWidget(); + kickTargetDistanceWidget.renderImGuiWidget(); + preKickWeightDistributionWidget.renderImGuiWidget(); + horizontalDistanceFromHandleWidget.renderImGuiWidget(); + stanceFootWidthWidget.renderImGuiWidget(); ImGui.popItemWidth(); ImGui.text("Execution mode:"); ImGui.sameLine(); @@ -157,13 +177,11 @@ protected void renderImGuiWidgetsInternal() ImGui.sameLine(); if (ImGui.radioButton(labels.get("Queue"), definition.getExecutionMode().getValue() == ExecutionMode.QUEUE)) definition.getExecutionMode().setValue(ExecutionMode.QUEUE); - } @Override public void deselectGizmos() { - editManuallyPlacedSteps.set(false); } public void render3DPanelImGuiOverlays() @@ -176,13 +194,11 @@ public void getRenderables(Array renderables, Pool pool) { if (state.areFramesInWorld()) { - - - for (RobotSide side : RobotSide.values) - { - goalFeetGraphics.get(side).setHighlighted(footstepsWidget.getIsHovered().get(side)); - goalFeetGraphics.get(side).getRenderables(renderables, pool); - } + for (RobotSide side : RobotSide.values) + { + goalFeetGraphics.get(side).setHighlighted(footstepsWidget.getIsHovered().get(side)); + goalFeetGraphics.get(side).getRenderables(renderables, pool); + } } } @@ -191,9 +207,4 @@ public String getActionTypeTitle() { return "Kick Door Approach Plan"; } - - public ImBoolean getEditManuallyPlacedSteps() - { - return editManuallyPlacedSteps; - } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index f0e28ce11051..c4a4da5c56fd 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -167,9 +167,13 @@ public void update() } state.setCanExecute(state.areFramesInWorld() && !invalidDefinition); + computeGoalFootPosesForKick(); + + state.getLeftFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.LEFT)); + state.getRightFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.RIGHT)); + if (state.getCanExecute()) { - computeGoalFootPosesForKick(); } for (RobotSide side : RobotSide.values) @@ -268,8 +272,6 @@ public void triggerActionExecution() state.getCurrentFootPoses().get(side).getValue().set(syncedFeetPoses.get(side)); state.getDesiredFootPoses().get(side).getValue().clear(); } - state.getLeftFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.LEFT)); - state.getRightFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.RIGHT)); state.setPositionDistanceToGoalTolerance(POSITION_TOLERANCE); state.setOrientationDistanceToGoalTolerance(ORIENTATION_TOLERANCE); From 2f00f3a02e97a069e11866124e389926d54dcf58 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 10 Apr 2024 17:36:56 -0500 Subject: [PATCH 21/32] started rendering the kick height --- .../RDXKickDoorApproachPlanAction.java | 28 +++++++++---------- .../tree/RDXBehaviorTreeNodeBuilder.java | 4 +-- .../KickDoorApproachPlanActionDefinition.java | 16 +++++++++++ .../KickDoorApproachPlanActionExecutor.java | 28 +++++++++++++------ .../KickDoorApproachPlanActionState.java | 9 ++++++ ...KickDoorApproachPlanDefinitionMessage_.idl | 4 +++ .../msg/KickDoorApproachPlanStateMessage_.idl | 4 +++ .../BehaviorTreeStateMessagePubSubType.java | 2 +- ...KickDoorApproachPlanDefinitionMessage.java | 27 ++++++++++++++++++ ...proachPlanDefinitionMessagePubSubType.java | 13 ++++++++- .../dds/KickDoorApproachPlanStateMessage.java | 19 +++++++++++++ ...oorApproachPlanStateMessagePubSubType.java | 12 +++++++- .../KickDoorApproachPlanDefinitionMessage.msg | 3 ++ .../msg/KickDoorApproachPlanStateMessage.msg | 3 ++ .../KickDoorApproachPlanDefinitionMessage.msg | 3 ++ .../msg/KickDoorApproachPlanStateMessage.msg | 3 ++ 16 files changed, 150 insertions(+), 28 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java index bf71704ed992..5ce8090097bd 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java @@ -42,8 +42,6 @@ public class RDXKickDoorApproachPlanAction extends RDXActionNode { - private final DRCRobotModel robotModel; - private final ROS2SyncedRobotModel syncedRobot; private final KickDoorApproachPlanActionState state; private final KickDoorApproachPlanActionDefinition definition; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); @@ -54,28 +52,22 @@ public class RDXKickDoorApproachPlanAction extends RDXActionNode goalFeetGraphics = new SideDependentList<>(); - private final RDX3DPanelTooltip tooltip; - private final ImGuiFootstepsWidget footstepsWidget = new ImGuiFootstepsWidget(); + private final RDXFootstepGraphic kickGoalGraphic; public RDXKickDoorApproachPlanAction(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory, RDXBaseUI baseUI, DRCRobotModel robotModel, - ROS2SyncedRobotModel syncedRobot, - ReferenceFrameLibrary referenceFrameLibrary, - FootstepPlannerParametersBasics footstepPlannerParameters) + ReferenceFrameLibrary referenceFrameLibrary) { super(new KickDoorApproachPlanActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); state = getState(); definition = getDefinition(); - this.robotModel = robotModel; - this.syncedRobot = syncedRobot; - definition.setName("Footstep plan"); swingDurationWidget = new ImDoubleWrapper(definition::getSwingDuration, @@ -100,6 +92,9 @@ public RDXKickDoorApproachPlanAction(long id, stanceFootWidthWidget = new ImDoubleWrapper(definition.getStanceFootWidth()::getValue, definition.getStanceFootWidth()::setValue, imDouble -> ImGui.inputDouble(labels.get("Stance foot width"), imDouble)); + kickHeightWidget = new ImDoubleWrapper(definition.getKickHeight()::getValue, + definition.getKickHeight()::setValue, + imDouble -> ImGui.inputDouble(labels.get("Kick height"), imDouble)); for (RobotSide side : RobotSide.values) { @@ -107,8 +102,10 @@ public RDXKickDoorApproachPlanAction(long id, goalFootGraphic.create(); goalFeetGraphics.put(side, goalFootGraphic); } + kickGoalGraphic = new RDXFootstepGraphic(robotModel.getContactPointParameters().getControllerFootGroundContactPoints(), RobotSide.LEFT); + kickGoalGraphic.create(); + - tooltip = new RDX3DPanelTooltip(baseUI.getPrimary3DPanel()); baseUI.getPrimary3DPanel().addImGuiOverlayAddition(this::render3DPanelImGuiOverlays); } @@ -123,6 +120,7 @@ public void update() // Update arrow graphic geometry + kickGoalGraphic.setPose(state.getKickGoalPose().getValueReadOnly()); goalFeetGraphics.get(RobotSide.LEFT).setPose(state.getLeftFootGoalPose().getValueReadOnly()); goalFeetGraphics.get(RobotSide.RIGHT).setPose(state.getRightFootGoalPose().getValueReadOnly()); } @@ -133,7 +131,6 @@ public void renderTreeViewIconArea() { super.renderTreeViewIconArea(); - footstepsWidget.render(ImGui.getFrameHeight()); ImGui.sameLine(); } @@ -169,6 +166,7 @@ protected void renderImGuiWidgetsInternal() preKickWeightDistributionWidget.renderImGuiWidget(); horizontalDistanceFromHandleWidget.renderImGuiWidget(); stanceFootWidthWidget.renderImGuiWidget(); + kickHeightWidget.renderImGuiWidget(); ImGui.popItemWidth(); ImGui.text("Execution mode:"); ImGui.sameLine(); @@ -196,9 +194,11 @@ public void getRenderables(Array renderables, Pool pool) { for (RobotSide side : RobotSide.values) { - goalFeetGraphics.get(side).setHighlighted(footstepsWidget.getIsHovered().get(side)); + goalFeetGraphics.get(side).setHighlighted(false); goalFeetGraphics.get(side).getRenderables(renderables, pool); } + kickGoalGraphic.setHighlighted(false); + kickGoalGraphic.getRenderables(renderables, pool); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java index 03285c650e92..9d63e3fefe04 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java @@ -105,9 +105,7 @@ public RDXBehaviorTreeNodeBuilder(DRCRobotModel robotModel, saveFileDirectory, baseUI, robotModel, - syncedRobot, - referenceFrameLibrary, - footstepPlannerParametersBasics); + referenceFrameLibrary); default -> throw new RuntimeException("No return type defined for class " + nodeType.getSimpleName()); }; } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java index c2a5b9086873..d5dec5297ca6 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -19,6 +19,7 @@ public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition i { public static final double KICK_IMPULSE = 55.0; public static final double KICK_TARGET_DISTANCE = 0.75; + public static final double KICK_HEIGHT = 0.55; public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; public static final double HORIZONTAL_DISTANCE_FROM_HANDLE = 0.1; public static final double STANCE_FOOT_WIDTH = 0.23; @@ -30,6 +31,7 @@ public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition i private final CRDTUnidirectionalEnumField kickSide; private final CRDTUnidirectionalDouble kickImpulse; + private final CRDTUnidirectionalDouble kickHeight; private final CRDTUnidirectionalDouble kickTargetDistance; private final CRDTUnidirectionalDouble prekickWeightDistribution; private final CRDTUnidirectionalDouble horizontalDistanceFromHandle; @@ -42,6 +44,7 @@ public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition i private String onDiskParentFrameName; private RobotSide onDiskKickSide; private double onDiskKickImpulse; + private double onDiskKickHeight; private double onDiskKickTargetDistance; private double onDiskPrekickWeightDistribution; private double onDiskHorizontalDistanceFromHandle; @@ -59,6 +62,7 @@ public KickDoorApproachPlanActionDefinition(CRDTInfo crdtInfo, WorkspaceResource kickSide = new CRDTUnidirectionalEnumField<>(ROS2ActorDesignation.OPERATOR, crdtInfo, RobotSide.LEFT); kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); + kickHeight = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_HEIGHT); kickTargetDistance = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_TARGET_DISTANCE); prekickWeightDistribution = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, PREKICK_WEIGHT_DISTRIBUTION); horizontalDistanceFromHandle = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, HORIZONTAL_DISTANCE_FROM_HANDLE); @@ -76,6 +80,7 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("parentFrame", parentFrameName.getValue()); jsonNode.put("kickImpulse", kickImpulse.getValue()); + jsonNode.put("kickHeight", kickHeight.getValue()); jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); jsonNode.put("horizontalDistanceFromHandle", horizontalDistanceFromHandle.getValue()); @@ -94,6 +99,7 @@ public void loadFromFile(JsonNode jsonNode) parentFrameName.setValue(jsonNode.get("parentFrame").textValue()); kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); + kickHeight.setValue(jsonNode.get("kickHeight").asDouble()); kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); horizontalDistanceFromHandle.setValue(jsonNode.get("horizontalDistanceFromHandle").asDouble()); @@ -112,6 +118,7 @@ public void setOnDiskFields() onDiskParentFrameName = parentFrameName.getValue(); onDiskKickSide = kickSide.getValue(); onDiskKickImpulse = kickImpulse.getValue(); + onDiskKickHeight = kickHeight.getValue(); onDiskKickTargetDistance = kickTargetDistance.getValue(); onDiskPrekickWeightDistribution = prekickWeightDistribution.getValue(); onDiskHorizontalDistanceFromHandle = horizontalDistanceFromHandle.getValue(); @@ -129,6 +136,7 @@ public void undoAllNontopologicalChanges() parentFrameName.setValue(onDiskParentFrameName); kickSide.setValue(onDiskKickSide); kickImpulse.setValue(onDiskKickImpulse); + kickHeight.setValue(onDiskKickHeight); kickTargetDistance.setValue(onDiskKickTargetDistance); prekickWeightDistribution.setValue(onDiskPrekickWeightDistribution); horizontalDistanceFromHandle.setValue(onDiskHorizontalDistanceFromHandle); @@ -146,6 +154,7 @@ public boolean hasChanges() unchanged &= parentFrameName.getValue().equals(onDiskParentFrameName); unchanged &= kickSide.getValue() == onDiskKickSide; unchanged &= kickImpulse.getValue() == onDiskKickImpulse; + unchanged &= kickHeight.getValue() == onDiskKickHeight; unchanged &= kickTargetDistance.getValue() == onDiskKickTargetDistance; unchanged &= prekickWeightDistribution.getValue() == onDiskPrekickWeightDistribution; unchanged &= horizontalDistanceFromHandle.getValue() == onDiskHorizontalDistanceFromHandle; @@ -165,6 +174,7 @@ public void toMessage(KickDoorApproachPlanDefinitionMessage message) message.setRobotSide(kickSide.toMessage().toByte()); message.setKickImpulse(kickImpulse.toMessage()); + message.setKickHeight(kickHeight.toMessage()); message.setKickTargetDistance(kickTargetDistance.toMessage()); message.setPrekickWeightDistribution(prekickWeightDistribution.toMessage()); message.setHorizontalDistanceFromHandle(horizontalDistanceFromHandle.toMessage()); @@ -181,6 +191,7 @@ public void fromMessage(KickDoorApproachPlanDefinitionMessage message) parentFrameName.fromMessage(message.getParentFrameNameAsString()); kickSide.fromMessage(RobotSide.fromByte(message.getRobotSide())); + kickHeight.fromMessage(message.getKickHeight()); kickImpulse.fromMessage(message.getKickImpulse()); kickTargetDistance.fromMessage(message.getKickTargetDistance()); prekickWeightDistribution.fromMessage(message.getPrekickWeightDistribution()); @@ -268,4 +279,9 @@ public CRDTUnidirectionalDouble getStanceFootWidth() { return stanceFootWidth; } + + public CRDTUnidirectionalDouble getKickHeight() + { + return kickHeight; + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index c4a4da5c56fd..545763124717 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -59,6 +59,7 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor footstepPlanNotification = new TypedNotification<>(); private final SideDependentList liveGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); + private final FramePose3D liveKickPose = new FramePose3D(); private final SideDependentList startFootPosesForThread = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private final SideDependentList goalFootPosesForThread = new SideDependentList<>(new FramePose3D(), new FramePose3D()); @@ -131,6 +132,7 @@ public void update() updateStateParentZUpFrame(); double kickImpulse = definition.getKickImpulse().getValue(); + double kickHeight = definition.getKickHeight().getValue(); double kickTargetDistance = definition.getKickTargetDistance().getValue(); double prekickWeightDistribution = definition.getPrekickWeightDistribution().getValue(); double kickStanceWidth = definition.getStanceFootWidth().getValue(); @@ -171,6 +173,7 @@ public void update() state.getLeftFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.LEFT)); state.getRightFootGoalPose().getValue().set(liveGoalFeetPoses.get(RobotSide.RIGHT)); + state.getKickGoalPose().getValue().set(liveKickPose); if (state.getCanExecute()) { @@ -203,17 +206,24 @@ private void computeSquaredUpGoalFootPosesFromDoorHandle() { RobotSide kickSide = definition.getKickSide().getValue(); - FramePose3D kickFootPose = new FramePose3D(); - kickFootPose.setToZero(stateParentZUpFrame); + FramePose3D preKickFootPose = new FramePose3D(); + preKickFootPose.setToZero(stateParentZUpFrame); //FIXME: This is a temp fix until we get stateParentZUpFrame to be the correct frame. // kickFootPose.setX(-definition.getKickTargetDistance().getValue()); // kickFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); // kickFootPose.getOrientation().setToYawOrientation(Math.PI); - kickFootPose.setX(definition.getKickTargetDistance().getValue()); - kickFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); + preKickFootPose.setX(definition.getKickTargetDistance().getValue()); + preKickFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); // kickFootPose.getOrientation().setToYawOrientation(Math.PI); - kickFootPose.changeFrame(ReferenceFrame.getWorldFrame()); - kickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + preKickFootPose.changeFrame(ReferenceFrame.getWorldFrame()); + preKickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + + FramePose3D kickPose = new FramePose3D(preKickFootPose); + kickPose.setZ(definition.getKickHeight().getValue()); + kickPose.changeFrame(stateParentZUpFrame); + kickPose.setX(0.0); + kickPose.appendPitchRotation(Math.PI / 2.0); + kickPose.changeFrame(ReferenceFrame.getWorldFrame()); FramePose3D stanceFootPose = new FramePose3D(); stanceFootPose.setToZero(stateParentZUpFrame); @@ -228,11 +238,13 @@ private void computeSquaredUpGoalFootPosesFromDoorHandle() stanceFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); FramePose3D centerOfMassPose = new FramePose3D(); - centerOfMassPose.interpolate(kickFootPose, stanceFootPose, 0.5); + centerOfMassPose.interpolate(preKickFootPose, stanceFootPose, 0.5); - soleFramesForPlanning.get(kickSide).setPoseAndUpdate(kickFootPose); + soleFramesForPlanning.get(kickSide).setPoseAndUpdate(preKickFootPose); soleFramesForPlanning.get(kickSide.getOppositeSide()).setPoseAndUpdate(stanceFootPose); + liveKickPose.set(kickPose); + centerOfMassControlFrameForPlanning.setPoseAndUpdate(centerOfMassPose); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java index da921454d236..bf0b15e10e85 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java @@ -14,6 +14,7 @@ public class KickDoorApproachPlanActionState extends ActionNodeState (100, new ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType()); @@ -80,6 +85,7 @@ public void set(KickDoorApproachPlanStateMessage other) { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.staticCopy(other.state_, state_); behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.staticCopy(other.definition_, definition_); + geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.kick_goal_pose_, kick_goal_pose_); geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.left_foot_goal_pose_, left_foot_goal_pose_); geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.right_foot_goal_pose_, right_foot_goal_pose_); execution_state_ = other.execution_state_; @@ -113,6 +119,15 @@ public behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessage getDefinition } + /** + * Kick goal pose + */ + public us.ihmc.euclid.geometry.Pose3D getKickGoalPose() + { + return kick_goal_pose_; + } + + /** * Left foot goal pose */ @@ -231,6 +246,7 @@ public boolean epsilonEquals(KickDoorApproachPlanStateMessage other, double epsi if (!this.state_.epsilonEquals(other.state_, epsilon)) return false; if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false; + if (!this.kick_goal_pose_.epsilonEquals(other.kick_goal_pose_, epsilon)) return false; if (!this.left_foot_goal_pose_.epsilonEquals(other.left_foot_goal_pose_, epsilon)) return false; if (!this.right_foot_goal_pose_.epsilonEquals(other.right_foot_goal_pose_, epsilon)) return false; if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_state_, other.execution_state_, epsilon)) return false; @@ -270,6 +286,7 @@ public boolean equals(Object other) if (!this.state_.equals(otherMyClass.state_)) return false; if (!this.definition_.equals(otherMyClass.definition_)) return false; + if (!this.kick_goal_pose_.equals(otherMyClass.kick_goal_pose_)) return false; if (!this.left_foot_goal_pose_.equals(otherMyClass.left_foot_goal_pose_)) return false; if (!this.right_foot_goal_pose_.equals(otherMyClass.right_foot_goal_pose_)) return false; if(this.execution_state_ != otherMyClass.execution_state_) return false; @@ -296,6 +313,8 @@ public java.lang.String toString() builder.append(this.state_); builder.append(", "); builder.append("definition="); builder.append(this.definition_); builder.append(", "); + builder.append("kick_goal_pose="); + builder.append(this.kick_goal_pose_); builder.append(", "); builder.append("left_foot_goal_pose="); builder.append(this.left_foot_goal_pose_); builder.append(", "); builder.append("right_foot_goal_pose="); diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java index fe90aab72acf..e76e734c46cf 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorApproachPlanStateMessagePubSubType.java @@ -15,7 +15,7 @@ public class KickDoorApproachPlanStateMessagePubSubType implements us.ihmc.pubsu @Override public final java.lang.String getDefinitionChecksum() { - return "61a537c9444792482ecd68daa073eccc3b8255b17eee37ec3f1792e6a77ef49f"; + return "468c99e85dede9b13315857e14567916ea49224d68b9f2da5142a0b3e32bbadc"; } @Override @@ -60,6 +60,8 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment); + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); current_alignment += 2 + us.ihmc.idl.CDR.alignment(current_alignment, 2); @@ -93,6 +95,8 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.KickDoorAppro current_alignment += behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getKickGoalPose(), current_alignment); + current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getLeftFootGoalPose(), current_alignment); current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getRightFootGoalPose(), current_alignment); @@ -128,6 +132,7 @@ public static void write(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.write(data.getState(), cdr); behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + geometry_msgs.msg.dds.PosePubSubType.write(data.getKickGoalPose(), cdr); geometry_msgs.msg.dds.PosePubSubType.write(data.getLeftFootGoalPose(), cdr); geometry_msgs.msg.dds.PosePubSubType.write(data.getRightFootGoalPose(), cdr); cdr.write_type_9(data.getExecutionState()); @@ -152,6 +157,7 @@ public static void read(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessage d { behavior_msgs.msg.dds.ActionNodeStateMessagePubSubType.read(data.getState(), cdr); behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + geometry_msgs.msg.dds.PosePubSubType.read(data.getKickGoalPose(), cdr); geometry_msgs.msg.dds.PosePubSubType.read(data.getLeftFootGoalPose(), cdr); geometry_msgs.msg.dds.PosePubSubType.read(data.getRightFootGoalPose(), cdr); data.setExecutionState(cdr.read_type_9()); @@ -174,6 +180,8 @@ public final void serialize(behavior_msgs.msg.dds.KickDoorApproachPlanStateMessa ser.write_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); + ser.write_type_a("kick_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getKickGoalPose()); + ser.write_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); ser.write_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); @@ -196,6 +204,8 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_ms ser.read_type_a("definition", new behavior_msgs.msg.dds.KickDoorApproachPlanDefinitionMessagePubSubType(), data.getDefinition()); + ser.read_type_a("kick_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getKickGoalPose()); + ser.read_type_a("left_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getLeftFootGoalPose()); ser.read_type_a("right_foot_goal_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getRightFootGoalPose()); diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg index f7b4f5934b7c..bdf67a6f8749 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg @@ -16,6 +16,9 @@ int32 execution_mode # Specifies the side of the robot that will execute the kick. byte robot_side 255 +# The impulse with which the kick should be executed. +float64 kick_height + # The impulse with which the kick should be executed. float64 kick_impulse diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg index 6e05fda31a06..5b0a28fa0039 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -9,6 +9,9 @@ behavior_msgs/ActionNodeStateMessage state # Definition behavior_msgs/KickDoorApproachPlanDefinitionMessage definition +# Kick goal pose +geometry_msgs/Pose kick_goal_pose + # Left foot goal pose geometry_msgs/Pose left_foot_goal_pose diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg index 80c545330c15..4313f76311a8 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanDefinitionMessage.msg @@ -18,6 +18,9 @@ int32 execution_mode # Field default value 255 int8 robot_side +# The impulse with which the kick should be executed. +float64 kick_height + # The impulse with which the kick should be executed. float64 kick_impulse diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg index bf00c294f3a9..98ad8be5a893 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/KickDoorApproachPlanStateMessage.msg @@ -13,6 +13,9 @@ behavior_msgs/ActionNodeStateMessage state # Definition behavior_msgs/KickDoorApproachPlanDefinitionMessage definition +# Kick goal pose +geometry_msgs/Pose kick_goal_pose + # Left foot goal pose geometry_msgs/Pose left_foot_goal_pose From 1fc07def2a5eb78b6a275df5e040367e15de0387 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Wed, 10 Apr 2024 17:54:39 -0500 Subject: [PATCH 22/32] Got the footstep plan working in the correct frame. Added messages to the kick behavior. --- .../actions/KickDoorActionExecutionState.java | 3 +- .../actions/KickDoorActionExecutor.java | 125 ++++++++++++------ .../KickDoorApproachPlanActionExecutor.java | 17 +-- 3 files changed, 94 insertions(+), 51 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java index 233e3d2d6208..a13428f9f6a2 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java @@ -3,8 +3,9 @@ public enum KickDoorActionExecutionState { STANDING, - PREPARING_KICK_FOOT, + SWITCHING_HIGH_LEVEL_CONTROLLER_TO_KICK_CONTROLLER, EXECUTING_KICKING, + SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER, KICK_COMPLETED; public static final KickDoorActionExecutionState[] values = values(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 8d627593fe0b..f32418da90cf 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -1,15 +1,17 @@ package us.ihmc.behaviors.sequence.actions; import controller_msgs.msg.dds.HighLevelStateMessage; +import controller_msgs.msg.dds.TriggerKickMessage; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.ros2.ROS2ControllerHelper; import us.ihmc.behaviors.sequence.ActionNodeExecutor; import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; +import us.ihmc.commons.time.Stopwatch; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; @@ -27,15 +29,22 @@ public class KickDoorActionExecutor extends ActionNodeExecutor + case SWITCHING_HIGH_LEVEL_CONTROLLER_TO_KICK_CONTROLLER -> { - //Move the kicking foot to the desired location - state.setIsExecuting(true); + stopwatch.start(); + if (!switchToKickControllerMessageSent) + { + // The kicking controller must be added with a boolean in the controller factory. + changeHighLevelState(HighLevelControllerName.CUSTOM1); + switchToKickControllerMessageSent = true; + } + + if (stopwatch.lapElapsed() >= 0.2) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.EXECUTING_KICKING); + } } case EXECUTING_KICKING -> { //Execute the kick + state.setIsExecuting(true); + stopwatch.reset(); + + if (!kickingMessageSent) + { + ros2ControllerHelper.publishToController(kickCommand); + kickingMessageSent = true; + } + + if (stopwatch.lapElapsed() >= 2.0) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.KICK_COMPLETED); + state.setIsExecuting(false); + } } - case KICK_COMPLETED -> + case SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER -> { - //Do nothing - state.setIsExecuting(false); + stopwatch.reset(); + if (!switchToWalkControllerMessageSent) + { + changeHighLevelState(HighLevelControllerName.WALKING); + switchToWalkControllerMessageSent = true; + } + + if (stopwatch.lapElapsed() >= 0.2) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.STANDING); + } } } } @@ -106,21 +159,19 @@ private void changeHighLevelState(HighLevelControllerName highLevelControllerNam ros2ControllerHelper.publishToController(highLevelStateMessage); } - /** - * Moves the kicking foot to a new step location based on the output of the {@link us.ihmc.closedSourceControl KickDynamicPlanner}. - * */ - private void prepareKickFoot(FramePoint2DReadOnly desiredSwingFootStartNominal) + public void computeTriggerKickCommand(FramePose3DReadOnly doorHandlePose, + double kickHeight, + double desiredKickDistance, + double desiredKickImpulse, + double desiredPrekickWeightDistribution, + TriggerKickCommand commandToPack) { -// // Create the footstep message and send it to the command input manager. -// FootstepDataListMessage footsteps = HumanoidMessageTools.createFootstepDataListMessage(); -// ReferenceFrame pelvisFrame = avatarSimulation.getControllerFullRobotModel().getPelvis().getBodyFixedFrame(); -// FramePoint3D location = new FramePoint3D(desiredSwingFootStartNominal); -// location.setZ(0.0); -// FrameQuaternion orientation = new FrameQuaternion(pelvisFrame, new Quaternion(0.0, 0.0, 0.0, 1.0)); -// orientation.changeFrame(ReferenceFrame.getWorldFrame()); -// FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(definition.getKickSide(), location, orientation); -// footsteps.getFootstepDataList().add().set(footstepData); -// ros2ControllerHelper.publishToController(footsteps); -// avatarSimulation.getSimulationConstructionSet().simulateNow(2.0); + TriggerKickMessage kickMessage = new TriggerKickMessage(); + kickMessage.setRobotSide(kickSide.toByte()); + kickMessage.setKickHeight(kickHeight); + kickMessage.setKickImpulse(desiredKickImpulse); + kickMessage.setKickTargetDistance(desiredKickDistance); + kickMessage.setPrekickWeightDistribution(desiredPrekickWeightDistribution); + commandToPack.setFromMessage(kickMessage); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 545763124717..942e11266bcf 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -96,13 +96,15 @@ public KickDoorApproachPlanActionExecutor(long id, this.footstepPlanner = footstepPlanner; this.footstepPlannerParameters = footstepPlannerParameters; - stateParentZUpFrame = new ZUpFrame(referenceFrameLibrary.findFrameByName("RightDoorLeverHandle"), "StateParentZUpFrame"); + stateParentZUpFrame = new ZUpFrame(referenceFrameLibrary.findFrameByName(definition.getParentFrameName()), "StateParentZUpFrame"); for (RobotSide robotSide : RobotSide.values) { - PoseReferenceFrame soleFrameForPlanning = new PoseReferenceFrame(robotSide.getLowerCaseName() + "SoleFrameForPlanning", ReferenceFrame.getWorldFrame()); + PoseReferenceFrame soleFrameForPlanning = new PoseReferenceFrame(robotSide.getLowerCaseName() + "SoleFrameForPlanning", + ReferenceFrame.getWorldFrame()); soleFramesForPlanning.put(robotSide, soleFrameForPlanning); } + this.kickDynamicPlanner = new KickDynamicPlanner(kickParameters, soleFramesForPlanning, centerOfMassControlFrameForPlanning, @@ -227,13 +229,8 @@ private void computeSquaredUpGoalFootPosesFromDoorHandle() FramePose3D stanceFootPose = new FramePose3D(); stanceFootPose.setToZero(stateParentZUpFrame); - //FIXME: This is a temp fix until we get stateParentZUpFrame to be the correct frame. -// stanceFootPose.setX(-definition.getKickTargetDistance().getValue()); -// stanceFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); -// stanceFootPose.getOrientation().setToYawOrientation(Math.PI); stanceFootPose.setX(definition.getKickTargetDistance().getValue()); stanceFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue() + definition.getStanceFootWidth().getValue())); -// stanceFootPose.getOrientation().setToYawOrientation(Math.PI); stanceFootPose.changeFrame(ReferenceFrame.getWorldFrame()); stanceFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); @@ -255,18 +252,12 @@ private void updateGoalPosesFromDynamicPlanner() FramePose3D stanceGoalPose = new FramePose3D(soleFramesForPlanning.get(stanceSide)); stanceGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); -// stanceGoalPose.setIncludingFrame(stateParentZUpFrame, stanceGoalPose.getPosition(), stanceGoalPose.getOrientation()); -// stanceGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); -// stanceGoalPose.setZ(0.0); liveGoalFeetPoses.get(stanceSide).setIncludingFrame(stanceGoalPose); FramePose3D kickStartPose = new FramePose3D(soleFramesForPlanning.get(kickSide)); kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); kickStartPose.getPosition().set(kickDynamicPlanner.getDesiredSwingFootStartNominal()); -// kickStartPose.setIncludingFrame(stateParentZUpFrame, kickStartPose.getPosition(), kickStartPose.getOrientation()); -// kickStartPose.changeFrame(ReferenceFrame.getWorldFrame()); -// kickStartPose.setZ(0.0); liveGoalFeetPoses.get(kickSide).setIncludingFrame(kickStartPose); } From 25f129c4c3d1c81bf22c5579f14ef4b61a7c1109 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Thu, 11 Apr 2024 10:57:12 -0500 Subject: [PATCH 23/32] Added an update for the parent frame in the state. The frame is getting removed from the tree somewhere, added a check for that in the update. --- .../KickDoorApproachPlanActionExecutor.java | 22 +++++++++---------- .../KickDoorApproachPlanActionState.java | 3 ++- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 942e11266bcf..a8f0484dab29 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -66,7 +66,8 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor soleFramesForPlanning = new SideDependentList<>(); - private final PoseReferenceFrame centerOfMassControlFrameForPlanning = new PoseReferenceFrame("CenterOfMassControlFrameForPlanning", ReferenceFrame.getWorldFrame()); + private final PoseReferenceFrame centerOfMassControlFrameForPlanning = new PoseReferenceFrame("CenterOfMassControlFrameForPlanning", + ReferenceFrame.getWorldFrame()); private static final double gravityZ = 9.81; // This needs to be positive for the planner to work private final KickDynamicPlanner kickDynamicPlanner; @@ -96,7 +97,7 @@ public KickDoorApproachPlanActionExecutor(long id, this.footstepPlanner = footstepPlanner; this.footstepPlannerParameters = footstepPlannerParameters; - stateParentZUpFrame = new ZUpFrame(referenceFrameLibrary.findFrameByName(definition.getParentFrameName()), "StateParentZUpFrame"); + stateParentZUpFrame = new ZUpFrame(state.getParentFrame(), "StateParentZUpFrame"); for (RobotSide robotSide : RobotSide.values) { @@ -115,11 +116,15 @@ public KickDoorApproachPlanActionExecutor(long id, private void updateStateParentZUpFrame() { - // the parent frame may have been changed. - if (stateParentZUpFrame.getParent() != state.getParentFrame()) + // check that the frame is still in the tree + if (referenceFrameLibrary.findFrameByName(definition.getParentFrameName()) != null) { - stateParentZUpFrame.remove(); - stateParentZUpFrame = new ZUpFrame(state.getParentFrame(), "StateParentZUpFrame"); + // the parent frame may have been changed. + if (stateParentZUpFrame.getParent() != state.getParentFrame()) + { + stateParentZUpFrame.remove(); + stateParentZUpFrame = new ZUpFrame(state.getParentFrame(), "StateParentZUpFrame"); + } } stateParentZUpFrame.update(); } @@ -210,13 +215,8 @@ private void computeSquaredUpGoalFootPosesFromDoorHandle() FramePose3D preKickFootPose = new FramePose3D(); preKickFootPose.setToZero(stateParentZUpFrame); - //FIXME: This is a temp fix until we get stateParentZUpFrame to be the correct frame. -// kickFootPose.setX(-definition.getKickTargetDistance().getValue()); -// kickFootPose.setY(kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); -// kickFootPose.getOrientation().setToYawOrientation(Math.PI); preKickFootPose.setX(definition.getKickTargetDistance().getValue()); preKickFootPose.setY(-kickSide.negateIfRightSide(definition.getHorizontalDistanceFromHandle().getValue())); -// kickFootPose.getOrientation().setToYawOrientation(Math.PI); preKickFootPose.changeFrame(ReferenceFrame.getWorldFrame()); preKickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java index bf0b15e10e85..b60aa998e270 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java @@ -17,7 +17,7 @@ public class KickDoorApproachPlanActionState extends ActionNodeState desiredFootPoses = new SideDependentList<>(); @@ -50,6 +50,7 @@ public KickDoorApproachPlanActionState(long id, CRDTInfo crdtInfo, WorkspaceReso @Override public void update() { + parentFrame = referenceFrameLibrary.findFrameByName(definition.getParentFrameName()); } public void toMessage(KickDoorApproachPlanStateMessage message) From 5de4d465b3d33c00bc1b33388401230d0fc09ba7 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Thu, 11 Apr 2024 12:06:27 -0500 Subject: [PATCH 24/32] Fixed the timer. --- .../actions/KickDoorActionExecutor.java | 35 +++++++++---------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index f32418da90cf..82869df704f6 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -7,14 +7,14 @@ import us.ihmc.behaviors.sequence.ActionNodeExecutor; import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; -import us.ihmc.commons.time.Stopwatch; +import us.ihmc.commons.Conversions; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.tools.NonWallTimer; import us.ihmc.tools.io.WorkspaceResourceDirectory; public class KickDoorActionExecutor extends ActionNodeExecutor @@ -33,9 +33,8 @@ public class KickDoorActionExecutor extends ActionNodeExecutor { - stopwatch.start(); if (!switchToKickControllerMessageSent) { // The kicking controller must be added with a boolean in the controller factory. changeHighLevelState(HighLevelControllerName.CUSTOM1); switchToKickControllerMessageSent = true; + stopwatch.reset(); } - if (stopwatch.lapElapsed() >= 0.2) + if (stopwatch.getElapsedTime() >= 0.2) { + stopwatch.reset(); state.getExecutionState().setValue(KickDoorActionExecutionState.EXECUTING_KICKING); } } @@ -120,32 +119,33 @@ public void updateCurrentlyExecuting() { //Execute the kick state.setIsExecuting(true); - stopwatch.reset(); if (!kickingMessageSent) { - ros2ControllerHelper.publishToController(kickCommand); + stopwatch.reset(); kickingMessageSent = true; + ros2ControllerHelper.publishToController(kickCommand); } - if (stopwatch.lapElapsed() >= 2.0) + if (stopwatch.getElapsedTime() >= 2.0) { - state.getExecutionState().setValue(KickDoorActionExecutionState.KICK_COMPLETED); state.setIsExecuting(false); + stopwatch.reset(); + state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER); } } case SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER -> { - stopwatch.reset(); if (!switchToWalkControllerMessageSent) { changeHighLevelState(HighLevelControllerName.WALKING); switchToWalkControllerMessageSent = true; } - if (stopwatch.lapElapsed() >= 0.2) + if (stopwatch.getElapsedTime() >= 0.2) { state.getExecutionState().setValue(KickDoorActionExecutionState.STANDING); + stopwatch.reset(); } } } @@ -159,8 +159,7 @@ private void changeHighLevelState(HighLevelControllerName highLevelControllerNam ros2ControllerHelper.publishToController(highLevelStateMessage); } - public void computeTriggerKickCommand(FramePose3DReadOnly doorHandlePose, - double kickHeight, + public void computeTriggerKickCommand(double kickHeight, double desiredKickDistance, double desiredKickImpulse, double desiredPrekickWeightDistribution, From 2332d152ed4709583f268cd8211a4bb1f9a2a8c3 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Mon, 15 Apr 2024 18:27:21 -0500 Subject: [PATCH 25/32] Added steps for squaring up. Need to debug a reference frame going missing from the KickDoorApproachPlanActionDefinition before we can check the step locations. --- .../actions/KickDoorActionDefinition.java | 17 +++++- .../actions/KickDoorActionExecutionState.java | 8 +-- .../actions/KickDoorActionExecutor.java | 58 ++++++++++++++++--- .../KickDoorApproachPlanActionDefinition.java | 2 +- .../msg/KickDoorActionDefinitionMessage_.idl | 4 ++ .../BehaviorTreeStateMessagePubSubType.java | 2 +- .../dds/KickDoorActionDefinitionMessage.java | 27 +++++++++ ...DoorActionDefinitionMessagePubSubType.java | 13 ++++- .../KickDoorActionStateMessagePubSubType.java | 2 +- .../msg/KickDoorActionDefinitionMessage.msg | 3 + .../msg/KickDoorActionDefinitionMessage.msg | 3 + 11 files changed, 122 insertions(+), 17 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java index 23e6b579ac62..b19593817c22 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -17,6 +17,7 @@ public class KickDoorActionDefinition extends ActionNodeDefinition implements Si public static final double KICK_IMPULSE = 55.0; public static final double KICK_TARGET_DISTANCE = 0.75; public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; + public static final double STANCE_FOOT_WIDTH = 0.23; private final CRDTUnidirectionalString parentFrameName; @@ -25,6 +26,7 @@ public class KickDoorActionDefinition extends ActionNodeDefinition implements Si private final CRDTUnidirectionalDouble kickImpulse; private final CRDTUnidirectionalDouble kickTargetDistance; private final CRDTUnidirectionalDouble prekickWeightDistribution; + private final CRDTUnidirectionalDouble stanceFootWidth; // On disk fields private String onDiskParentFrameName; @@ -33,6 +35,7 @@ public class KickDoorActionDefinition extends ActionNodeDefinition implements Si private double onDiskKickImpulse; private double onDiskKickTargetDistance; private double onDiskPrekickWeightDistribution; + private double onDiskStanceFootWidth; public KickDoorActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory) { @@ -45,7 +48,7 @@ public KickDoorActionDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory sa kickImpulse = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_IMPULSE); kickTargetDistance = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, KICK_TARGET_DISTANCE); prekickWeightDistribution = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, PREKICK_WEIGHT_DISTRIBUTION); - + stanceFootWidth = new CRDTUnidirectionalDouble(ROS2ActorDesignation.OPERATOR, crdtInfo, STANCE_FOOT_WIDTH); } @Override @@ -60,6 +63,7 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("kickImpulse", kickImpulse.getValue()); jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); + jsonNode.put("stanceFootWidth", stanceFootWidth.getValue()); } @Override @@ -74,6 +78,7 @@ public void loadFromFile(JsonNode jsonNode) kickImpulse.setValue(jsonNode.get("kickImpulse").asDouble()); kickTargetDistance.setValue(jsonNode.get("kickTargetDistance").asDouble()); prekickWeightDistribution.setValue(jsonNode.get("prekickWeightDistribution").asDouble()); + stanceFootWidth.setValue(jsonNode.get("stanceFootWidth").asDouble()); } @Override @@ -88,6 +93,7 @@ public void setOnDiskFields() onDiskKickImpulse = kickImpulse.getValue(); onDiskKickTargetDistance = kickTargetDistance.getValue(); onDiskPrekickWeightDistribution = prekickWeightDistribution.getValue(); + onDiskStanceFootWidth = stanceFootWidth.getValue(); } @Override @@ -102,6 +108,7 @@ public void undoAllNontopologicalChanges() kickImpulse.setValue(onDiskKickImpulse); kickTargetDistance.setValue(onDiskKickTargetDistance); prekickWeightDistribution.setValue(onDiskPrekickWeightDistribution); + stanceFootWidth.setValue(onDiskStanceFootWidth); } @Override @@ -116,6 +123,7 @@ public boolean hasChanges() unchanged &= kickImpulse.getValue() == onDiskKickImpulse; unchanged &= kickTargetDistance.getValue() == onDiskKickTargetDistance; unchanged &= prekickWeightDistribution.getValue() == onDiskPrekickWeightDistribution; + unchanged &= stanceFootWidth.getValue() == onDiskStanceFootWidth; return !unchanged; } @@ -131,6 +139,7 @@ public void toMessage(KickDoorActionDefinitionMessage message) message.setKickImpulse(kickImpulse.getValue()); message.setKickTargetDistance(kickTargetDistance.getValue()); message.setPrekickWeightDistribution(prekickWeightDistribution.getValue()); + message.setStanceFootWidth(stanceFootWidth.getValue()); } public void fromMessage(KickDoorActionDefinitionMessage message) @@ -144,6 +153,7 @@ public void fromMessage(KickDoorActionDefinitionMessage message) kickImpulse.fromMessage(message.getKickImpulse()); kickTargetDistance.fromMessage(message.getKickTargetDistance()); prekickWeightDistribution.fromMessage(message.getPrekickWeightDistribution()); + stanceFootWidth.fromMessage(message.getStanceFootWidth()); } @Override @@ -206,4 +216,9 @@ public void setPrekickWeightDistribution(double prekickWeightDistribution) { this.prekickWeightDistribution.setValue(prekickWeightDistribution); } + + public double getStanceFootWidth() + { + return stanceFootWidth.getValue(); + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java index a13428f9f6a2..7073c79d479c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutionState.java @@ -3,10 +3,10 @@ public enum KickDoorActionExecutionState { STANDING, - SWITCHING_HIGH_LEVEL_CONTROLLER_TO_KICK_CONTROLLER, - EXECUTING_KICKING, - SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER, - KICK_COMPLETED; + SWITCHING_TO_KICK_CONTROLLER, + KICKING, + SWITCHING_TO_WALKING_CONTROLLER, + SQUARING_UP; public static final KickDoorActionExecutionState[] values = values(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 82869df704f6..7185fd01f4f0 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -1,5 +1,7 @@ package us.ihmc.behaviors.sequence.actions; +import controller_msgs.msg.dds.FootstepDataListMessage; +import controller_msgs.msg.dds.FootstepDataMessage; import controller_msgs.msg.dds.HighLevelStateMessage; import controller_msgs.msg.dds.TriggerKickMessage; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; @@ -11,7 +13,9 @@ import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; +import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; +import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.tools.NonWallTimer; @@ -32,9 +36,11 @@ public class KickDoorActionExecutor extends ActionNodeExecutor + case SWITCHING_TO_KICK_CONTROLLER -> { if (!switchToKickControllerMessageSent) { // The kicking controller must be added with a boolean in the controller factory. + state.getLogger().info("Switching to kick controller message."); changeHighLevelState(HighLevelControllerName.CUSTOM1); switchToKickControllerMessageSent = true; stopwatch.reset(); @@ -112,10 +119,10 @@ public void updateCurrentlyExecuting() if (stopwatch.getElapsedTime() >= 0.2) { stopwatch.reset(); - state.getExecutionState().setValue(KickDoorActionExecutionState.EXECUTING_KICKING); + state.getExecutionState().setValue(KickDoorActionExecutionState.KICKING); } } - case EXECUTING_KICKING -> + case KICKING -> { //Execute the kick state.setIsExecuting(true); @@ -124,28 +131,50 @@ public void updateCurrentlyExecuting() { stopwatch.reset(); kickingMessageSent = true; - ros2ControllerHelper.publishToController(kickCommand); + state.getLogger().info("Executing kick."); +// ros2ControllerHelper.publishToController(kickCommand); } if (stopwatch.getElapsedTime() >= 2.0) { - state.setIsExecuting(false); + state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_TO_WALKING_CONTROLLER); stopwatch.reset(); - state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER); } } - case SWITCHING_HIGH_LEVEL_CONTROLLER_TO_WALKING_CONTROLLER -> + case SWITCHING_TO_WALKING_CONTROLLER -> { if (!switchToWalkControllerMessageSent) { + stopwatch.reset(); + state.getLogger().info("Sending switch to walking controller message."); changeHighLevelState(HighLevelControllerName.WALKING); switchToWalkControllerMessageSent = true; } if (stopwatch.getElapsedTime() >= 0.2) + { + state.getExecutionState().setValue(KickDoorActionExecutionState.SQUARING_UP); + stopwatch.reset(); + } + } + case SQUARING_UP -> + { + if (!squareUpFootstepsSent) + { + stopwatch.reset(); + state.getLogger().info("Sending square up footsteps."); + computeSquaredUpFootsteps(); + ros2ControllerHelper.publishToController(footstepDataListMessage); + squareUpFootstepsSent = true; + } + + if (stopwatch.getElapsedTime() >= 4.0) { state.getExecutionState().setValue(KickDoorActionExecutionState.STANDING); + state.setIsExecuting(false); stopwatch.reset(); + state.getLogger().info("Going back to standing state."); + state.setIsExecuting(false); } } } @@ -173,4 +202,17 @@ public void computeTriggerKickCommand(double kickHeight, kickMessage.setPrekickWeightDistribution(desiredPrekickWeightDistribution); commandToPack.setFromMessage(kickMessage); } + + public void computeSquaredUpFootsteps() + { + FramePose3D kickFootGoalPose = new FramePose3D(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame)); + kickFootGoalPose.setY(kickSide.negateIfRightSide(definition.getStanceFootWidth())); + FootstepDataMessage kickFootStep = HumanoidMessageTools.createFootstepDataMessage(kickSide, kickFootGoalPose); + footstepDataListMessage.getFootstepDataList().add().set(kickFootStep); + + FramePose3D supportFootGoalPose = new FramePose3D(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame)); + supportFootGoalPose.setY(kickSide.negateIfRightSide(-definition.getStanceFootWidth())); + FootstepDataMessage supportFootStep = HumanoidMessageTools.createFootstepDataMessage(kickSide.getOppositeSide(), supportFootGoalPose); + footstepDataListMessage.getFootstepDataList().add().set(supportFootStep); + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java index d5dec5297ca6..2029c37819d7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -18,8 +18,8 @@ public class KickDoorApproachPlanActionDefinition extends ActionNodeDefinition implements SidedObject { public static final double KICK_IMPULSE = 55.0; - public static final double KICK_TARGET_DISTANCE = 0.75; public static final double KICK_HEIGHT = 0.55; + public static final double KICK_TARGET_DISTANCE = 0.75; public static final double PREKICK_WEIGHT_DISTRIBUTION = 0.5; public static final double HORIZONTAL_DISTANCE_FROM_HANDLE = 0.1; public static final double STANCE_FOOT_WIDTH = 0.23; diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl index 126ff5d40ec5..c9ddb192ba18 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/KickDoorActionDefinitionMessage_.idl @@ -37,6 +37,10 @@ module behavior_msgs * The target distance from the robot to where the kick should be aimed. */ double kick_target_distance; + /** + * The stance foot width. + */ + double stance_foot_width; /** * Weight distribution before the kick. 1.0 means all weight on the kicking foot. Default is 0.5. */ 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 a22ae0946b58..73482b344f37 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 "dad0fc97d5b513ec3323b9fcc1c800e2bcd7b970f2b41e18aa1a8a904e86fcc9"; + return "2a1141fd6462a134a96c0efdf00da2047b46e69bad700595ae9631c0b9e95f39"; } @Override diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java index 13d13e3c265a..bfd2d0075086 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/KickDoorActionDefinitionMessage.java @@ -32,6 +32,10 @@ public class KickDoorActionDefinitionMessage extends Packet Date: Tue, 16 Apr 2024 12:24:04 -0500 Subject: [PATCH 26/32] Corrected the square up steps. Hopefully fixed the reference frame null pointer (it might still get removed from the tree but we don't look for it in the update anymore). --- .../actions/KickDoorActionExecutor.java | 27 ++++++++++++------- .../KickDoorApproachPlanActionExecutor.java | 2 ++ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 7185fd01f4f0..3422595dc8fe 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -12,10 +12,11 @@ import us.ihmc.commons.Conversions; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; -import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; +import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.tools.NonWallTimer; @@ -66,7 +67,8 @@ public KickDoorActionExecutor(long id, computeTriggerKickCommand(definition.getKickHeight(), definition.getKickTargetDistance(), definition.getKickImpulse(), - definition.getPrekickWeightDistribution(), kickCommand); + definition.getPrekickWeightDistribution(), + kickCommand); } /** @@ -132,10 +134,10 @@ public void updateCurrentlyExecuting() stopwatch.reset(); kickingMessageSent = true; state.getLogger().info("Executing kick."); -// ros2ControllerHelper.publishToController(kickCommand); + // ros2ControllerHelper.publishToController(kickCommand); } - if (stopwatch.getElapsedTime() >= 2.0) + if (stopwatch.getElapsedTime() >= 2.0) { state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_TO_WALKING_CONTROLLER); stopwatch.reset(); @@ -151,7 +153,7 @@ public void updateCurrentlyExecuting() switchToWalkControllerMessageSent = true; } - if (stopwatch.getElapsedTime() >= 0.2) + if (stopwatch.getElapsedTime() >= 0.2) { state.getExecutionState().setValue(KickDoorActionExecutionState.SQUARING_UP); stopwatch.reset(); @@ -168,7 +170,7 @@ public void updateCurrentlyExecuting() squareUpFootstepsSent = true; } - if (stopwatch.getElapsedTime() >= 4.0) + if (stopwatch.getElapsedTime() >= 4.0) { state.getExecutionState().setValue(KickDoorActionExecutionState.STANDING); state.setIsExecuting(false); @@ -205,13 +207,18 @@ public void computeTriggerKickCommand(double kickHeight, public void computeSquaredUpFootsteps() { - FramePose3D kickFootGoalPose = new FramePose3D(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame)); - kickFootGoalPose.setY(kickSide.negateIfRightSide(definition.getStanceFootWidth())); + MovingReferenceFrame pelvisFrame = syncedRobot.getReferenceFrames().getMidFeetUnderPelvisFrame(); + FramePose3D kickFootGoalPose = new FramePose3D(pelvisFrame); + kickFootGoalPose.setY(kickSide.negateIfRightSide(definition.getStanceFootWidth() / 2.0)); + kickFootGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); + kickFootGoalPose.setZ(0.0); FootstepDataMessage kickFootStep = HumanoidMessageTools.createFootstepDataMessage(kickSide, kickFootGoalPose); footstepDataListMessage.getFootstepDataList().add().set(kickFootStep); - FramePose3D supportFootGoalPose = new FramePose3D(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame)); - supportFootGoalPose.setY(kickSide.negateIfRightSide(-definition.getStanceFootWidth())); + FramePose3D supportFootGoalPose = new FramePose3D(pelvisFrame); + supportFootGoalPose.setY(kickSide.negateIfRightSide(-definition.getStanceFootWidth() / 2.0)); + supportFootGoalPose.changeFrame(ReferenceFrame.getWorldFrame()); + supportFootGoalPose.setZ(0.0); FootstepDataMessage supportFootStep = HumanoidMessageTools.createFootstepDataMessage(kickSide.getOppositeSide(), supportFootGoalPose); footstepDataListMessage.getFootstepDataList().add().set(supportFootStep); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index a8f0484dab29..60276d340e7d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -293,6 +293,8 @@ public void triggerActionExecution() @Override public void updateCurrentlyExecuting() { + updateStateParentZUpFrame(); + switch (state.getExecutionState().getValue()) { case FOOTSTEP_PLANNING -> From 6ac0d599f723f3bc4375de5b708e0d5fe972fcd2 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Tue, 16 Apr 2024 14:15:33 -0500 Subject: [PATCH 27/32] Switched to using a detachable reference frame to keep with behavior conventions and avoid null pointers. --- .../KickDoorApproachPlanActionExecutor.java | 142 ++++++++---------- .../KickDoorApproachPlanActionState.java | 17 ++- 2 files changed, 78 insertions(+), 81 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 60276d340e7d..0b566de10f42 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -24,9 +24,9 @@ import us.ihmc.footstepPlanning.tools.FootstepPlannerRejectionReasonReport; import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; +import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; import us.ihmc.robotics.referenceFrames.PoseReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; -import us.ihmc.robotics.referenceFrames.ZUpFrame; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.tools.io.WorkspaceResourceDirectory; @@ -71,7 +71,7 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor @@ -344,59 +325,64 @@ private void startFootstepPlanningAsync() footstepPlanNotification.poll(); // Make sure it's cleared footstepPlanningThread.execute(() -> - { - footstepPlannerParameters.setFinalTurnProximity(1.0); - - FootstepPlannerRequest footstepPlannerRequest = new FootstepPlannerRequest(); - footstepPlannerRequest.setPlanBodyPath(false); - footstepPlannerRequest.setStartFootPoses(startFootPosesForThread.get(RobotSide.LEFT), startFootPosesForThread.get(RobotSide.RIGHT)); - // TODO: Set start footholds!! - for (RobotSide side : RobotSide.values) - { - footstepPlannerRequest.setGoalFootPose(side, goalFootPosesForThread.get(side)); - } - - footstepPlannerRequest.setAssumeFlatGround(true); // TODO: Incorporate height map - - footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); - double idealFootstepLength = 0.5; - footstepPlanner.getFootstepPlannerParameters().setIdealFootstepLength(idealFootstepLength); - footstepPlanner.getFootstepPlannerParameters().setMaximumStepReach(idealFootstepLength); - state.getLogger().info("Planning footsteps..."); - FootstepPlannerOutput footstepPlannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); - FootstepPlan footstepPlan = footstepPlannerOutput.getFootstepPlan(); - state.getLogger().info("Footstep planner completed with {}, {} step(s)", footstepPlannerOutput.getFootstepPlanningResult(), footstepPlan.getNumberOfSteps()); - - if (footstepPlan.getNumberOfSteps() < 1) // failed - { - FootstepPlannerRejectionReasonReport rejectionReasonReport = new FootstepPlannerRejectionReasonReport(footstepPlanner); - rejectionReasonReport.update(); - for (BipedalFootstepPlannerNodeRejectionReason reason : rejectionReasonReport.getSortedReasons()) - { - double rejectionPercentage = rejectionReasonReport.getRejectionReasonPercentage(reason); - state.getLogger().info("Rejection {}%: {}", FormattingTools.getFormattedToSignificantFigures(rejectionPercentage, 3), reason); - } - state.getLogger().info("Footstep planning failure..."); - footstepPlanNotification.set(new FootstepPlan()); - } - else - { - for (int i = 0; i < footstepPlan.getNumberOfSteps(); i++) - { - if (i == 0) - footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration() / 2.0); - else - footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration()); - - footstepPlan.getFootstep(i).setSwingDuration(getDefinition().getSwingDuration()); - } - footstepPlanNotification.set(new FootstepPlan(footstepPlan)); // Copy of the output to be safe - } - - FootstepPlannerLogger footstepPlannerLogger = new FootstepPlannerLogger(footstepPlanner); - footstepPlannerLogger.logSession(); - FootstepPlannerLogger.deleteOldLogs(); - }, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE); + { + footstepPlannerParameters.setFinalTurnProximity(1.0); + + FootstepPlannerRequest footstepPlannerRequest = new FootstepPlannerRequest(); + footstepPlannerRequest.setPlanBodyPath(false); + footstepPlannerRequest.setStartFootPoses(startFootPosesForThread.get(RobotSide.LEFT), + startFootPosesForThread.get(RobotSide.RIGHT)); + // TODO: Set start footholds!! + for (RobotSide side : RobotSide.values) + { + footstepPlannerRequest.setGoalFootPose(side, goalFootPosesForThread.get(side)); + } + + footstepPlannerRequest.setAssumeFlatGround(true); // TODO: Incorporate height map + + footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); + double idealFootstepLength = 0.5; + footstepPlanner.getFootstepPlannerParameters().setIdealFootstepLength(idealFootstepLength); + footstepPlanner.getFootstepPlannerParameters().setMaximumStepReach(idealFootstepLength); + state.getLogger().info("Planning footsteps..."); + FootstepPlannerOutput footstepPlannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); + FootstepPlan footstepPlan = footstepPlannerOutput.getFootstepPlan(); + state.getLogger() + .info("Footstep planner completed with {}, {} step(s)", + footstepPlannerOutput.getFootstepPlanningResult(), + footstepPlan.getNumberOfSteps()); + + if (footstepPlan.getNumberOfSteps() < 1) // failed + { + FootstepPlannerRejectionReasonReport rejectionReasonReport = new FootstepPlannerRejectionReasonReport(footstepPlanner); + rejectionReasonReport.update(); + for (BipedalFootstepPlannerNodeRejectionReason reason : rejectionReasonReport.getSortedReasons()) + { + double rejectionPercentage = rejectionReasonReport.getRejectionReasonPercentage(reason); + state.getLogger() + .info("Rejection {}%: {}", FormattingTools.getFormattedToSignificantFigures(rejectionPercentage, 3), reason); + } + state.getLogger().info("Footstep planning failure..."); + footstepPlanNotification.set(new FootstepPlan()); + } + else + { + for (int i = 0; i < footstepPlan.getNumberOfSteps(); i++) + { + if (i == 0) + footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration() / 2.0); + else + footstepPlan.getFootstep(i).setTransferDuration(getDefinition().getTransferDuration()); + + footstepPlan.getFootstep(i).setSwingDuration(getDefinition().getSwingDuration()); + } + footstepPlanNotification.set(new FootstepPlan(footstepPlan)); // Copy of the output to be safe + } + + FootstepPlannerLogger footstepPlannerLogger = new FootstepPlannerLogger(footstepPlanner); + footstepPlannerLogger.logSession(); + FootstepPlannerLogger.deleteOldLogs(); + }, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE); } private void buildAndSendCommandAndSetDesiredState() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java index b60aa998e270..df3176848e7a 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionState.java @@ -5,7 +5,10 @@ import us.ihmc.communication.crdt.*; import us.ihmc.communication.ros2.ROS2ActorDesignation; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.robotics.referenceFrames.ZUpFrame; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.tools.io.WorkspaceResourceDirectory; @@ -17,6 +20,7 @@ public class KickDoorApproachPlanActionState extends ActionNodeState Date: Wed, 17 Apr 2024 10:38:45 -0500 Subject: [PATCH 28/32] Added kicking back to the ControllerAPI and partially fixed the world frame error. --- .../controllerAPI/ControllerAPI.java | 3 +++ .../actions/KickDoorActionExecutor.java | 26 +++++++------------ .../KickDoorApproachPlanActionExecutor.java | 21 +++++++++------ 3 files changed, 26 insertions(+), 24 deletions(-) diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java index a3cbfec68955..baa0953927a6 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java @@ -76,6 +76,9 @@ public final class ControllerAPI inputMessageClasses.add(WholeBodyTrajectoryMessage.class); inputMessageClasses.add(WholeBodyStreamingMessage.class); + // Commands supported by the kicking controller + inputMessageClasses.add(TriggerKickMessage.class); + // Statuses supported by bipedal walking controller {@link WalkingControllerState} outputMessageClasses.add(CapturabilityBasedStatus.class); outputMessageClasses.add(FootstepStatusMessage.class); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 3422595dc8fe..3843870715c8 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -13,7 +13,6 @@ import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; import us.ihmc.mecano.frames.MovingReferenceFrame; @@ -31,7 +30,6 @@ public class KickDoorActionExecutor extends ActionNodeExecutor= 2.0) @@ -190,19 +187,16 @@ private void changeHighLevelState(HighLevelControllerName highLevelControllerNam ros2ControllerHelper.publishToController(highLevelStateMessage); } - public void computeTriggerKickCommand(double kickHeight, - double desiredKickDistance, - double desiredKickImpulse, - double desiredPrekickWeightDistribution, - TriggerKickCommand commandToPack) + public void computeKickMessage(double kickHeight, + double desiredKickDistance, + double desiredKickImpulse, + double desiredPrekickWeightDistribution) { - TriggerKickMessage kickMessage = new TriggerKickMessage(); kickMessage.setRobotSide(kickSide.toByte()); kickMessage.setKickHeight(kickHeight); kickMessage.setKickImpulse(desiredKickImpulse); kickMessage.setKickTargetDistance(desiredKickDistance); kickMessage.setPrekickWeightDistribution(desiredPrekickWeightDistribution); - commandToPack.setFromMessage(kickMessage); } public void computeSquaredUpFootsteps() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 0b566de10f42..7d0f54428db9 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -72,6 +72,7 @@ public class KickDoorApproachPlanActionExecutor extends ActionNodeExecutor Date: Thu, 18 Apr 2024 14:10:48 -0500 Subject: [PATCH 29/32] did some tuning on the way the behaviors worked to get things to actually run --- .../donkeyKick/KickingWBCCParameters.java | 2 +- .../actions/KickDoorActionDefinition.java | 2 +- .../actions/KickDoorActionExecutor.java | 24 +++++++++---------- .../KickDoorApproachPlanActionDefinition.java | 2 +- .../KickDoorApproachPlanActionExecutor.java | 3 +++ 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java index 8cc4e7a731dc..f339cdfc6a75 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickingWBCCParameters.java @@ -22,7 +22,7 @@ public class KickingWBCCParameters private final static double chamberFootWeight = 50.0; private final PID3DGains touchdownFootGains = new DefaultPID3DGains(); - private final static double touchdownFootWeight = 50.0; + private final static double touchdownFootWeight = 150.0; private final PID3DGains settlingFootGains = new DefaultPID3DGains(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java index b19593817c22..82d8d2a0dc66 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionDefinition.java @@ -58,7 +58,7 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("parentFrame", parentFrameName.getValue()); - jsonNode.put("side", kickSide.getValue().getLowerCaseName()); + jsonNode.put("kickSide", kickSide.getValue().getLowerCaseName()); jsonNode.put("kickHeight", kickHeight.getValue()); jsonNode.put("kickImpulse", kickImpulse.getValue()); jsonNode.put("kickTargetDistance", kickTargetDistance.getValue()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 3843870715c8..0eff570b2131 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -2,6 +2,7 @@ import controller_msgs.msg.dds.FootstepDataListMessage; import controller_msgs.msg.dds.FootstepDataMessage; +import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage; import controller_msgs.msg.dds.HighLevelStateMessage; import controller_msgs.msg.dds.TriggerKickMessage; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; @@ -15,9 +16,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; +import us.ihmc.log.LogTools; import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Input; import us.ihmc.tools.NonWallTimer; import us.ihmc.tools.io.WorkspaceResourceDirectory; @@ -62,10 +65,7 @@ public KickDoorActionExecutor(long id, this.controllerStatusTracker = controllerStatusTracker; this.walkingControllerParameters = walkingControllerParameters; - computeKickMessage(definition.getKickHeight(), - definition.getKickTargetDistance(), - definition.getKickImpulse(), - definition.getPrekickWeightDistribution()); + computeKickMessage(); } /** @@ -131,10 +131,11 @@ public void updateCurrentlyExecuting() stopwatch.reset(); kickingMessageSent = true; state.getLogger().info("Executing kick."); + computeKickMessage(); ros2ControllerHelper.publishToController(kickMessage); } - if (stopwatch.getElapsedTime() >= 2.0) + if (stopwatch.getElapsedTime() >= 6.0) { state.getExecutionState().setValue(KickDoorActionExecutionState.SWITCHING_TO_WALKING_CONTROLLER); stopwatch.reset(); @@ -187,16 +188,13 @@ private void changeHighLevelState(HighLevelControllerName highLevelControllerNam ros2ControllerHelper.publishToController(highLevelStateMessage); } - public void computeKickMessage(double kickHeight, - double desiredKickDistance, - double desiredKickImpulse, - double desiredPrekickWeightDistribution) + public void computeKickMessage() { kickMessage.setRobotSide(kickSide.toByte()); - kickMessage.setKickHeight(kickHeight); - kickMessage.setKickImpulse(desiredKickImpulse); - kickMessage.setKickTargetDistance(desiredKickDistance); - kickMessage.setPrekickWeightDistribution(desiredPrekickWeightDistribution); + kickMessage.setKickHeight(definition.getKickHeight()); + kickMessage.setKickImpulse(definition.getKickImpulse()); + kickMessage.setKickTargetDistance( definition.getKickTargetDistance()); + kickMessage.setPrekickWeightDistribution(definition.getPrekickWeightDistribution()); } public void computeSquaredUpFootsteps() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java index 2029c37819d7..22bbde0bd9b0 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionDefinition.java @@ -85,7 +85,7 @@ public void saveToFile(ObjectNode jsonNode) jsonNode.put("prekickWeightDistribution", prekickWeightDistribution.getValue()); jsonNode.put("horizontalDistanceFromHandle", horizontalDistanceFromHandle.getValue()); jsonNode.put("stanceFootWidth", stanceFootWidth.getValue()); - jsonNode.put("kickSide", kickSide.getValue().toString()); + jsonNode.put("kickSide", kickSide.getValue().getLowerCaseName()); } @Override diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java index 7d0f54428db9..242556c30353 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorApproachPlanActionExecutor.java @@ -24,6 +24,7 @@ import us.ihmc.footstepPlanning.tools.FootstepPlannerRejectionReasonReport; import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; +import us.ihmc.log.LogTools; import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame; import us.ihmc.robotics.referenceFrames.PoseReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; @@ -208,7 +209,9 @@ private void computeSquaredUpGoalFootPosesFromDoorHandle() preKickFootPose.changeFrame(worldFrame); preKickFootPose.getPosition().setZ(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getMidFeetUnderPelvisFrame).getZ()); + FramePose3D kickPose = new FramePose3D(preKickFootPose); + kickPose.changeFrame(syncedRobot.getReferenceFrames().getMidFeetZUpFrame()); kickPose.setZ(definition.getKickHeight().getValue()); kickPose.changeFrame(doorHandleFrame.getReferenceFrame()); kickPose.setX(0.0); From ce227899cd0afa9434bfb74a86adaaac696948ed Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Fri, 19 Apr 2024 15:20:30 -0500 Subject: [PATCH 30/32] Name refactor to KickDoorMessage.msg --- .../donkeyKick/KickInputParameters.java | 6 +-- .../factories/ControllerAPIDefinition.java | 2 +- .../WalkingCommandConsumer.java | 8 ++-- .../controllerAPI/ControllerAPI.java | 2 +- .../actions/KickDoorActionExecutor.java | 9 ++-- ...rKickCommand.java => KickDoorCommand.java} | 12 ++--- ...rKickMessage_.idl => KickDoorMessage_.idl} | 8 ++-- ...rKickMessage.java => KickDoorMessage.java} | 22 +++++----- ...pe.java => KickDoorMessagePubSubType.java} | 44 +++++++++---------- ...gerKickMessage.msg => KickDoorMessage.msg} | 0 ...gerKickMessage.msg => KickDoorMessage.msg} | 0 11 files changed, 55 insertions(+), 58 deletions(-) rename ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/{TriggerKickCommand.java => KickDoorCommand.java} (89%) rename ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/{TriggerKickMessage_.idl => KickDoorMessage_.idl} (86%) rename ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/{TriggerKickMessage.java => KickDoorMessage.java} (91%) rename ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/{TriggerKickMessagePubSubType.java => KickDoorMessagePubSubType.java} (71%) rename ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/{TriggerKickMessage.msg => KickDoorMessage.msg} (100%) rename ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/{TriggerKickMessage.msg => KickDoorMessage.msg} (100%) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java index 97dad93f2e7c..5db1e17172a4 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickInputParameters.java @@ -1,6 +1,6 @@ package us.ihmc.commonWalkingControlModules.donkeyKick; -import us.ihmc.humanoidRobotics.communication.controllerAPI.command.TriggerKickCommand; +import us.ihmc.humanoidRobotics.communication.controllerAPI.command.KickDoorCommand; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; @@ -24,7 +24,7 @@ public KickInputParameters(YoRegistry parentRegistry) kickTargetDistance = new YoDouble("inputKickTargetDistance", registry); prekickWeightDistribution = new YoDouble("inputPrekickWeightDistribution", registry); - // These default parameters are generally overwritten with set(TriggerKickCommand kickMessage) + // These default parameters are generally overwritten with set(KickDoorCommand kickMessage) kickFootSide.set(RobotSide.LEFT); kickHeight.set(0.55); kickImpulse.set(55.0); @@ -85,7 +85,7 @@ public void setPrekickWeightDistribution(double prekickWeightDistribution) this.prekickWeightDistribution.set(prekickWeightDistribution); } - public void set(TriggerKickCommand kickMessage) + public void set(KickDoorCommand kickMessage) { setKickFootSide(kickMessage.getRobotSide()); setKickHeight(kickMessage.getKickHeight()); diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java index 332d0a205f2c..76844cbf020e 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ControllerAPIDefinition.java @@ -70,7 +70,7 @@ public class ControllerAPIDefinition commands.add(AbortWalkingCommand.class); commands.add(PrepareForLocomotionCommand.class); commands.add(PauseWalkingCommand.class); - commands.add(TriggerKickCommand.class); + commands.add(KickDoorCommand.class); commands.add(SpineDesiredAccelerationsCommand.class); commands.add(HandLoadBearingCommand.class); commands.add(HandHybridJointspaceTaskspaceTrajectoryCommand.class); 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 fccb538a95e7..a58fda9d1b17 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 @@ -123,7 +123,7 @@ public WalkingCommandConsumer(CommandInputManager commandInputManager, commandsToRegister.add(CenterOfMassTrajectoryCommand.class); commandsToRegister.add(AbortWalkingCommand.class); commandsToRegister.add(PrepareForLocomotionCommand.class); - commandsToRegister.add(TriggerKickCommand.class); + commandsToRegister.add(KickDoorCommand.class); commandsToRegister.add(DirectionalControlInputCommand.class); commandsToRegister.add(FastWalkingGaitParametersCommand.class); @@ -573,12 +573,12 @@ public void consumeAbortWalkingCommands(YoBoolean abortWalkingRequested) abortWalkingRequested.set(commandConsumerWithDelayBuffers.pollNewestCommand(AbortWalkingCommand.class).isAbortWalkingRequested()); } - public TriggerKickCommand consumeTriggerKickCommands() + public KickDoorCommand consumeTriggerKickCommands() { - if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(TriggerKickCommand.class)) + if (!commandConsumerWithDelayBuffers.isNewCommandAvailable(KickDoorCommand.class)) return null; - TriggerKickCommand command = commandConsumerWithDelayBuffers.pollNewestCommand(TriggerKickCommand.class); + KickDoorCommand command = commandConsumerWithDelayBuffers.pollNewestCommand(KickDoorCommand.class); return command; } diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java index baa0953927a6..03ff16da39cc 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/ControllerAPI.java @@ -77,7 +77,7 @@ public final class ControllerAPI inputMessageClasses.add(WholeBodyStreamingMessage.class); // Commands supported by the kicking controller - inputMessageClasses.add(TriggerKickMessage.class); + inputMessageClasses.add(KickDoorMessage.class); // Statuses supported by bipedal walking controller {@link WalkingControllerState} outputMessageClasses.add(CapturabilityBasedStatus.class); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 0eff570b2131..72d3a4776b44 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -2,9 +2,8 @@ import controller_msgs.msg.dds.FootstepDataListMessage; import controller_msgs.msg.dds.FootstepDataMessage; -import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage; import controller_msgs.msg.dds.HighLevelStateMessage; -import controller_msgs.msg.dds.TriggerKickMessage; +import controller_msgs.msg.dds.KickDoorMessage; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.ros2.ROS2ControllerHelper; import us.ihmc.behaviors.sequence.ActionNodeExecutor; @@ -16,11 +15,9 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools; import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName; -import us.ihmc.log.LogTools; import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.ros2.ROS2Input; import us.ihmc.tools.NonWallTimer; import us.ihmc.tools.io.WorkspaceResourceDirectory; @@ -41,7 +38,7 @@ public class KickDoorActionExecutor extends ActionNodeExecutor +public class KickDoorCommand implements Command { private long sequenceId; private boolean triggerKickRequested = false; @@ -27,7 +27,7 @@ public void clear() } @Override - public void set(TriggerKickCommand other) + public void set(KickDoorCommand other) { sequenceId = other.sequenceId; triggerKickRequested = other.triggerKickRequested; @@ -39,7 +39,7 @@ public void set(TriggerKickCommand other) } @Override - public void setFromMessage(TriggerKickMessage message) + public void setFromMessage(KickDoorMessage message) { sequenceId = message.getSequenceId(); triggerKickRequested = true; @@ -106,9 +106,9 @@ public void setPrekickWeightDistribution(double prekickWeightDistribution) } @Override - public Class getMessageClass() + public Class getMessageClass() { - return TriggerKickMessage.class; + return KickDoorMessage.class; } @Override diff --git a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/KickDoorMessage_.idl similarity index 86% rename from ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl rename to ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/KickDoorMessage_.idl index 144cf4c08ed8..3283fe419ae7 100644 --- a/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/TriggerKickMessage_.idl +++ b/ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/KickDoorMessage_.idl @@ -1,5 +1,5 @@ -#ifndef __controller_msgs__msg__TriggerKickMessage__idl__ -#define __controller_msgs__msg__TriggerKickMessage__idl__ +#ifndef __controller_msgs__msg__KickDoorMessage__idl__ +#define __controller_msgs__msg__KickDoorMessage__idl__ module controller_msgs { @@ -18,8 +18,8 @@ module controller_msgs * This message is part of the IHMC whole-body controller API. * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. */ - @TypeCode(type="controller_msgs::msg::dds_::TriggerKickMessage_") - struct TriggerKickMessage + @TypeCode(type="controller_msgs::msg::dds_::KickDoorMessage_") + struct KickDoorMessage { /** * Unique ID used to identify this message, should preferably be consecutively increasing. diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessage.java similarity index 91% rename from ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java rename to ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessage.java index 806003ca4545..938b42ded91e 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessage.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessage.java @@ -10,7 +10,7 @@ * This message is part of the IHMC whole-body controller API. * This message is used to trigger a donkey (back) kick. The robot should be in an appropriate position to execute the kick. */ -public class TriggerKickMessage extends Packet implements Settable, EpsilonComparable +public class KickDoorMessage extends Packet implements Settable, EpsilonComparable { public static final byte ROBOT_SIDE_LEFT = (byte) 0; public static final byte ROBOT_SIDE_RIGHT = (byte) 1; @@ -43,17 +43,17 @@ public class TriggerKickMessage extends Packet implements Se */ public double prekick_weight_distribution_; - public TriggerKickMessage() + public KickDoorMessage() { } - public TriggerKickMessage(TriggerKickMessage other) + public KickDoorMessage(KickDoorMessage other) { this(); set(other); } - public void set(TriggerKickMessage other) + public void set(KickDoorMessage other) { sequence_id_ = other.sequence_id_; @@ -177,19 +177,19 @@ public double getPrekickWeightDistribution() } - public static Supplier getPubSubType() + public static Supplier getPubSubType() { - return TriggerKickMessagePubSubType::new; + return KickDoorMessagePubSubType::new; } @Override public Supplier getPubSubTypePacket() { - return TriggerKickMessagePubSubType::new; + return KickDoorMessagePubSubType::new; } @Override - public boolean epsilonEquals(TriggerKickMessage other, double epsilon) + public boolean epsilonEquals(KickDoorMessage other, double epsilon) { if(other == null) return false; if(other == this) return true; @@ -217,9 +217,9 @@ public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; - if(!(other instanceof TriggerKickMessage)) return false; + if(!(other instanceof KickDoorMessage)) return false; - TriggerKickMessage otherMyClass = (TriggerKickMessage) other; + KickDoorMessage otherMyClass = (KickDoorMessage) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; @@ -244,7 +244,7 @@ public java.lang.String toString() { StringBuilder builder = new StringBuilder(); - builder.append("TriggerKickMessage {"); + builder.append("KickDoorMessage {"); builder.append("sequence_id="); builder.append(this.sequence_id_); builder.append(", "); builder.append("robot_side="); diff --git a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessagePubSubType.java similarity index 71% rename from ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java rename to ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessagePubSubType.java index c718daf9b0dd..6b3988f57101 100644 --- a/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TriggerKickMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KickDoorMessagePubSubType.java @@ -2,20 +2,20 @@ /** * -* Topic data type of the struct "TriggerKickMessage" defined in "TriggerKickMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* Topic data type of the struct "KickDoorMessage" defined in "KickDoorMessage_.idl". Use this class to provide the TopicDataType to a Participant. * -* This file was automatically generated from TriggerKickMessage_.idl by us.ihmc.idl.generator.IDLGenerator. -* Do not update this file directly, edit TriggerKickMessage_.idl instead. +* This file was automatically generated from KickDoorMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit KickDoorMessage_.idl instead. * */ -public class TriggerKickMessagePubSubType implements us.ihmc.pubsub.TopicDataType +public class KickDoorMessagePubSubType implements us.ihmc.pubsub.TopicDataType { - public static final java.lang.String name = "controller_msgs::msg::dds_::TriggerKickMessage_"; + public static final java.lang.String name = "controller_msgs::msg::dds_::KickDoorMessage_"; @Override public final java.lang.String getDefinitionChecksum() { - return "60d1adda16bc13fae6bca562cfca24a13f70658c3d5a25cb2f68dd8ba55645d2"; + return "b1357ce9e113f26ceb2843a82d4f242f1fc1bb1d286d3b3469528444c5669609"; } @Override @@ -28,7 +28,7 @@ public final java.lang.String getDefinitionVersion() private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); @Override - public void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + public void serialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException { serializeCDR.serialize(serializedPayload); write(data, serializeCDR); @@ -36,7 +36,7 @@ public void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.p } @Override - public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.TriggerKickMessage data) throws java.io.IOException + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, controller_msgs.msg.dds.KickDoorMessage data) throws java.io.IOException { deserializeCDR.deserialize(serializedPayload); read(data, deserializeCDR); @@ -70,12 +70,12 @@ public static int getMaxCdrSerializedSize(int current_alignment) return current_alignment - initial_alignment; } - public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKickMessage data) + public final static int getCdrSerializedSize(controller_msgs.msg.dds.KickDoorMessage data) { return getCdrSerializedSize(data, 0); } - public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKickMessage data, int current_alignment) + public final static int getCdrSerializedSize(controller_msgs.msg.dds.KickDoorMessage data, int current_alignment) { int initial_alignment = current_alignment; @@ -104,7 +104,7 @@ public final static int getCdrSerializedSize(controller_msgs.msg.dds.TriggerKick return current_alignment - initial_alignment; } - public static void write(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + public static void write(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) { cdr.write_type_4(data.getSequenceId()); @@ -122,7 +122,7 @@ public static void write(controller_msgs.msg.dds.TriggerKickMessage data, us.ihm } - public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + public static void read(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) { data.setSequenceId(cdr.read_type_4()); @@ -142,7 +142,7 @@ public static void read(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc } @Override - public final void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.InterchangeSerializer ser) + public final void serialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.InterchangeSerializer ser) { ser.write_type_4("sequence_id", data.getSequenceId()); ser.write_type_9("robot_side", data.getRobotSide()); @@ -154,7 +154,7 @@ public final void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us. } @Override - public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.TriggerKickMessage data) + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.KickDoorMessage data) { data.setSequenceId(ser.read_type_4("sequence_id")); data.setRobotSide(ser.read_type_9("robot_side")); @@ -165,15 +165,15 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_ data.setPrekickWeightDistribution(ser.read_type_6("prekick_weight_distribution")); } - public static void staticCopy(controller_msgs.msg.dds.TriggerKickMessage src, controller_msgs.msg.dds.TriggerKickMessage dest) + public static void staticCopy(controller_msgs.msg.dds.KickDoorMessage src, controller_msgs.msg.dds.KickDoorMessage dest) { dest.set(src); } @Override - public controller_msgs.msg.dds.TriggerKickMessage createData() + public controller_msgs.msg.dds.KickDoorMessage createData() { - return new controller_msgs.msg.dds.TriggerKickMessage(); + return new controller_msgs.msg.dds.KickDoorMessage(); } @Override public int getTypeSize() @@ -187,24 +187,24 @@ public java.lang.String getName() return name; } - public void serialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + public void serialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) { write(data, cdr); } - public void deserialize(controller_msgs.msg.dds.TriggerKickMessage data, us.ihmc.idl.CDR cdr) + public void deserialize(controller_msgs.msg.dds.KickDoorMessage data, us.ihmc.idl.CDR cdr) { read(data, cdr); } - public void copy(controller_msgs.msg.dds.TriggerKickMessage src, controller_msgs.msg.dds.TriggerKickMessage dest) + public void copy(controller_msgs.msg.dds.KickDoorMessage src, controller_msgs.msg.dds.KickDoorMessage dest) { staticCopy(src, dest); } @Override - public TriggerKickMessagePubSubType newInstance() + public KickDoorMessagePubSubType newInstance() { - return new TriggerKickMessagePubSubType(); + return new KickDoorMessagePubSubType(); } } diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/KickDoorMessage.msg similarity index 100% rename from ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/TriggerKickMessage.msg rename to ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/KickDoorMessage.msg diff --git a/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg b/ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/KickDoorMessage.msg similarity index 100% rename from ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/TriggerKickMessage.msg rename to ihmc-interfaces/src/main/messages/ros1/controller_msgs/msg/KickDoorMessage.msg From 8239166e7af3a05fc8e0e5bd85371018db9bff12 Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Fri, 19 Apr 2024 15:30:38 -0500 Subject: [PATCH 31/32] Removing old comments. --- .../donkeyKick/KickDynamicPlanner.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java index 7e169eba764b..2732977ba40c 100644 --- a/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java +++ b/ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/donkeyKick/KickDynamicPlanner.java @@ -255,7 +255,6 @@ private void computePositionAfterShift(double omega) CapturePointTools.computeDesiredCapturePointPosition(omega, -kickMotionDuration, acpInWorldBeforeImpact, desiredStanceCoP, acpAfterShift); - //////// Begin "the lines above" //////// ALIPTools.computeCenterOfMassPosition(-kickMotionDuration, omega, totalMass, @@ -280,7 +279,6 @@ private void computeInitialCoPAndCoM(KickInputParameters inputParameters, double RobotSide supportSide = kickSide.getOppositeSide(); double shiftDuration = kickParameters.getShiftDuration(); double exponential = Math.exp(omega * shiftDuration); - // double initialWeightDistribution = inputParameters.getPrekickWeightDistribution(); ReferenceFrame supportSoleFrame = soleFrames.get(supportSide); // from the y icp dynamics, we can figure out what the necessary distribution between the stance and swing foot are for weight for the constant cop From 1591088f5e83f01bd91869ac684c8dfa2992248c Mon Sep 17 00:00:00 2001 From: Christian DeBuys Date: Tue, 9 Jul 2024 17:24:01 -0500 Subject: [PATCH 32/32] Fixes needed after merge due to behavior refactor. --- .../behavior/actions/RDXKickDoorAction.java | 4 ++-- .../RDXKickDoorApproachPlanAction.java | 23 ++----------------- .../tree/RDXBehaviorTreeNodeBuilder.java | 7 ++++-- .../BehaviorTreeExecutorNodeBuilder.java | 2 +- .../ros2/ROS2BehaviorTreeMessageTools.java | 7 ++---- .../actions/KickDoorActionExecutor.java | 10 +------- .../KickDoorApproachPlanActionExecutor.java | 9 ++++---- 7 files changed, 17 insertions(+), 45 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java index 8dd476c7d823..5a0386151ee3 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorAction.java @@ -6,7 +6,7 @@ import us.ihmc.behaviors.sequence.actions.KickDoorActionDefinition; import us.ihmc.behaviors.sequence.actions.KickDoorActionState; import us.ihmc.communication.crdt.CRDTInfo; -import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; +import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.rdx.imgui.ImDoubleWrapper; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.ui.RDXBaseUI; @@ -33,7 +33,7 @@ public RDXKickDoorAction(long id, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobot, ReferenceFrameLibrary referenceFrameLibrary, - FootstepPlannerParametersBasics footstepPlannerParameters) + DefaultFootstepPlannerParametersBasics footstepPlannerParameters) { super(new KickDoorActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java index 5ce8090097bd..7dbd443ef0d6 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/actions/RDXKickDoorApproachPlanAction.java @@ -1,40 +1,21 @@ package us.ihmc.rdx.ui.behavior.actions; -import com.badlogic.gdx.graphics.Color; import com.badlogic.gdx.graphics.g3d.Renderable; import com.badlogic.gdx.utils.Array; import com.badlogic.gdx.utils.Pool; import imgui.ImGui; -import imgui.type.ImBoolean; import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; -import us.ihmc.behaviors.sequence.actions.*; -import us.ihmc.commons.lists.RecyclingArrayList; -import us.ihmc.commons.thread.Notification; -import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.behaviors.sequence.actions.KickDoorApproachPlanActionDefinition; +import us.ihmc.behaviors.sequence.actions.KickDoorApproachPlanActionState; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.communication.packets.ExecutionMode; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.footstepPlanning.graphSearch.parameters.FootstepPlannerParametersBasics; -import us.ihmc.rdx.imgui.ImBooleanWrapper; import us.ihmc.rdx.imgui.ImDoubleWrapper; -import us.ihmc.rdx.imgui.ImGuiReferenceFrameLibraryCombo; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.input.ImGui3DViewInput; -import us.ihmc.rdx.mesh.RDXMutableArrowModel; -import us.ihmc.rdx.ui.RDX3DPanelTooltip; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.behavior.sequence.RDXActionNode; -import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo; -import us.ihmc.rdx.ui.gizmo.RDXSelectablePose3DGizmo; import us.ihmc.rdx.ui.graphics.RDXFootstepGraphic; -import us.ihmc.rdx.ui.widgets.ImGuiFootstepsWidget; import us.ihmc.rdx.vr.RDXVRContext; -import us.ihmc.robotics.lists.RecyclingArrayListTools; import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java index 28f7131e3722..c147208aebb6 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/tree/RDXBehaviorTreeNodeBuilder.java @@ -2,6 +2,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeDefinition; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeStateBuilder; import us.ihmc.behaviors.behaviorTree.BehaviorTreeRootNodeDefinition; @@ -11,6 +12,8 @@ import us.ihmc.behaviors.sequence.ActionNodeInitialization; import us.ihmc.behaviors.sequence.ActionSequenceDefinition; import us.ihmc.behaviors.sequence.actions.*; +import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker; +import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters; import us.ihmc.communication.crdt.CRDTInfo; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.rdx.ui.RDX3DPanel; @@ -158,11 +161,11 @@ public RDXBehaviorTreeNodeBuilder(DRCRobotModel robotModel, } if (nodeType == KickDoorActionDefinition.class) { - return new KickDoorActionDefinition() + return new RDXKickDoorAction(id, crdtInfo, saveFileDirectory, baseUI, robotModel, syncedRobot, referenceFrameLibrary, footstepPlannerParametersBasics); } if (nodeType == KickDoorApproachPlanActionDefinition.class) { - return new KickDoorApproachPlanActionDefinition() + return new RDXKickDoorApproachPlanAction(id, crdtInfo, saveFileDirectory, baseUI, robotModel, referenceFrameLibrary); } else { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java index 1acc6b851b5b..08f83d454ec7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutorNodeBuilder.java @@ -143,7 +143,7 @@ public BehaviorTreeExecutorNodeBuilder(DRCRobotModel robotModel, } if (nodeType == KickDoorActionDefinition.class) { - return new KickDoorActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, syncedRobot, controllerStatusTracker, referenceFrameLibrary, walkingControllerParameters); + return new KickDoorActionExecutor(id, crdtInfo, saveFileDirectory, ros2ControllerHelper, syncedRobot, referenceFrameLibrary); } throw new RuntimeException("Node defintion type not found: " + nodeType.getSimpleName()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java index 358b33c41d3b..21e2c286c6cd 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeMessageTools.java @@ -345,11 +345,8 @@ public static void packSubscriptionNode(byte nodeType, subscriptionNode.setBehaviorTreeNodeDefinitionMessage(kickDoorApproachPlanActionStateMessage.getDefinition().getDefinition().getDefinition()); } default -> - { - Class behaviorClass = nodeEnum.getTypeClass(); - String name = behaviorClass != null ? behaviorClass.getName() : "Undefined"; - throw new RuntimeException("Undefined behavior tree state message type " + name); - } + throw new RuntimeException("Undefined behavior tree state message type."); + } } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java index 72d3a4776b44..4b7f6ae134e7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/KickDoorActionExecutor.java @@ -28,10 +28,7 @@ public class KickDoorActionExecutor extends ActionNodeExecutor footstepPlanNotification = new TypedNotification<>(); private final SideDependentList liveGoalFeetPoses = new SideDependentList<>(() -> new FramePose3D()); @@ -84,7 +83,7 @@ public KickDoorApproachPlanActionExecutor(long id, ReferenceFrameLibrary referenceFrameLibrary, WalkingControllerParameters walkingControllerParameters, FootstepPlanningModule footstepPlanner, - FootstepPlannerParametersBasics footstepPlannerParameters) + DefaultFootstepPlannerParametersBasics footstepPlannerParameters) { super(new KickDoorApproachPlanActionState(id, crdtInfo, saveFileDirectory, referenceFrameLibrary)); @@ -351,7 +350,7 @@ private void startFootstepPlanningAsync() footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); double idealFootstepLength = 0.5; footstepPlanner.getFootstepPlannerParameters().setIdealFootstepLength(idealFootstepLength); - footstepPlanner.getFootstepPlannerParameters().setMaximumStepReach(idealFootstepLength); + footstepPlanner.getFootstepPlannerParameters().setMaxStepReach(idealFootstepLength); state.getLogger().info("Planning footsteps..."); FootstepPlannerOutput footstepPlannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); FootstepPlan footstepPlan = footstepPlannerOutput.getFootstepPlan();