<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/LinearMapping.java<|end_filename|>
package us.ihmc.robotics.functionApproximation;

import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.Arrays;

/**
 * <p>Title: LinearMapping</p>
 *
 * <p>Description: Defines a mapping from an n dimensional input space to an n dimensional output space, where the
 * transformation between the two is a simple scaling of each dimension.</p>
 *
 * @author jrebula
 * @version 1.0
 */
public class LinearMapping
{
   private ArrayList<double[]> inputDimensions;
   private ArrayList<double[]> outputDimensions;

   /**
    * Creates a mapping in n space to another n space that performs a simple scale in each dimension
    *
    * @param inputDimensions ArrayList list of length n, each element is a double array of length two, containing the min and max values for that dimension in the input space
    * @param outputDimensions ArrayList list of length n, each element is a double array of length two, containing the min and max values for that dimension in the output space
    */
   public LinearMapping(ArrayList<double[]> inputDimensions, ArrayList<double[]> outputDimensions)
   {
      this.inputDimensions = new ArrayList<double[]>();
      this.outputDimensions = new ArrayList<double[]>();

      if (inputDimensions.size() != outputDimensions.size())
      {
         throw new IllegalArgumentException("must have the same number of dimensions in the input and output spaces");
      }

      for (int i = 0; i < inputDimensions.size(); i++)
      {
         double[] inputLimits = inputDimensions.get(i);
         double[] outputLimits = outputDimensions.get(i);

         if ((inputLimits.length != 2) || (outputLimits.length != 2))
         {
            throw new IllegalArgumentException("each element of the input and output dimension arrays must be 2 (a min and a max)");
         }

         double[] inputLimitsCopy = new double[2];
         double[] outputLimitsCopy = new double[2];

         System.arraycopy(inputLimits, 0, inputLimitsCopy, 0, 2);
         System.arraycopy(outputLimits, 0, outputLimitsCopy, 0, 2);

         this.inputDimensions.add(inputLimitsCopy);
         this.outputDimensions.add(outputLimitsCopy);
      }

//    this.inputDimensions.addAll(inputDimensions);
//    this.outputDimensions.addAll(outputDimensions);
   }

   public double[] mapFromInputSpaceToOutputSpace(double[] input)
   {
      double[] output = new double[input.length];
      mapFromInputSpaceToOutputSpacePacked(input, output);

      return output;
   }

   public void writeOut(PrintWriter printWriter)
   {
      printWriter.println(inputDimensions.size());

      for (double[] inputArray : inputDimensions)
      {
         printWriter.println(Arrays.toString(inputArray));
      }

      for (double[] inputArray : outputDimensions)
      {
         printWriter.println(Arrays.toString(inputArray));
      }
   }

//   public static LinearMapping readIn(BufferedReader bufferedReader) throws IOException
//   {
//      ArrayList<double[]> inputDimensions = new ArrayList<double[]>();
//      ArrayList<double[]> outputDimensions = new ArrayList<double[]>();
//
//      int numberOfDimensions = Integer.parseInt(bufferedReader.readLine());
//      for (int i = 0; i < numberOfDimensions; i++)
//      {
//         double[] inputArray = ArrayTools.parseDoubleArray(bufferedReader);
//         inputDimensions.add(inputArray);
//      }
//
//      for (int i = 0; i < numberOfDimensions; i++)
//      {
//         double[] outputArray = ArrayTools.parseDoubleArray(bufferedReader);
//         outputDimensions.add(outputArray);
//      }
//
//      LinearMapping ret = new LinearMapping(inputDimensions, outputDimensions);
//
//      return ret;
//   }

   /**
    * Takes a vector in the input space, and copies the mapped vector in the output space into the supplied output array.
    *
    * @param input double[]
    * @param output double[]
    */
   public void mapFromInputSpaceToOutputSpacePacked(double[] input, double[] output)
   {
      if (input.length != inputDimensions.size())
      {
         throw new IllegalArgumentException(
             "must provide a vector in the input space with the same dimension of the inputDimensions vector used to create the mapping");
      }

      if (output.length != inputDimensions.size())
      {
         throw new IllegalArgumentException(
             "must provide a vector in the output space with the same dimension of the inputDimensions vector used to create the mapping");
      }

      for (int i = 0; i < inputDimensions.size(); i++)
      {
         double alpha = getAlphaFromValue(input[i], inputDimensions.get(i));
         output[i] = getValueFromAlpha(alpha, outputDimensions.get(i));
      }
   }

   public double[] mapFromOutputSpaceToInputSpace(double[] output)
   {
      double[] input = new double[output.length];
      mapFromOutputSpaceToInputSpacePacked(input, output);

      return input;
   }

   /**
    * Takes a vector in the output space, and copies the mapped vector in the input space into the supplied input array.
    * Note that the ordering of the arguments is (input, output)
    *
    * @param input double[]
    * @param output double[]
    */
   public void mapFromOutputSpaceToInputSpacePacked(double[] input, double[] output)
   {
      if (input.length != inputDimensions.size())
      {
         throw new IllegalArgumentException(
             "must provide a vector in the input space with the same dimension of the inputDimensions vector used to create the mapping");
      }

      if (output.length != inputDimensions.size())
      {
         throw new IllegalArgumentException(
             "must provide a vector in the output space with the same dimension of the inputDimensions vector used to create the mapping");
      }

      for (int i = 0; i < inputDimensions.size(); i++)
      {
         double alpha = getAlphaFromValue(output[i], outputDimensions.get(i));
         double outputValue = getValueFromAlpha(alpha, inputDimensions.get(i));
         input[i] = outputValue;
      }
   }

   private double getAlphaFromValue(double value, double[] limits)
   {
      return (value - limits[0]) / (limits[1] - limits[0]);
   }

   private double getValueFromAlpha(double alpha, double[] limits)
   {
      return limits[0] + (alpha * (limits[1] - limits[0]));
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/walking/LoadBearingRequest.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.walking;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;

public enum LoadBearingRequest
{
   @RosEnumValueDocumentation(documentation = "Request to load the given end-effector.")
   LOAD,
   @RosEnumValueDocumentation(documentation = "Request to unload the given end-effector.")
   UNLOAD;

   public static final LoadBearingRequest[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static LoadBearingRequest fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ContactableBodiesFactory.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories;

import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.ListOfPointsContactableFoot;
import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.ListOfPointsContactablePlaneBody;
import us.ihmc.commonWalkingControlModules.bipedSupportPolygons.SimpleContactPointPlaneBody;
import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactableFoot;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotModels.FullLeggedRobotModel;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.robotSide.RobotSegment;
import us.ihmc.robotics.robotSide.SegmentDependentList;
import us.ihmc.robotics.screwTheory.ScrewTools;
import us.ihmc.sensorProcessing.frames.CommonLeggedReferenceFrames;
import us.ihmc.tools.factories.FactoryTools;
import us.ihmc.tools.factories.OptionalFactoryField;
import us.ihmc.tools.factories.RequiredFactoryField;

import java.util.ArrayList;
import java.util.List;

public class ContactableBodiesFactory<E extends Enum<E> & RobotSegment<E>>
{
   private final RequiredFactoryField<SegmentDependentList<E, ? extends List<Point2D>>> feetContactPoints = new RequiredFactoryField<>("feetContactPoints");
   private final RequiredFactoryField<FullLeggedRobotModel<E>> fullRobotModel = new RequiredFactoryField<>("fullRobotModel");
   private final RequiredFactoryField<CommonLeggedReferenceFrames<E>> referenceFrames = new RequiredFactoryField<>("referenceFrames");

   private final OptionalFactoryField<SegmentDependentList<E, ? extends Point2D>> toeContactPoints = new OptionalFactoryField<>("toeContactPoints");
   private final OptionalFactoryField<SegmentDependentList<E, ? extends LineSegment2D>> toeContactLines = new OptionalFactoryField<>("toeContactLines");

   private final OptionalFactoryField<ArrayList<String>> additionalContactRigidBodyNames = new OptionalFactoryField<>("additionalContactRigidBodyNames");
   private final OptionalFactoryField<ArrayList<String>> additionalContactNames = new OptionalFactoryField<>("additionalContactNames");
   private final OptionalFactoryField<ArrayList<RigidBodyTransform>> additionalContactTransforms = new OptionalFactoryField<>("additionalContactTransforms");

   public void setFootContactPoints(SegmentDependentList<E, ? extends List<Point2D>> feetContactPoints)
   {
      this.feetContactPoints.set(feetContactPoints);
   }

   public void setFullRobotModel(FullLeggedRobotModel<E> fullRobotModel)
   {
      this.fullRobotModel.set(fullRobotModel);
   }

   public void setReferenceFrames(CommonLeggedReferenceFrames<E> referenceFrames)
   {
      this.referenceFrames.set(referenceFrames);
   }

   public void setToeContactParameters(SegmentDependentList<E, ? extends Point2D> toeContactPoints, SegmentDependentList<E, ? extends LineSegment2D> toeContactLines)
   {
      this.toeContactPoints.set(toeContactPoints);
      this.toeContactLines.set(toeContactLines);
   }

   public void addAdditionalContactPoint(String bodyName, String contactName, RigidBodyTransform transformFromParentLinkToPoint)
   {
      if (!additionalContactNames.hasValue())
      {
         additionalContactRigidBodyNames.set(new ArrayList<>());
         additionalContactNames.set(new ArrayList<>());
         additionalContactTransforms.set(new ArrayList<>());
      }
      else if (additionalContactRigidBodyNames.get().contains(bodyName))
      {
         // TODO fix this
         throw new RuntimeException("Currently only supporting one additional contact point per rigid body.");
      }

      additionalContactRigidBodyNames.get().add(bodyName);
      additionalContactNames.get().add(contactName);
      additionalContactTransforms.get().add(transformFromParentLinkToPoint);
   }

   public SegmentDependentList<E, ContactablePlaneBody> createFootContactablePlaneBodies()
   {
      FactoryTools.checkAllFactoryFieldsAreSet(this);

      FullLeggedRobotModel<E> fullRobotModel = this.fullRobotModel.get();
      CommonLeggedReferenceFrames<E> referenceFrames = this.referenceFrames.get();
      E[] robotSegments = fullRobotModel.getRobotSegments();

      SegmentDependentList<E, ContactablePlaneBody> footContactableBodies = new SegmentDependentList<>(robotSegments[0].getClassType());

      for (E segment : robotSegments)
      {
         RigidBodyBasics foot = fullRobotModel.getFoot(segment);
         ReferenceFrame soleFrame = referenceFrames.getSoleFrame(segment);
         List<Point2D> contactPointsInSoleFrame = feetContactPoints.get().get(segment);

         ListOfPointsContactablePlaneBody footContactableBody = new ListOfPointsContactablePlaneBody(foot, soleFrame, contactPointsInSoleFrame);
         footContactableBodies.put(segment, footContactableBody);
      }

      feetContactPoints.dispose();
      toeContactPoints.dispose();
      toeContactLines.dispose();

      return footContactableBodies;
   }

   public SegmentDependentList<E, ContactableFoot> createFootContactableFeet()
   {
      FactoryTools.checkAllFactoryFieldsAreSet(this);
      toeContactLines.get();
      toeContactPoints.get();

      FullLeggedRobotModel<E> fullRobotModel = this.fullRobotModel.get();
      CommonLeggedReferenceFrames<E> referenceFrames = this.referenceFrames.get();
      E[] robotSegments = fullRobotModel.getRobotSegments();

      SegmentDependentList<E, ContactableFoot> footContactableBodies = new SegmentDependentList<>(robotSegments[0].getClassType());

      for (E segment : robotSegments)
      {
         RigidBodyBasics foot = fullRobotModel.getFoot(segment);
         ReferenceFrame soleFrame = referenceFrames.getSoleFrame(segment);
         List<Point2D> contactPointsInSoleFrame = feetContactPoints.get().get(segment);

         Point2D toeOffContactPoint = toeContactPoints.get().get(segment);
         LineSegment2D toeOffContactLine = toeContactLines.get().get(segment);
         ListOfPointsContactableFoot footContactableBody = new ListOfPointsContactableFoot(foot, soleFrame, contactPointsInSoleFrame, toeOffContactPoint,
               toeOffContactLine);
         footContactableBodies.put(segment, footContactableBody);
      }

      feetContactPoints.dispose();
      toeContactPoints.dispose();
      toeContactLines.dispose();

      return footContactableBodies;
   }

   public List<ContactablePlaneBody> createAdditionalContactPoints()
   {
      ArrayList<ContactablePlaneBody> contactablePlaneBodies = new ArrayList<>();
      if (!additionalContactNames.hasValue())
         return contactablePlaneBodies;

      FullLeggedRobotModel<E> fullRobotModel = this.fullRobotModel.get();
      RigidBodyBasics[] bodies = fullRobotModel.getElevator().subtreeArray();

      for (int pointIdx = 0; pointIdx < additionalContactRigidBodyNames.get().size(); pointIdx++)
      {
         String bodyName = additionalContactRigidBodyNames.get().get(pointIdx);
         String contactName = additionalContactNames.get().get(pointIdx);
         RigidBodyTransform contactFramePoseInJoint = additionalContactTransforms.get().get(pointIdx);

         RigidBodyBasics[] rigidBodies = ScrewTools.findRigidBodiesWithNames(bodies, bodyName);

         if (rigidBodies.length == 0)
            throw new RuntimeException("Did not find body with name " + bodyName);
         if (rigidBodies.length > 1)
            throw new RuntimeException("Found multiple bodies with name " + bodyName);

         RigidBodyBasics rigidBody = rigidBodies[0];
         contactablePlaneBodies.add(new SimpleContactPointPlaneBody(contactName, rigidBody, contactFramePoseInJoint));
      }

      return contactablePlaneBodies;
   }

   public void disposeFactory()
   {
      FactoryTools.disposeFactory(this);
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/manipulation/wholeBodyTrajectory/WholeBodyTrajectoryToolboxSettings.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory;

import java.util.Random;

import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotics.robotSide.RobotSide;

public class WholeBodyTrajectoryToolboxSettings
{
   public static double timeCoefficient = 2.5;
   // TODO : will be get rid of. just for test.
   public static Random randomManager = new Random(1);

   // TODO : this is useless..?
   public static ConfigurationSpaceName[] getDefaultExplorationConfiguratSpaces(FullHumanoidRobotModel fullRobotModel, RigidBodyBasics rigidBody)
   {
      ConfigurationSpaceName[] configurationSpaces;

      if (rigidBody == fullRobotModel.getHand(RobotSide.LEFT))
      {
         configurationSpaces = new ConfigurationSpaceName[6];
         configurationSpaces[0] = ConfigurationSpaceName.X;
         configurationSpaces[1] = ConfigurationSpaceName.Y;
         configurationSpaces[2] = ConfigurationSpaceName.Z;
         configurationSpaces[3] = ConfigurationSpaceName.YAW;
         configurationSpaces[4] = ConfigurationSpaceName.PITCH;
         configurationSpaces[5] = ConfigurationSpaceName.ROLL;
      }
      else if (rigidBody == fullRobotModel.getHand(RobotSide.RIGHT))
      {
         configurationSpaces = new ConfigurationSpaceName[6];
         configurationSpaces[0] = ConfigurationSpaceName.X;
         configurationSpaces[1] = ConfigurationSpaceName.Y;
         configurationSpaces[2] = ConfigurationSpaceName.Z;
         configurationSpaces[3] = ConfigurationSpaceName.YAW;
         configurationSpaces[4] = ConfigurationSpaceName.PITCH;
         configurationSpaces[5] = ConfigurationSpaceName.ROLL;
      }
      else if (rigidBody == fullRobotModel.getPelvis())
      {
         configurationSpaces = new ConfigurationSpaceName[1];
         configurationSpaces[0] = ConfigurationSpaceName.Z;
         //         configurationSpaces[1] = ConfigurationSpaceName.YAW;
         //         configurationSpaces[2] = ConfigurationSpaceName.PITCH;
         //         configurationSpaces[3] = ConfigurationSpaceName.ROLL;
      }
      else if (rigidBody == fullRobotModel.getChest())
      {
         configurationSpaces = new ConfigurationSpaceName[3];
         configurationSpaces[0] = ConfigurationSpaceName.YAW;
         configurationSpaces[1] = ConfigurationSpaceName.PITCH;
         configurationSpaces[2] = ConfigurationSpaceName.ROLL;
      }
      else
         configurationSpaces = null;

      return configurationSpaces;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/wholeBodyTrajectoryToolboxModule/ConstrainedRigidBodyTrajectory.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.wholeBodyTrajectoryToolboxModule;

import java.util.ArrayList;
import java.util.List;

import controller_msgs.msg.dds.KinematicsToolboxRigidBodyMessage;
import gnu.trove.list.array.TDoubleArrayList;
import us.ihmc.commons.MathTools;
import us.ihmc.commons.PrintTools;
import us.ihmc.commons.RandomNumbers;
import us.ihmc.communication.packets.MessageTools;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory.ConfigurationSpaceName;
import us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory.WholeBodyTrajectoryToolboxSettings;
import us.ihmc.humanoidRobotics.communication.wholeBodyTrajectoryToolboxAPI.RigidBodyExplorationConfigurationCommand;
import us.ihmc.humanoidRobotics.communication.wholeBodyTrajectoryToolboxAPI.WaypointBasedTrajectoryCommand;
import us.ihmc.manipulation.planning.exploringSpatial.SpatialData;
import us.ihmc.manipulation.planning.exploringSpatial.SpatialNode;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.screwTheory.SelectionMatrix6D;

public class ConstrainedRigidBodyTrajectory
{
   private static final boolean VERBOSE = false;

   private final RigidBodyBasics rigidBody;

   private final TDoubleArrayList waypointTimes = new TDoubleArrayList();
   private final ArrayList<Pose3D> waypoints = new ArrayList<Pose3D>();

   private final SelectionMatrix6D trajectorySelectionMatrix = new SelectionMatrix6D();
   private final SelectionMatrix6D explorationSelectionMatrix = new SelectionMatrix6D();

   private final List<ConfigurationSpaceName> explorationConfigurationSpaces = new ArrayList<>();

   //private final TDoubleArrayList explorationRangeAmplitudes = new TDoubleArrayList();

   private final TDoubleArrayList explorationRangeUpperLimits = new TDoubleArrayList();
   private final TDoubleArrayList explorationRangeLowerLimits = new TDoubleArrayList();

   private final Pose3D controlFramePose = new Pose3D();

   private final RigidBodyTransform initialGuessResult = new RigidBodyTransform();
   private final RigidBodyTransform initialGuessHolder = new RigidBodyTransform();
   private final RigidBodyTransform initialGuessCandidate = new RigidBodyTransform();

   private final boolean hasTrajectoryCommand;

   private double weight;
   private static double DEFAULT_WEIGHT = 20.0;

   // For given trajectory and appending exploration transform.
   public ConstrainedRigidBodyTrajectory(RigidBodyBasics rigidBody, WaypointBasedTrajectoryCommand trajectoryCommand,
                                         RigidBodyExplorationConfigurationCommand explorationCommand)
   {
      this.rigidBody = rigidBody;
      if (trajectoryCommand != null)
      {
         for (int i = 0; i < trajectoryCommand.getNumberOfWaypoints(); i++)
         {
            waypointTimes.add(trajectoryCommand.getWaypointTime(i));
            waypoints.add(new Pose3D(trajectoryCommand.getWaypoint(i)));
         }
         trajectorySelectionMatrix.set(trajectoryCommand.getSelectionMatrix());

         FramePose3D controlFramePose = new FramePose3D(trajectoryCommand.getControlFramePose());
         controlFramePose.changeFrame(trajectoryCommand.getEndEffector().getBodyFixedFrame());
         this.controlFramePose.set(controlFramePose);
         this.hasTrajectoryCommand = true;
         if (Double.isNaN(trajectoryCommand.getWeight()) || trajectoryCommand.getWeight() < 0.0)
            weight = DEFAULT_WEIGHT;
         else
            weight = trajectoryCommand.getWeight();
      }
      else
      {
         waypointTimes.add(0.0);
         waypointTimes.add(Double.MAX_VALUE);
         Pose3D originOfRigidBody = new Pose3D(rigidBody.getBodyFixedFrame().getTransformToWorldFrame());
         waypoints.add(originOfRigidBody);
         waypoints.add(originOfRigidBody);
         trajectorySelectionMatrix.clearSelection();

         controlFramePose.setToZero();
         this.hasTrajectoryCommand = false;
         weight = DEFAULT_WEIGHT;
      }

      explorationSelectionMatrix.clearSelection();
      explorationConfigurationSpaces.clear();

      explorationRangeUpperLimits.reset();
      explorationRangeLowerLimits.reset();

      if (explorationCommand != null && explorationCommand.getNumberOfDegreesOfFreedomToExplore() > 0)
      {
         for (int i = 0; i < explorationCommand.getNumberOfDegreesOfFreedomToExplore(); i++)
         {
            WholeBodyTrajectoryToolboxHelper.setSelectionMatrix(explorationSelectionMatrix, explorationCommand.getDegreeOfFreedomToExplore(i), true);

            explorationRangeUpperLimits.add(explorationCommand.getExplorationRangeUpperLimits(i));
            explorationRangeLowerLimits.add(explorationCommand.getExplorationRangeLowerLimits(i));
            explorationConfigurationSpaces.add(explorationCommand.getDegreeOfFreedomToExplore(i));
         }
      }
   }

   public boolean hasTrajectoryCommand()
   {
      return hasTrajectoryCommand;
   }

   public RigidBodyBasics getRigidBody()
   {
      return rigidBody;
   }

   public SelectionMatrix6D getSelectionMatrix()
   {
      SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
      selectionMatrix.clearSelection();

      if (trajectorySelectionMatrix.getLinearPart().isXSelected() || explorationSelectionMatrix.getLinearPart().isXSelected())
         selectionMatrix.getLinearPart().selectXAxis(true);

      if (trajectorySelectionMatrix.getLinearPart().isYSelected() || explorationSelectionMatrix.getLinearPart().isYSelected())
         selectionMatrix.getLinearPart().selectYAxis(true);

      if (trajectorySelectionMatrix.getLinearPart().isZSelected() || explorationSelectionMatrix.getLinearPart().isZSelected())
         selectionMatrix.getLinearPart().selectZAxis(true);

      if (trajectorySelectionMatrix.getAngularPart().isXSelected() || explorationSelectionMatrix.getAngularPart().isXSelected())
         selectionMatrix.getAngularPart().selectXAxis(true);

      if (trajectorySelectionMatrix.getAngularPart().isYSelected() || explorationSelectionMatrix.getAngularPart().isYSelected())
         selectionMatrix.getAngularPart().selectYAxis(true);

      if (trajectorySelectionMatrix.getAngularPart().isZSelected() || explorationSelectionMatrix.getAngularPart().isZSelected())
         selectionMatrix.getAngularPart().selectZAxis(true);

      return selectionMatrix;
   }

   public Pose3D getPose(double time)
   {
      Pose3D current = new Pose3D();

      Pose3D previous = null;
      Pose3D next = null;
      double t0 = Double.NaN;
      double tf = Double.NaN;

      for (int i = 1; i < waypoints.size(); i++)
      {
         t0 = waypointTimes.get(i - 1);
         tf = waypointTimes.get(i);
         previous = waypoints.get(i - 1);
         next = waypoints.get(i);
         if (time < tf)
         {
            break;
         }
      }

      double alpha = (time - t0) / (tf - t0);
      alpha = MathTools.clamp(alpha, 0, 1);

      current.interpolate(previous, next, alpha);

      return current;
   }

   private Pose3D appendPoseToTrajectory(double timeInTrajectory, Pose3D poseToAppend)
   {
      Pose3D pose = getPose(timeInTrajectory);

      if (hasTrajectoryCommand)
         pose.appendTransform(new RigidBodyTransform(initialGuessResult));

      RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(poseToAppend.getOrientation(), poseToAppend.getPosition());
      pose.appendTransform(rigidBodyTransform);

      return pose;
   }

   public KinematicsToolboxRigidBodyMessage createMessage(double timeInTrajectory, Pose3D poseToAppend)
   {
      Pose3D desiredEndEffectorPose = appendPoseToTrajectory(timeInTrajectory, poseToAppend);

      KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(rigidBody);
      message.getDesiredPositionInWorld().set(desiredEndEffectorPose.getPosition());
      message.getDesiredOrientationInWorld().set(desiredEndEffectorPose.getOrientation());
      message.getControlFramePositionInEndEffector().set(controlFramePose.getPosition());
      message.getControlFrameOrientationInEndEffector().set(controlFramePose.getOrientation());
      message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(getSelectionMatrix().getAngularPart()));
      message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(getSelectionMatrix().getLinearPart()));
      message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weight));
      message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weight)); // Sylvain's value :: 0.5

      return message;
   }

   public Pose3D getPoseToWorldFrame(Pose3D poseToAppend)
   {
      return appendPoseToTrajectory(0.0, poseToAppend);
   }

   public Pose3D getPoseToWorldFrame(SpatialNode spatialNode)
   {
      return appendPoseToTrajectory(spatialNode.getTime(), spatialNode.getSpatialData(getRigidBody()));
   }

   public void appendRandomSpatial(SpatialData spatialData)
   {
      // TODO: need a boolean variable to enable or disable exploration.
      // TODO: that is for dual hand reaching.
      RigidBodyTransform randomPose = new RigidBodyTransform();

      String[] configurationNames = new String[explorationConfigurationSpaces.size()];
      double[] configurationData = new double[explorationConfigurationSpaces.size()];

      for (int i = 0; i < explorationConfigurationSpaces.size(); i++)
      {
         ConfigurationSpaceName configurationSpaceName = explorationConfigurationSpaces.get(i);

         double lowerBound = 1.0 * explorationRangeLowerLimits.get(i);
         double upperBound = 1.0 * explorationRangeUpperLimits.get(i);
         double value = RandomNumbers.nextDouble(WholeBodyTrajectoryToolboxSettings.randomManager, lowerBound, upperBound);

         configurationNames[i] = rigidBody + "_" + configurationSpaceName.name();
         configurationData[i] = value;

         if (VERBOSE)
            PrintTools.info("" + i + " " + rigidBody + " " + value + " " + lowerBound + " " + upperBound);

         switch (configurationSpaceName)
         {
         case X:
            randomPose.appendTranslation(value, 0.0, 0.0);
            break;
         case Y:
            randomPose.appendTranslation(0.0, value, 0.0);
            break;
         case Z:
            randomPose.appendTranslation(0.0, 0.0, value);
            break;
         case ROLL:
            randomPose.appendRollRotation(value);
            break;
         case PITCH:
            randomPose.appendPitchRotation(value);
            break;
         case YAW:
            randomPose.appendYawRotation(value);
            break;
         default:
            break;
         }
      }
      initialGuessCandidate.set(randomPose);

      spatialData.appendSpatial(getRigidBody().getName(), configurationNames, configurationData, randomPose);
   }

   public void holdConfiguration(RigidBodyBasics rigidBody)
   {
      if (hasTrajectoryCommand)
      {
         initialGuessHolder.set(initialGuessCandidate);
      }
      else
      {
         initialGuessHolder.set(rigidBody.getBodyFixedFrame().getTransformToWorldFrame());
      }
   }

   public void updateInitialResult()
   {
      if (hasTrajectoryCommand)
      {
         initialGuessResult.set(initialGuessHolder);
      }
      else
      {
         waypoints.set(0, new Pose3D(initialGuessHolder));
         waypoints.set(1, new Pose3D(initialGuessHolder));
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasAuxiliaryRobotData.java<|end_filename|>
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;

/**
       * Message specific to Atlas, it is reported by the IHMC state estimator.
       */
public class AtlasAuxiliaryRobotData extends Packet<AtlasAuxiliaryRobotData> implements Settable<AtlasAuxiliaryRobotData>, EpsilonComparable<AtlasAuxiliaryRobotData>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.Float  electric_joint_temperatures_;
   public us.ihmc.idl.IDLSequence.Float  electric_joint_currents_;
   public us.ihmc.idl.IDLSequence.Boolean  electric_joint_enabled_array_;
   public float[] raw_imu_timestamps_;
   public float[] raw_imu_packet_counts_;
   public us.ihmc.euclid.tuple3D.Vector3D[] raw_imu_rates_;
   public us.ihmc.euclid.tuple3D.Vector3D[] raw_imu_deltas_;
   public boolean battery_charging_;
   public float battery_voltage_;
   public float battery_current_;
   public float remaining_battery_time_;
   public float remaining_amp_hours_;
   public float remaining_charge_percentage_;
   public long battery_cycle_count_;
   public float pump_inlet_pressure_;
   public float pump_supply_pressure_;
   public float air_sump_pressure_;
   public float pump_supply_temperature_;
   public float pump_rpm_;
   public float motor_temperature_;
   public float motor_driver_temperature_;

   public AtlasAuxiliaryRobotData()
   {
      electric_joint_temperatures_ = new us.ihmc.idl.IDLSequence.Float (6, "type_5");

      electric_joint_currents_ = new us.ihmc.idl.IDLSequence.Float (6, "type_5");

      electric_joint_enabled_array_ = new us.ihmc.idl.IDLSequence.Boolean (6, "type_7");

      raw_imu_timestamps_ = new float[15];

      raw_imu_packet_counts_ = new float[15];

      raw_imu_rates_ = new us.ihmc.euclid.tuple3D.Vector3D[15];

      for(int i1 = 0; i1 < raw_imu_rates_.length; ++i1)
      {
          raw_imu_rates_[i1] = new us.ihmc.euclid.tuple3D.Vector3D();
      }
      raw_imu_deltas_ = new us.ihmc.euclid.tuple3D.Vector3D[15];

      for(int i3 = 0; i3 < raw_imu_deltas_.length; ++i3)
      {
          raw_imu_deltas_[i3] = new us.ihmc.euclid.tuple3D.Vector3D();
      }
   }

   public AtlasAuxiliaryRobotData(AtlasAuxiliaryRobotData other)
   {
      this();
      set(other);
   }

   public void set(AtlasAuxiliaryRobotData other)
   {
      sequence_id_ = other.sequence_id_;

      electric_joint_temperatures_.set(other.electric_joint_temperatures_);
      electric_joint_currents_.set(other.electric_joint_currents_);
      electric_joint_enabled_array_.set(other.electric_joint_enabled_array_);
      for(int i5 = 0; i5 < raw_imu_timestamps_.length; ++i5)
      {
            raw_imu_timestamps_[i5] = other.raw_imu_timestamps_[i5];

      }

      for(int i7 = 0; i7 < raw_imu_packet_counts_.length; ++i7)
      {
            raw_imu_packet_counts_[i7] = other.raw_imu_packet_counts_[i7];

      }

      for(int i9 = 0; i9 < raw_imu_rates_.length; ++i9)
      {
            geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.raw_imu_rates_[i9], raw_imu_rates_[i9]);}

      for(int i11 = 0; i11 < raw_imu_deltas_.length; ++i11)
      {
            geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.raw_imu_deltas_[i11], raw_imu_deltas_[i11]);}

      battery_charging_ = other.battery_charging_;

      battery_voltage_ = other.battery_voltage_;

      battery_current_ = other.battery_current_;

      remaining_battery_time_ = other.remaining_battery_time_;

      remaining_amp_hours_ = other.remaining_amp_hours_;

      remaining_charge_percentage_ = other.remaining_charge_percentage_;

      battery_cycle_count_ = other.battery_cycle_count_;

      pump_inlet_pressure_ = other.pump_inlet_pressure_;

      pump_supply_pressure_ = other.pump_supply_pressure_;

      air_sump_pressure_ = other.air_sump_pressure_;

      pump_supply_temperature_ = other.pump_supply_temperature_;

      pump_rpm_ = other.pump_rpm_;

      motor_temperature_ = other.motor_temperature_;

      motor_driver_temperature_ = other.motor_driver_temperature_;

   }

   /**
            * 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 us.ihmc.idl.IDLSequence.Float  getElectricJointTemperatures()
   {
      return electric_joint_temperatures_;
   }


   public us.ihmc.idl.IDLSequence.Float  getElectricJointCurrents()
   {
      return electric_joint_currents_;
   }


   public us.ihmc.idl.IDLSequence.Boolean  getElectricJointEnabledArray()
   {
      return electric_joint_enabled_array_;
   }


   public float[] getRawImuTimestamps()
   {
      return raw_imu_timestamps_;
   }


   public float[] getRawImuPacketCounts()
   {
      return raw_imu_packet_counts_;
   }


   public us.ihmc.euclid.tuple3D.Vector3D[] getRawImuRates()
   {
      return raw_imu_rates_;
   }


   public us.ihmc.euclid.tuple3D.Vector3D[] getRawImuDeltas()
   {
      return raw_imu_deltas_;
   }

   public void setBatteryCharging(boolean battery_charging)
   {
      battery_charging_ = battery_charging;
   }
   public boolean getBatteryCharging()
   {
      return battery_charging_;
   }

   public void setBatteryVoltage(float battery_voltage)
   {
      battery_voltage_ = battery_voltage;
   }
   public float getBatteryVoltage()
   {
      return battery_voltage_;
   }

   public void setBatteryCurrent(float battery_current)
   {
      battery_current_ = battery_current;
   }
   public float getBatteryCurrent()
   {
      return battery_current_;
   }

   public void setRemainingBatteryTime(float remaining_battery_time)
   {
      remaining_battery_time_ = remaining_battery_time;
   }
   public float getRemainingBatteryTime()
   {
      return remaining_battery_time_;
   }

   public void setRemainingAmpHours(float remaining_amp_hours)
   {
      remaining_amp_hours_ = remaining_amp_hours;
   }
   public float getRemainingAmpHours()
   {
      return remaining_amp_hours_;
   }

   public void setRemainingChargePercentage(float remaining_charge_percentage)
   {
      remaining_charge_percentage_ = remaining_charge_percentage;
   }
   public float getRemainingChargePercentage()
   {
      return remaining_charge_percentage_;
   }

   public void setBatteryCycleCount(long battery_cycle_count)
   {
      battery_cycle_count_ = battery_cycle_count;
   }
   public long getBatteryCycleCount()
   {
      return battery_cycle_count_;
   }

   public void setPumpInletPressure(float pump_inlet_pressure)
   {
      pump_inlet_pressure_ = pump_inlet_pressure;
   }
   public float getPumpInletPressure()
   {
      return pump_inlet_pressure_;
   }

   public void setPumpSupplyPressure(float pump_supply_pressure)
   {
      pump_supply_pressure_ = pump_supply_pressure;
   }
   public float getPumpSupplyPressure()
   {
      return pump_supply_pressure_;
   }

   public void setAirSumpPressure(float air_sump_pressure)
   {
      air_sump_pressure_ = air_sump_pressure;
   }
   public float getAirSumpPressure()
   {
      return air_sump_pressure_;
   }

   public void setPumpSupplyTemperature(float pump_supply_temperature)
   {
      pump_supply_temperature_ = pump_supply_temperature;
   }
   public float getPumpSupplyTemperature()
   {
      return pump_supply_temperature_;
   }

   public void setPumpRpm(float pump_rpm)
   {
      pump_rpm_ = pump_rpm;
   }
   public float getPumpRpm()
   {
      return pump_rpm_;
   }

   public void setMotorTemperature(float motor_temperature)
   {
      motor_temperature_ = motor_temperature;
   }
   public float getMotorTemperature()
   {
      return motor_temperature_;
   }

   public void setMotorDriverTemperature(float motor_driver_temperature)
   {
      motor_driver_temperature_ = motor_driver_temperature;
   }
   public float getMotorDriverTemperature()
   {
      return motor_driver_temperature_;
   }


   public static Supplier<AtlasAuxiliaryRobotDataPubSubType> getPubSubType()
   {
      return AtlasAuxiliaryRobotDataPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AtlasAuxiliaryRobotDataPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AtlasAuxiliaryRobotData other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.electric_joint_temperatures_, other.electric_joint_temperatures_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.electric_joint_currents_, other.electric_joint_currents_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBooleanSequence(this.electric_joint_enabled_array_, other.electric_joint_enabled_array_, epsilon)) return false;

      for(int i13 = 0; i13 < raw_imu_timestamps_.length; ++i13)
      {
                if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.raw_imu_timestamps_[i13], other.raw_imu_timestamps_[i13], epsilon)) return false;
      }

      for(int i15 = 0; i15 < raw_imu_packet_counts_.length; ++i15)
      {
                if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.raw_imu_packet_counts_[i15], other.raw_imu_packet_counts_[i15], epsilon)) return false;
      }

      for(int i17 = 0; i17 < raw_imu_rates_.length; ++i17)
      {
              if (!this.raw_imu_rates_[i17].epsilonEquals(other.raw_imu_rates_[i17], epsilon)) return false;
      }

      for(int i19 = 0; i19 < raw_imu_deltas_.length; ++i19)
      {
              if (!this.raw_imu_deltas_[i19].epsilonEquals(other.raw_imu_deltas_[i19], epsilon)) return false;
      }

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.battery_charging_, other.battery_charging_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.battery_voltage_, other.battery_voltage_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.battery_current_, other.battery_current_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.remaining_battery_time_, other.remaining_battery_time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.remaining_amp_hours_, other.remaining_amp_hours_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.remaining_charge_percentage_, other.remaining_charge_percentage_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.battery_cycle_count_, other.battery_cycle_count_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.pump_inlet_pressure_, other.pump_inlet_pressure_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.pump_supply_pressure_, other.pump_supply_pressure_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.air_sump_pressure_, other.air_sump_pressure_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.pump_supply_temperature_, other.pump_supply_temperature_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.pump_rpm_, other.pump_rpm_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.motor_temperature_, other.motor_temperature_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.motor_driver_temperature_, other.motor_driver_temperature_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AtlasAuxiliaryRobotData)) return false;

      AtlasAuxiliaryRobotData otherMyClass = (AtlasAuxiliaryRobotData) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.electric_joint_temperatures_.equals(otherMyClass.electric_joint_temperatures_)) return false;
      if (!this.electric_joint_currents_.equals(otherMyClass.electric_joint_currents_)) return false;
      if (!this.electric_joint_enabled_array_.equals(otherMyClass.electric_joint_enabled_array_)) return false;
      for(int i21 = 0; i21 < raw_imu_timestamps_.length; ++i21)
      {
                if(this.raw_imu_timestamps_[i21] != otherMyClass.raw_imu_timestamps_[i21]) return false;

      }
      for(int i23 = 0; i23 < raw_imu_packet_counts_.length; ++i23)
      {
                if(this.raw_imu_packet_counts_[i23] != otherMyClass.raw_imu_packet_counts_[i23]) return false;

      }
      for(int i25 = 0; i25 < raw_imu_rates_.length; ++i25)
      {
                if (!this.raw_imu_rates_[i25].equals(otherMyClass.raw_imu_rates_[i25])) return false;
      }
      for(int i27 = 0; i27 < raw_imu_deltas_.length; ++i27)
      {
                if (!this.raw_imu_deltas_[i27].equals(otherMyClass.raw_imu_deltas_[i27])) return false;
      }
      if(this.battery_charging_ != otherMyClass.battery_charging_) return false;

      if(this.battery_voltage_ != otherMyClass.battery_voltage_) return false;

      if(this.battery_current_ != otherMyClass.battery_current_) return false;

      if(this.remaining_battery_time_ != otherMyClass.remaining_battery_time_) return false;

      if(this.remaining_amp_hours_ != otherMyClass.remaining_amp_hours_) return false;

      if(this.remaining_charge_percentage_ != otherMyClass.remaining_charge_percentage_) return false;

      if(this.battery_cycle_count_ != otherMyClass.battery_cycle_count_) return false;

      if(this.pump_inlet_pressure_ != otherMyClass.pump_inlet_pressure_) return false;

      if(this.pump_supply_pressure_ != otherMyClass.pump_supply_pressure_) return false;

      if(this.air_sump_pressure_ != otherMyClass.air_sump_pressure_) return false;

      if(this.pump_supply_temperature_ != otherMyClass.pump_supply_temperature_) return false;

      if(this.pump_rpm_ != otherMyClass.pump_rpm_) return false;

      if(this.motor_temperature_ != otherMyClass.motor_temperature_) return false;

      if(this.motor_driver_temperature_ != otherMyClass.motor_driver_temperature_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AtlasAuxiliaryRobotData {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("electric_joint_temperatures=");
      builder.append(this.electric_joint_temperatures_);      builder.append(", ");
      builder.append("electric_joint_currents=");
      builder.append(this.electric_joint_currents_);      builder.append(", ");
      builder.append("electric_joint_enabled_array=");
      builder.append(this.electric_joint_enabled_array_);      builder.append(", ");
      builder.append("raw_imu_timestamps=");
      builder.append(java.util.Arrays.toString(this.raw_imu_timestamps_));      builder.append(", ");
      builder.append("raw_imu_packet_counts=");
      builder.append(java.util.Arrays.toString(this.raw_imu_packet_counts_));      builder.append(", ");
      builder.append("raw_imu_rates=");
      builder.append(java.util.Arrays.toString(this.raw_imu_rates_));      builder.append(", ");
      builder.append("raw_imu_deltas=");
      builder.append(java.util.Arrays.toString(this.raw_imu_deltas_));      builder.append(", ");
      builder.append("battery_charging=");
      builder.append(this.battery_charging_);      builder.append(", ");
      builder.append("battery_voltage=");
      builder.append(this.battery_voltage_);      builder.append(", ");
      builder.append("battery_current=");
      builder.append(this.battery_current_);      builder.append(", ");
      builder.append("remaining_battery_time=");
      builder.append(this.remaining_battery_time_);      builder.append(", ");
      builder.append("remaining_amp_hours=");
      builder.append(this.remaining_amp_hours_);      builder.append(", ");
      builder.append("remaining_charge_percentage=");
      builder.append(this.remaining_charge_percentage_);      builder.append(", ");
      builder.append("battery_cycle_count=");
      builder.append(this.battery_cycle_count_);      builder.append(", ");
      builder.append("pump_inlet_pressure=");
      builder.append(this.pump_inlet_pressure_);      builder.append(", ");
      builder.append("pump_supply_pressure=");
      builder.append(this.pump_supply_pressure_);      builder.append(", ");
      builder.append("air_sump_pressure=");
      builder.append(this.air_sump_pressure_);      builder.append(", ");
      builder.append("pump_supply_temperature=");
      builder.append(this.pump_supply_temperature_);      builder.append(", ");
      builder.append("pump_rpm=");
      builder.append(this.pump_rpm_);      builder.append(", ");
      builder.append("motor_temperature=");
      builder.append(this.motor_temperature_);      builder.append(", ");
      builder.append("motor_driver_temperature=");
      builder.append(this.motor_driver_temperature_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/UserCommandNetworkReader.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication;

import java.io.IOException;

public interface UserCommandNetworkReader
{
   public int readInt() throws IOException;

   public float readFloat() throws IOException;

   public double readDouble() throws IOException;
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/MidiControlMap.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

/**
 * A MIDI connection can be used to send or receive control commands. 127 controllers can
 * be supported by a MIDI device. In the case of a sliderboard each input slider or knob is
 * mapped to a control channel. This mapping is sliderboard dependent and needs to be defined
 * for each supported sliderboard by implementing this interface.
 */
public interface MidiControlMap
{
   public static int INVALID = -1;

   /**
    * Gets the MIDI control channel (between 0 and 127) associated with the given slider index for a
    * sliderboard.
    *
    * @param sliderIndex the index of a slider on the sliderboard.
    * @return the MIDI control channel associated with the slider index.
    */
   int getSliderChannel(int sliderIndex);

   /**
    * Gets the slider index of the provided MIDI control channel. This is the inverse function of
    * {@link #getSliderChannel(int)}.
    *
    * @param sliderChannel the MIDI channel associated with a slider on the slider board.
    * @return the index of the slider corresponding to the provided MIDI channel.
    */
   int getSliderIndex(int sliderChannel);

   /**
    * Gets the MIDI control channel (between 0 and 127) associated with the given button index for a
    * sliderboard.
    *
    * @param buttonIndex the index of a button on the sliderboard.
    * @return the MIDI control channel associated with the button index.
    */
   int getButtonChannel(int buttonIndex);

   /**
    * Gets the button index of the provided MIDI control channel. This is the inverse function of
    * {@link #getButtonChannel(int)}.
    *
    * @param buttonChannel the MIDI channel associated with a button on the slider board.
    * @return the index of the button corresponding to the provided MIDI channel.
    */
   int getButtonIndex(int buttonChannel);

   /**
    * Gets the channel associated with the "Delay/Variation Send" status.
    *
    * @return the index of the "Delay/Variation Send" controller.
    */
   int getDelayVariationChannel();

   static boolean isInRange(int index, int min, int max)
   {
      return index >= min && index <= max;
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/FootTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.FootTrajectoryMessage;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.humanoidBehaviors.behaviors.primitives.FootTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.robotSide.RobotSide;

public class FootTrajectoryTask extends BehaviorAction
{
   private final FootTrajectoryMessage footTrajectoryMessage;
   private final FootTrajectoryBehavior footPoseBehavior;

   public FootTrajectoryTask(RobotSide robotSide, Point3D position, Quaternion orientation, FootTrajectoryBehavior behavior, double trajectoryTime)
   {

      super(behavior);
      this.footPoseBehavior = behavior;
      footTrajectoryMessage = HumanoidMessageTools.createFootTrajectoryMessage(robotSide, trajectoryTime, position, orientation);
   }

   public FootTrajectoryTask(RobotSide robotSide, FramePose3D pose, FootTrajectoryBehavior behavior, double trajectoryTime)
   {
      super(behavior);
      this.footPoseBehavior = behavior;

      pose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
      Point3D position = new Point3D();
      Quaternion orientation = new Quaternion();
      pose.get(position, orientation);
      footTrajectoryMessage = HumanoidMessageTools.createFootTrajectoryMessage(robotSide, trajectoryTime, position, orientation);
   }

   @Override
   protected void setBehaviorInput()
   {
      footPoseBehavior.setInput(footTrajectoryMessage);
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/walkingpath/footstep/SkeletonPathFootStepPlanner.java<|end_filename|>
package us.ihmc.manipulation.planning.walkingpath.footstep;

import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.ArrayList;

import us.ihmc.commons.PrintTools;
import us.ihmc.robotics.robotSide.RobotSide;

public class SkeletonPathFootStepPlanner
{
   // footstep info   
   public ArrayList<SkeletonPathFootStep> footSteps = new ArrayList<SkeletonPathFootStep>();

   // path info 
   public SkeletonPath skeletonPathSegments;

   public SkeletonPath pathSegmentsRightSide;
   public SkeletonPath pathSegmentsLeftSide;

   // L, w, d
   private double stepLength;
   private double stepWidth;
   private double stepStride;

   public Point2D tempPoint;

   public SkeletonPathFootStepPlanner(double[] skeletonPathX, double[] skeletonPathY, double stepLength, double stepWidth)
   {
      PrintTools.info("Number of path node is " + skeletonPathX.length);

      this.stepLength = stepLength;
      this.stepWidth = stepWidth;
      this.stepStride = Math.sqrt(this.stepLength * this.stepLength + this.stepWidth * this.stepWidth);

      this.skeletonPathSegments = new SkeletonPath(skeletonPathX, skeletonPathY);
      this.pathSegmentsRightSide = new SkeletonPath(-this.stepWidth / 2, this.skeletonPathSegments);
      this.pathSegmentsLeftSide = new SkeletonPath(this.stepWidth / 2, this.skeletonPathSegments);

      PrintTools.info("final " + skeletonPathSegments.getFinalPoint().getX() + " " + skeletonPathSegments.getFinalPoint().getY());
      //      for(int i =0;i<skeletonPathSegments.get().size();i++)
      //      {
      //         PrintTools.info(""+i+" seg "+skeletonPathSegments.get().get(i).getX1()+" "+skeletonPathSegments.get().get(i).getY1()+" "+ skeletonPathSegments.get().get(i).getX2()+" "+skeletonPathSegments.get().get(i).getY2());
      //      }
   }

   public void setZeroStep(RobotSide robotSide)
   {
      Point2D footStepLocation = new Point2D.Double();
      if (robotSide == RobotSide.RIGHT)
      {
         footStepLocation.setLocation(0.0, -stepWidth / 2);
         SkeletonPathFootStep footStep = new SkeletonPathFootStep(RobotSide.RIGHT, footStepLocation, pathSegmentsRightSide.getYawAngleOfSegment(0));
         footStep.setIndexOfSegment(0);
         footSteps.add(footStep);
      }
      else
      {
         footStepLocation.setLocation(0.0, stepWidth / 2);
         SkeletonPathFootStep footStep = new SkeletonPathFootStep(RobotSide.LEFT, footStepLocation, pathSegmentsLeftSide.getYawAngleOfSegment(0));
         footStep.setIndexOfSegment(0);
         footSteps.add(footStep);
      }
   }

   public void createFootSteps()
   {
      for (int i = 0; i < 100; i++)
      {
         addNextStep();
         if (footSteps.get(footSteps.size() - 1).getLocation().distance(skeletonPathSegments.getFinalPoint()) < stepWidth / 2 + 0.0001)
         {
            PrintTools.info("" + i + " " + footSteps.get(footSteps.size() - 1).getLocation().distance(skeletonPathSegments.getFinalPoint()));
            addNextStep();
            PrintTools.info("numberof steps " + footSteps.size());
            break;
         }

      }
   }

   private void addNextStep()
   {
      SkeletonPathFootStep newFootStep = new SkeletonPathFootStep();
      SkeletonPathFootStep curFootStep = footSteps.get(footSteps.size() - 1);

      newFootStep = getNextStep(curFootStep);

      footSteps.add(newFootStep);
   }

   public SkeletonPathFootStep getNextStep(SkeletonPathFootStep curStep)
   {
      SkeletonPathFootStep newFootStep = new SkeletonPathFootStep();

      if (curStep.getRobotSide() == RobotSide.RIGHT)
      {
         newFootStep = getNextLeftStep(curStep);
      }
      else
      {
         newFootStep = getNextRightStep(curStep);
      }

      return newFootStep;
   }

   public SkeletonPathFootStep getNextRightStep(SkeletonPathFootStep curStep)
   {
      SkeletonPathFootStep ret = new SkeletonPathFootStep();

      int indexOfCurSegment = curStep.getIndexOfSegment();
      Line2D curSegment = skeletonPathSegments.getSegment(indexOfCurSegment);

      double curMatric;
      double maxMatric = 0.0;

      int numberOfCandidates = 20;
      for (int i = 0; i < numberOfCandidates; i++)
      {
         double minSearching = Math.PI * (-0.9);
         double maxSearching = Math.PI * (-0.0);
         double angleOfCandidate = minSearching + i * (maxSearching - minSearching) / (numberOfCandidates - 1);

         Point2D aCandidate = getACandidate(curStep, this.stepStride, angleOfCandidate);

         SkeletonPathFootStep aProjectedCandidate = getProjectedFootStep(RobotSide.RIGHT, aCandidate);

         if (isValidStep(RobotSide.RIGHT, aProjectedCandidate.getLocation(), curSegment))
         {
            if (aProjectedCandidate.getLocation().distance(curStep.getLocation()) < stepStride)
            {
               curMatric = getXMatric(aProjectedCandidate.getLocation(), curSegment);
               //               PrintTools.info(""+i+" "+angleOfCandidate*180/Math.PI+" matric "+ curMatric);
               if (curMatric > maxMatric)
               {
                  maxMatric = curMatric;
                  ret = aProjectedCandidate;
               }
            }
         }
      }

      //      PrintTools.info("!!! "+ret.getLocation().getX()+" "+ret.getLocation().getY());
      tempPoint = new Point2D.Double(ret.getLocation().getX(), ret.getLocation().getY());

      return ret;
   }

   public SkeletonPathFootStep getNextLeftStep(SkeletonPathFootStep curStep)
   {
      SkeletonPathFootStep ret = new SkeletonPathFootStep();

      int indexOfCurSegment = curStep.getIndexOfSegment();
      Line2D curSegment = skeletonPathSegments.getSegment(indexOfCurSegment);

      double curMatric;
      double maxMatric = 0.0;

      int numberOfCandidates = 20;
      for (int i = 0; i < numberOfCandidates; i++)
      {
         double minSearching = Math.PI * (0.0);
         double maxSearching = Math.PI * (0.9);
         double angleOfCandidate = minSearching + i * (maxSearching - minSearching) / (numberOfCandidates - 1);

         Point2D aCandidate = getACandidate(curStep, this.stepStride, angleOfCandidate);

         SkeletonPathFootStep aProjectedCandidate = getProjectedFootStep(RobotSide.LEFT, aCandidate);

         if (isValidStep(RobotSide.LEFT, aProjectedCandidate.getLocation(), curSegment))
         {
            if (aProjectedCandidate.getLocation().distance(curStep.getLocation()) < stepStride)
            {
               curMatric = getXMatric(aProjectedCandidate.getLocation(), curSegment);
               //PrintTools.info(""+i+" "+angleOfCandidate*180/Math.PI+" matric "+ curMatric);
               if (curMatric > maxMatric)
               {
                  maxMatric = curMatric;
                  ret = aProjectedCandidate;
               }
            }
         }
      }

      //PrintTools.info("!!! "+ret.getLocation().getX()+" "+ret.getLocation().getY());
      tempPoint = new Point2D.Double(ret.getLocation().getX(), ret.getLocation().getY());

      return ret;
   }

   public SkeletonPathFootStep getProjectedFootStep(RobotSide robotSide, Point2D aPoint)
   {
      SkeletonPathFootStep retFootStep = new SkeletonPathFootStep();

      SkeletonPath segments;

      if (robotSide == RobotSide.RIGHT)
      {
         segments = pathSegmentsRightSide;
      }
      else
      {
         segments = pathSegmentsLeftSide;
      }

      int indexOfClosestSegment = segments.getIndexOfClosestSegment(aPoint);

      Point2D locationOfProjectedFootStep = segments.getLocationOfClosestPointOnSegment(aPoint);

      double yawAngleOfProjectedFootStep = segments.getYawAngleOfClosestPointOnSegment(aPoint);

      retFootStep.setRobotSide(robotSide);
      retFootStep.setLocation(locationOfProjectedFootStep);
      retFootStep.setYawAngle(yawAngleOfProjectedFootStep);
      retFootStep.setIndexOfSegment(indexOfClosestSegment);

      return retFootStep;
   }

   public Point2D getACandidate(SkeletonPathFootStep curStep, double radius, double rotationAngle)
   {
      Point2D ret = new Point2D.Double();
      Point2D curLocation = curStep.getLocation();

      double finalAngle = curStep.getYawAngle() + rotationAngle;

      ret.setLocation(curLocation.getX() + radius * Math.cos(finalAngle), curLocation.getY() + radius * Math.sin(finalAngle));

      return ret;
   }

   private boolean isValidStep(RobotSide robotSideOfStep, Point2D aPoint, Line2D curSegment)
   {
      if (robotSideOfStep == RobotSide.LEFT)
      {
         if (getYMatric(aPoint, curSegment) < 0)
         {
            return false;
         }
         else
         {
            return true;
         }
      }
      else
      {
         if (getYMatric(aPoint, curSegment) >= 0)
         {
            return false;
         }
         else
         {
            return true;
         }
      }
   }

   public double getXMatric(Point2D aPoint, Line2D curSegment)
   {
      Point2D aVector = new Point2D.Double(aPoint.getX() - curSegment.getX1(), aPoint.getY() - curSegment.getY1());
      Point2D refVector = new Point2D.Double((curSegment.getX2() - curSegment.getX1()), curSegment.getY2() - curSegment.getY1());
      double norm = Math.sqrt(refVector.getX() * refVector.getX() + refVector.getY() * refVector.getY());

      Point2D refVectorNorm = new Point2D.Double(refVector.getX() / norm, refVector.getY() / norm);

      return aVector.getX() * refVectorNorm.getX() + aVector.getY() * refVectorNorm.getY();
   }

   public double getYMatric(Point2D aPoint, Line2D curSegment)
   {
      Point2D aVector = new Point2D.Double(aPoint.getX() - curSegment.getX1(), aPoint.getY() - curSegment.getY1());
      Point2D refVector = new Point2D.Double(-(curSegment.getY2() - curSegment.getY1()), curSegment.getX2() - curSegment.getX1());
      double norm = Math.sqrt(refVector.getX() * refVector.getX() + refVector.getY() * refVector.getY());

      Point2D refVectorNorm = new Point2D.Double(refVector.getX() / norm, refVector.getY() / norm);

      return aVector.getX() * refVectorNorm.getX() + aVector.getY() * refVectorNorm.getY();
   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/packetCommunicator/KryoPacketCloningSendingTask.java<|end_filename|>
package us.ihmc.communication.packetCommunicator;

import java.util.ArrayList;
import java.util.concurrent.Callable;
import java.util.concurrent.ConcurrentLinkedQueue;
import java.util.concurrent.atomic.AtomicBoolean;

import com.esotericsoftware.kryo.Kryo;

import us.ihmc.communication.net.NetClassList;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.packets.Packet;

public class KryoPacketCloningSendingTask implements Callable<Void>
{
   private PacketConsumer consumer;
   private final ConcurrentLinkedQueue<Packet> packetQueue = new ConcurrentLinkedQueue<Packet>();
   private Packet packetToSend;
   private final AtomicBoolean isRunning = new AtomicBoolean();
   private final Kryo kryo = new Kryo();
   private final ArrayList<Class<?>> packetList = new ArrayList<Class<?>>();
   private final boolean clonePackets = true;

   
   //should just populate the class list using the passed in classes instead of the whole classlist
   public KryoPacketCloningSendingTask(NetClassList classList, PacketConsumer consumer)
   {
      classList.getPacketClassList(packetList);
      for (Class<?> clazz : packetList)
      {
         kryo.register(clazz);
      }
      for (Class<?> type : classList.getPacketFieldList())
      {
         kryo.register(type);
      }
      this.consumer = consumer;
   }

   public void setConsumer(PacketConsumer consumer)
   {
      this.consumer = consumer;
   }

   public PacketConsumer getConsumer()
   {
      return this.consumer;
   }

   public void submitPacket(Packet packet)
   {
      packetQueue.add(packet);
   }

   @Override
   public Void call() throws Exception
   {
      isRunning.set(true);
      while ((packetToSend = packetQueue.poll()) != null)
      {
         if (packetList.contains(packetToSend.getClass()) && clonePackets )
         {
            Packet<?> clonedPacket = kryo.copy(packetToSend);
            consumer.receivedPacket(clonedPacket);
         }
         else
         {
            consumer.receivedPacket(packetToSend);
         }
      }
      isRunning.set(false);
      return null;
   }

   public boolean isRunning()
   {
      return isRunning.get();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/ConvexPolygon2dIntersectionSetCalculator.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.util.ArrayList;

import us.ihmc.euclid.geometry.BoundingBox2D;
import us.ihmc.euclid.geometry.ConvexPolygon2D;

/**
 * <p>Title: </p>
 *
 * <p>Description: </p>
 *
 * <p>Copyright: Copyright (c) 2006</p>
 *
 * <p>Company: </p>
 *
 * @author not attributable
 * @version 1.0
 */
public class ConvexPolygon2dIntersectionSetCalculator
{
   private BoundingBoxKDTree2D kdTree;
   private final ArrayList<ConvexPolygon2D> convexPolygon2dsToSearchForIntersection = new ArrayList<ConvexPolygon2D>();
   private final ArrayList<BoundingBox2D> boundingBoxes = new ArrayList<>();

   private final ConvexPolygonTools convexPolygonTools = new ConvexPolygonTools();

   public ConvexPolygon2dIntersectionSetCalculator(ArrayList<ConvexPolygon2D> convexPolygon2d)
   {
      resetBaseConvexPolygon2ds(convexPolygon2d);
   }


   /**
    *    Finds the Tentative List of Polygons that intersect with the target polygon.
    *
    *    @param ConvexPolygon2D targetPolygon
    *    @param ArrayList<ConvexPolygon2d> convexPolygon2d The list of the polygon to search from.
    *    @return ArrayList<ConvexPolygon2d> The list of the polygons that might intersect with the target polygon.
    */
   public ArrayList<ConvexPolygon2D> findTentativeListOfPolygonsIntersectingTargetPolygon(ConvexPolygon2D targetPolygon)
   {
      ArrayList<ConvexPolygon2D> tentativeListOfPolygonsIntersectingTargetPolygon = new ArrayList<ConvexPolygon2D>();
      ArrayList<Object> intersectingObjects = kdTree.getIntersectingObjects(targetPolygon.getBoundingBox());
      for (Object o : intersectingObjects)
      {
         tentativeListOfPolygonsIntersectingTargetPolygon.add((ConvexPolygon2D) o);
      }

      return tentativeListOfPolygonsIntersectingTargetPolygon;
   }

   public ArrayList<ConvexPolygon2D> findIntersectionPolygonList(ConvexPolygon2D targetPolygon)
   {
      ArrayList<ConvexPolygon2D> tentativeList = findTentativeListOfPolygonsIntersectingTargetPolygon(targetPolygon);

      if (tentativeList == null || tentativeList.isEmpty())
         return null;

      ArrayList<ConvexPolygon2D> ret = new ArrayList<ConvexPolygon2D>();

      for (ConvexPolygon2D tentativePolygon : tentativeList)
      {
         ConvexPolygon2D intersection = new ConvexPolygon2D();
         boolean success = convexPolygonTools.computeIntersectionOfPolygons(targetPolygon, tentativePolygon, intersection);

         if (success)
            ret.add(intersection);
      }

      return ret;
   }


   public void resetBaseConvexPolygon2ds(ArrayList<ConvexPolygon2D> convexPolygon2ds)
   {
      this.convexPolygon2dsToSearchForIntersection.clear();
      this.boundingBoxes.clear();

      for (ConvexPolygon2D convexPolygon2d : convexPolygon2ds)
      {
         this.convexPolygon2dsToSearchForIntersection.add(convexPolygon2d);
      }

      for (int i = 0; i < convexPolygon2dsToSearchForIntersection.size(); i++)
      {
         ConvexPolygon2D convexPolygon2d = convexPolygon2dsToSearchForIntersection.get(i);
         boundingBoxes.add(convexPolygon2d.getBoundingBox());
      }

      ArrayList<Object> allObjects = new ArrayList<Object>(convexPolygon2dsToSearchForIntersection);
      this.kdTree = new BoundingBoxKDTree2D(boundingBoxes, allObjects);

   }

}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/GuiRegistry.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements;

import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.function.Predicate;

import javafx.beans.property.BooleanProperty;
import javafx.beans.property.SimpleBooleanProperty;

public class GuiRegistry extends GuiElement
{
   private final List<GuiRegistry> registries = new ArrayList<>();
   private final List<GuiParameter> parameters = new ArrayList<>();

   private final Map<String, GuiRegistry> registryMap = new LinkedHashMap<>();
   private final Map<String, GuiParameter> parameterMap = new LinkedHashMap<>();

   private BooleanProperty isRoot = new SimpleBooleanProperty();

   public GuiRegistry(String name, GuiRegistry parent)
   {
      super(name, parent);
      isRoot.set(parent == null);
   }

   public void addRegistry(GuiRegistry registry)
   {
      if (!registry.getParent().equals(this))
      {
         throw new RuntimeException("Can not add a parameter with other parent.");
      }
      registries.add(registry);
      if (registryMap.put(registry.getUniqueName(), registry) != null)
      {
         throw new RuntimeException("Already have registry " + registry.getName());
      }
   }

   public void addParameter(GuiParameter parameter)
   {
      if (!parameter.getParent().equals(this))
      {
         throw new RuntimeException("Can not add a parameter with other parent.");
      }
      parameters.add(parameter);
      if (parameterMap.put(parameter.getUniqueName(), parameter) != null)
      {
         throw new RuntimeException("Already have parameter " + parameter.getName());
      }
   }

   public List<GuiRegistry> getRegistries()
   {
      return registries;
   }

   public List<GuiParameter> getParameters()
   {
      return parameters;
   }

   public Map<String, GuiRegistry> getRegistryMap()
   {
      return registryMap;
   }

   public Map<String, GuiParameter> getParameterMap()
   {
      return parameterMap;
   }

   public List<GuiParameter> getAllParameters()
   {
      List<GuiParameter> ret = new ArrayList<>();
      packParametersRecursive(ret);
      return ret;
   }

   private void packParametersRecursive(List<GuiParameter> listToPack)
   {
      listToPack.addAll(parameters);
      registries.stream().forEach(registry -> registry.packParametersRecursive(listToPack));
   }

   public List<GuiRegistry> getAllRegistries()
   {
      List<GuiRegistry> ret = new ArrayList<>();
      packRegistriesRecursive(ret);
      return ret;
   }

   private void packRegistriesRecursive(List<GuiRegistry> listToPack)
   {
      listToPack.addAll(registries);
      registries.stream().forEach(registry -> registry.packRegistriesRecursive(listToPack));
   }

   public GuiRegistry createFullCopy()
   {
      return createFullCopy(p -> true);
   }

   public GuiRegistry createFullCopy(Predicate<GuiParameter> predicate)
   {
      return createFullCopy(null, predicate);
   }

   private GuiRegistry createFullCopy(GuiRegistry parentToAttachTo, Predicate<GuiParameter> predicate)
   {
      GuiRegistry copy = new GuiRegistry(getName(), parentToAttachTo);
      getParameters().stream().filter(predicate).forEach(parameter -> copy.addParameter(parameter.createCopy(copy)));
      getRegistries().stream().forEach(registry -> {
         GuiRegistry childCopy = registry.createFullCopy(copy, predicate);
         if (!childCopy.getAllParameters().isEmpty())
         {
            copy.addRegistry(childCopy);
         }
      });
      return copy;
   }

   @Override
   public String toString()
   {
      String ret = getUniqueName();
      ret += "\n" + parameters.size() + " parameters";
      ret += "\n" + registries.size() + " registries";
      return ret;
   }

   public BooleanProperty isRoot()
   {
      return isRoot;
   }

   public void makeRoot()
   {
      GuiRegistry parent = getParent();
      while (parent != null)
      {
         parent.isRoot.set(false);
         parent = parent.getParent();
      }
      removeRootRecusive();
      isRoot.set(true);
   }

   private void removeRootRecusive()
   {
      isRoot.set(false);
      registries.forEach(registry -> registry.removeRootRecusive());
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/pointClouds/shape/IhmcPointCloudOps.java<|end_filename|>
package us.ihmc.sensorProcessing.pointClouds.shape;

import java.util.List;

import georegression.geometry.UtilPoint3D_F64;
import georegression.metric.ClosestPoint3D_F64;
import georegression.struct.line.LineParametric3D_F64;
import georegression.struct.plane.PlaneGeneral3D_F64;
import georegression.struct.plane.PlaneNormal3D_F64;
import georegression.struct.point.Point3D_F64;

/**
 * @author <NAME>
 */
public class IhmcPointCloudOps
{
   /**
    * Converts a generalized plane equation into a {@link PlaneNormal3D_F64}.  The point on the plane is the mean of the
    * provided points. Normal is normalized to 1.
    *
    * @param original (Input) Generalized plane equation
    * @param points (Input) List of points on the plane
    * @param output (Output) Converted normal.
    */
   public static PlaneNormal3D_F64 convert( PlaneGeneral3D_F64 original , List<Point3D_F64> points , PlaneNormal3D_F64 output ) {
      if (output == null)
         output = new PlaneNormal3D_F64();
      
      UtilPoint3D_F64.mean(points,output.p);
      ClosestPoint3D_F64.closestPoint(original,output.p,output.p);

      output.n.set( original.A, original.B, original.C );
      output.n.normalize();
      
      return output;
   }

   /**
    * Adjusts normals of each plane so that they point outwards.  Norms of each plane will be normalized to 1.  The point on
    * each plane must not be the same for each plane.
    *
    * @param a plane
    * @param b plane
    * @param c plane
    */
   public static void adjustBoxNormals( PlaneNormal3D_F64 a , PlaneNormal3D_F64 b , PlaneNormal3D_F64 c ) {
      Point3D_F64 closest = new Point3D_F64();

      LineParametric3D_F64 lineA = new LineParametric3D_F64(a.p,a.n);
      LineParametric3D_F64 lineB = new LineParametric3D_F64(b.p,b.n);
      LineParametric3D_F64 lineC = new LineParametric3D_F64(c.p,c.n);


      ClosestPoint3D_F64.closestPoint(lineA,lineB,closest);

      a.n.set( a.p.x - closest.x,a.p.y - closest.y,a.p.z - closest.z);
      a.n.normalize();
      b.n.set( b.p.x - closest.x,b.p.y - closest.y,b.p.z - closest.z);
      b.n.normalize();
      ClosestPoint3D_F64.closestPoint(lineC,closest,closest);
      c.n.set( c.p.x - closest.x,c.p.y - closest.y,c.p.z - closest.z);
      c.n.normalize();

   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tree/ParameterTreeCell.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tree;

import javafx.scene.control.TreeCell;

public class ParameterTreeCell extends TreeCell<ParameterTreeValue>
{
   @Override
   protected void updateItem(ParameterTreeValue value, boolean empty)
   {
      super.updateItem(value, empty);
      setText(null);

      if (empty || value == null)
      {
         setGraphic(null);
      }
      else
      {
         setGraphic(value.getOrCreateNode());
      }
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/frames/CommonQuadrupedReferenceFrames.java<|end_filename|>
package us.ihmc.sensorProcessing.frames;

import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.robotics.robotSide.RobotQuadrant;
import us.ihmc.robotics.robotSide.RobotSide;

public interface CommonQuadrupedReferenceFrames extends CommonLeggedReferenceFrames<RobotQuadrant>
{
   ReferenceFrame getBodyFrame();

   ReferenceFrame getBodyZUpFrame();

   ReferenceFrame getSideDependentMidFeetZUpFrame(RobotSide robotSide);

   ReferenceFrame getRootJointFrame();

   ReferenceFrame getLegAttachmentFrame(RobotQuadrant robotQuadrant);

   ReferenceFrame getHipRollFrame(RobotQuadrant robotQuadrant);

   ReferenceFrame getHipPitchFrame(RobotQuadrant robotQuadrant);

   ReferenceFrame getKneeFrame(RobotQuadrant robotQuadrant);

   ReferenceFrame getCenterOfFourHipsFrame();

   ReferenceFrame getCenterOfMassZUpFrame();

   ReferenceFrame getFrameBeforeLegJoint(RobotQuadrant robotQuadrant, LegJointName legJointName);

   double getLegLength(RobotQuadrant robotQuadrant);

//   public abstract ReferenceFrame getMidTrotLineZUpFrame(RobotQuadrant quadrantAssocaitedWithTrotLine);
   
   /**
    * returns the center of the polygon made up using the provided robot quadrants, 
    * averaging the lowest front and the lowest hind Z values, 
    * and using the nominal yaw, pitch, and roll
    * @param feetQuadrants, feet 
    */
   public abstract ReferenceFrame getTripleSupportFrameAveragingLowestZHeightsAcrossEnds(RobotQuadrant footToExclude);

   /**
    * returns the center of the polygon made up using the provided robot quadrants, 
    * averaging the lowest front and the lowest hind Z values, 
    * and using the nominal yaw
    */
//   public abstract ReferenceFrame getZUpTripleSupportFrameAveragingLowestZHeightsAcrossEnds(RobotQuadrant footToExclude);

   /**
    * returns the center of the polygon made up using the four feet, 
    * averaging the lowest front and the lowest hind Z values, 
    * and using the nominal yaw, pitch, and roll
    */
   public abstract ReferenceFrame getCenterOfFeetFrameAveragingLowestZHeightsAcrossEnds();
   
   /**
    * returns the center of the four foot polygon, 
    * averaging the lowest front and the lowest hind Z values, 
    * and using the nominal yaw
    */
   public abstract ReferenceFrame getCenterOfFeetZUpFrameAveragingLowestZHeightsAcrossEnds();

}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/calculators/Omega0CalculatorInterface.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.calculators;

import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.mecano.spatial.interfaces.SpatialForceReadOnly;
import us.ihmc.robotics.robotSide.SideDependentList;

public interface Omega0CalculatorInterface
{
   public abstract double computeOmega0(SideDependentList<FramePoint2D> cops, SpatialForceReadOnly totalGroundReactionWrench);
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ComPositionPacket.java<|end_filename|>
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 quadruped controller API.
       */
public class ComPositionPacket extends Packet<ComPositionPacket> implements Settable<ComPositionPacket>, EpsilonComparable<ComPositionPacket>
{
   public us.ihmc.euclid.tuple3D.Point3D position_;

   public ComPositionPacket()
   {
      position_ = new us.ihmc.euclid.tuple3D.Point3D();
   }

   public ComPositionPacket(ComPositionPacket other)
   {
      this();
      set(other);
   }

   public void set(ComPositionPacket other)
   {
      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.position_, position_);   }


   public us.ihmc.euclid.tuple3D.Point3D getPosition()
   {
      return position_;
   }


   public static Supplier<ComPositionPacketPubSubType> getPubSubType()
   {
      return ComPositionPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ComPositionPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ComPositionPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!this.position_.epsilonEquals(other.position_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ComPositionPacket)) return false;

      ComPositionPacket otherMyClass = (ComPositionPacket) other;

      if (!this.position_.equals(otherMyClass.position_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ComPositionPacket {");
      builder.append("position=");
      builder.append(this.position_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/perturbance/Collidable.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.perturbance;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.yoVariables.variable.YoDouble;



public interface Collidable
{
   public abstract void handleCollision(Vector3D ballVelocity, double ballMass, YoDouble coefficientOfRestitution);
}

<|start_filename|>atlas/src/test/java/us/ihmc/atlas/AtlasStandingOnPlanarRegionsListDefinedEnvironmentExample.java<|end_filename|>
package us.ihmc.atlas;

import us.ihmc.avatar.simulationStarter.DRCSimulationStarter;
import us.ihmc.avatar.simulationStarter.DRCSimulationTools;
import us.ihmc.robotics.geometry.PlanarRegionsListGenerator;
import us.ihmc.simulationConstructionSetTools.util.environments.PlanarRegionsListDefinedEnvironment;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class AtlasStandingOnPlanarRegionsListDefinedEnvironmentExample
{
   public static void main(String[] args)
   {
      AtlasRobotModel robotModel = AtlasRobotModelFactory.createDefaultRobotModel();

      PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator();

      generator.addRectangle(20, 20);

      PlanarRegionsListDefinedEnvironment environment = new PlanarRegionsListDefinedEnvironment("FlatPlanarRegionsList", generator.getPlanarRegionsList(), 0.005, false);

      DRCSimulationStarter simulationStarter = new DRCSimulationStarter(robotModel, environment);
      simulationStarter.setRunMultiThreaded(true);
      simulationStarter.setInitializeEstimatorToActual(true);
      DRCSimulationTools.startSimulationWithGraphicSelector(simulationStarter, null, null);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/PID3DConfiguration.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains.implementations;

import us.ihmc.robotics.controllers.pidGains.GainCoupling;
import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly;

/**
 * A class that defines the configuration of 3D PID gains. It provides
 * a structure for information needed when creating yo-variable or parameter
 * versions of PID 3D gains.
 */
public class PID3DConfiguration
{
   private final boolean useIntegrator;

   private final GainCoupling gainCoupling;

   private final PID3DGainsReadOnly gains;

   public PID3DConfiguration(GainCoupling gainCoupling, boolean useIntegrator)
   {
      this(gainCoupling, useIntegrator, null);
   }

   public PID3DConfiguration(GainCoupling gainCoupling, boolean useIntegrator, PID3DGainsReadOnly gains)
   {
      this.useIntegrator = useIntegrator;
      this.gainCoupling = gainCoupling;
      this.gains = gains;
   }

   /**
   * Returns whether the gains created from this configuration should create
   * an I gain or not (if this returns {@code false} the controller will behave
   * as a PD controller).
   * <p>
   * This is especially useful when using the YoVariable implementation of
   * PID 3D gains {@link DefaultYoPID3DGains} is used since is will avoid
   * creating the YoVariables for tuning the integration.
   * </p>
   * @return whether the gains should include integrator gains.
   */
   public boolean isUseIntegrator()
   {
      return useIntegrator;
   }

   /**
    * Returns the type of gain coupling for the three dimensions.
    * <p>
    * The gain coupling determines whether all axis should use different
    * gains or if some axis (e.g. the X and Y axis for {@link GainCoupling#XY})
    * should use the same controller gains. This is useful when using a
    * YoVariable implementation of the gains {@link DefaultYoPID3DGains} since
    * it will cause the creation of different set of tuning variables.
    * </p>
    * @return the coupling of the controller gains for the three axes.
    */
   public GainCoupling getGainCoupling()
   {
      return gainCoupling;
   }

   /**
    * Returns the default set of gains to be used when PID gains are
    * created from this configuration. This can be null if the configuration
    * was created without default gains.
    *
    * @return default gains.
    */
   public PID3DGainsReadOnly getGains()
   {
      return gains;
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/io/files/DeprecatedFileTools.java<|end_filename|>
package us.ihmc.tools.io.files;

import java.io.BufferedInputStream;
import java.io.BufferedOutputStream;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.util.ArrayList;

public class DeprecatedFileTools
{

   /**
    * @deprecated Use FileTools.newBufferedReader(path). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static BufferedReader getFileReader(String filename) throws FileNotFoundException
   {
      return new BufferedReader(new InputStreamReader(new FileInputStream(filename)));
   }

   /**
    * @deprecated Use FileTools.newPrintWriter(path). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static PrintWriter getFileWriter(String filename) throws FileNotFoundException, IOException
   {
      return new PrintWriter(new BufferedWriter(new FileWriter(filename)));
   }

   /**
    * @deprecated Use Use FileTools.newPrintWriter(path, true). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static PrintWriter getFileWriterWithAppend(String filename) throws FileNotFoundException, IOException
   {
      return new PrintWriter(new BufferedWriter(new FileWriter(filename, true)));
   }

   /**
    * @deprecated Use FileTools.getFileDataOutputStream(path). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static DataOutputStream getFileDataOutputStream(String filename) throws FileNotFoundException, IOException
   {
      return new DataOutputStream(new BufferedOutputStream(new FileOutputStream(filename)));
   }

   /**
    * @deprecated Use FileTools.getFileDataInputStream(path). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static DataInputStream getFileDataInputStream(String filename) throws FileNotFoundException, IOException
   {
      return new DataInputStream(new BufferedInputStream(new FileInputStream(filename)));
   }

   /**
    * Gets all files in the directory
    * through recursive calls
    * 
    * @deprecated Use PathTools.walkRecursively(directory, basicFileVisitor). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    *
    * @param directory File
    * @return ArrayList
    */
   public static ArrayList<File> getAllFilesInDirectoryRecursive(File directory)
   {      
      return DeprecatedFileTools.getAllFilesInDirectoryRecursiveRegex(".*", directory);
   }

   /**
    * @deprecated Use PathTools.findAllPathsRecursivelyThatMatchRegex(rootPath, regex). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static ArrayList<File> getAllFilesInDirectoryRecursiveRegex(String regex, File directory)
   {
   // File[] files = new File[0];
      if (!directory.isDirectory())
      {
         throw new IllegalArgumentException(directory.getAbsolutePath() + " is not a directory");
      }
   
      ArrayList<File> ret = new ArrayList<File>();
      File[] contents = directory.listFiles();
      for (File file : contents)
      {
         if (!file.isHidden())
         {
            if (file.isDirectory())
            {
               ret.addAll(getAllFilesInDirectoryRecursiveRegex(regex, file));
            }
            else
            {
               if (file.getName().matches(regex))
                  ret.add(file);
            }
         }
      }
   
      return ret;
   }

   /**
    * @deprecated Use PathTools.findAllPathsRecursivelyThatMatchRegex(rootPath, regex). See <a href="http://docs.oracle.com/javase/tutorial/essential/io/fileio.html">File I/O (Featuring NIO.2)</a>
    */
   public static ArrayList<File> getAllFilesInDirectoryWithSuffix(String suffix, File directory)
   {
      if (directory.getName().equals(""))
         directory = new File(".");
   
      if (!directory.isDirectory())
      {
         throw new RuntimeException("This method can only be called with a directory: " + directory.toString());
      }
   
      ArrayList<File> allValidFiles = new ArrayList<File>();
   
      File[] allFiles = directory.listFiles();
      if ((allFiles == null) || (allFiles.length <= 0))
         return allValidFiles;
   
      for (File file : allFiles)
      {
         if (file.getName().endsWith(suffix) &&!file.isDirectory())
            allValidFiles.add(file);
      }
   
      return allValidFiles;
   }

}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tabs/TextImage.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tabs;

import javafx.scene.Scene;
import javafx.scene.image.Image;
import javafx.scene.layout.StackPane;
import javafx.scene.text.Text;

public class TextImage
{

   public static Image create(String text)
   {
      Text t = new Text(text);
      new Scene(new StackPane(t));
      return t.snapshot(null, null);
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/ProcessedIMUSensorsWriteOnlyInterface.java<|end_filename|>
package us.ihmc.robotics.sensors;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.tuple3D.Vector3D;

public interface ProcessedIMUSensorsWriteOnlyInterface
{
   public abstract void setAcceleration(FrameVector3D accelerationInWorld, int imuIndex);

   public abstract void setRotation(RotationMatrix rotationMatrix, int imuIndex);

   public abstract void setAngularVelocityInBody(Vector3D angularVelocityInBody, int imuIndex);

   public abstract void setAngularAccelerationInBody(Vector3D angularAccelerationInBody, int imuIndex);
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/EuclideanTrajectoryMessage.java<|end_filename|>
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 carries the information to execute a trajectory in taskspace (position only) by defining trajectory points.
       * A third order polynomial function is used to interpolate positions.
       * To execute a single straight line trajectory to reach a desired position, set only one trajectory point with zero velocity and its time to be equal to the desired trajectory time.
       */
public class EuclideanTrajectoryMessage extends Packet<EuclideanTrajectoryMessage> implements Settable<EuclideanTrajectoryMessage>, EpsilonComparable<EuclideanTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * List of trajectory points (in taskpsace) to go through while executing the trajectory.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.EuclideanTrajectoryPointMessage>  taskspace_trajectory_points_;
   /**
            * The selection matrix for each axis.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage selection_matrix_;
   /**
            * Frame information for this message.
            */
   public controller_msgs.msg.dds.FrameInformation frame_information_;
   /**
            * The weight matrix for each axis.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage weight_matrix_;
   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public boolean use_custom_control_frame_;
   /**
            * Pose of custom control frame. This is the frame attached to the rigid body that the taskspace trajectory is defined for.
            */
   public us.ihmc.euclid.geometry.Pose3D control_frame_pose_;
   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage queueing_properties_;

   public EuclideanTrajectoryMessage()
   {
      taskspace_trajectory_points_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.EuclideanTrajectoryPointMessage> (50, new controller_msgs.msg.dds.EuclideanTrajectoryPointMessagePubSubType());
      selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      frame_information_ = new controller_msgs.msg.dds.FrameInformation();
      weight_matrix_ = new controller_msgs.msg.dds.WeightMatrix3DMessage();
      control_frame_pose_ = new us.ihmc.euclid.geometry.Pose3D();
      queueing_properties_ = new controller_msgs.msg.dds.QueueableMessage();

   }

   public EuclideanTrajectoryMessage(EuclideanTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(EuclideanTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      taskspace_trajectory_points_.set(other.taskspace_trajectory_points_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.selection_matrix_, selection_matrix_);
      controller_msgs.msg.dds.FrameInformationPubSubType.staticCopy(other.frame_information_, frame_information_);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.staticCopy(other.weight_matrix_, weight_matrix_);
      use_custom_control_frame_ = other.use_custom_control_frame_;

      geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.control_frame_pose_, control_frame_pose_);
      controller_msgs.msg.dds.QueueableMessagePubSubType.staticCopy(other.queueing_properties_, queueing_properties_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * List of trajectory points (in taskpsace) to go through while executing the trajectory.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.EuclideanTrajectoryPointMessage>  getTaskspaceTrajectoryPoints()
   {
      return taskspace_trajectory_points_;
   }


   /**
            * The selection matrix for each axis.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage getSelectionMatrix()
   {
      return selection_matrix_;
   }


   /**
            * Frame information for this message.
            */
   public controller_msgs.msg.dds.FrameInformation getFrameInformation()
   {
      return frame_information_;
   }


   /**
            * The weight matrix for each axis.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage getWeightMatrix()
   {
      return weight_matrix_;
   }

   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public void setUseCustomControlFrame(boolean use_custom_control_frame)
   {
      use_custom_control_frame_ = use_custom_control_frame;
   }
   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public boolean getUseCustomControlFrame()
   {
      return use_custom_control_frame_;
   }


   /**
            * Pose of custom control frame. This is the frame attached to the rigid body that the taskspace trajectory is defined for.
            */
   public us.ihmc.euclid.geometry.Pose3D getControlFramePose()
   {
      return control_frame_pose_;
   }


   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage getQueueingProperties()
   {
      return queueing_properties_;
   }


   public static Supplier<EuclideanTrajectoryMessagePubSubType> getPubSubType()
   {
      return EuclideanTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return EuclideanTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(EuclideanTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.taskspace_trajectory_points_.size() != other.taskspace_trajectory_points_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.taskspace_trajectory_points_.size(); i++)
         {  if (!this.taskspace_trajectory_points_.get(i).epsilonEquals(other.taskspace_trajectory_points_.get(i), epsilon)) return false; }
      }

      if (!this.selection_matrix_.epsilonEquals(other.selection_matrix_, epsilon)) return false;
      if (!this.frame_information_.epsilonEquals(other.frame_information_, epsilon)) return false;
      if (!this.weight_matrix_.epsilonEquals(other.weight_matrix_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_custom_control_frame_, other.use_custom_control_frame_, epsilon)) return false;

      if (!this.control_frame_pose_.epsilonEquals(other.control_frame_pose_, epsilon)) return false;
      if (!this.queueing_properties_.epsilonEquals(other.queueing_properties_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof EuclideanTrajectoryMessage)) return false;

      EuclideanTrajectoryMessage otherMyClass = (EuclideanTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.taskspace_trajectory_points_.equals(otherMyClass.taskspace_trajectory_points_)) return false;
      if (!this.selection_matrix_.equals(otherMyClass.selection_matrix_)) return false;
      if (!this.frame_information_.equals(otherMyClass.frame_information_)) return false;
      if (!this.weight_matrix_.equals(otherMyClass.weight_matrix_)) return false;
      if(this.use_custom_control_frame_ != otherMyClass.use_custom_control_frame_) return false;

      if (!this.control_frame_pose_.equals(otherMyClass.control_frame_pose_)) return false;
      if (!this.queueing_properties_.equals(otherMyClass.queueing_properties_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("EuclideanTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("taskspace_trajectory_points=");
      builder.append(this.taskspace_trajectory_points_);      builder.append(", ");
      builder.append("selection_matrix=");
      builder.append(this.selection_matrix_);      builder.append(", ");
      builder.append("frame_information=");
      builder.append(this.frame_information_);      builder.append(", ");
      builder.append("weight_matrix=");
      builder.append(this.weight_matrix_);      builder.append(", ");
      builder.append("use_custom_control_frame=");
      builder.append(this.use_custom_control_frame_);      builder.append(", ");
      builder.append("control_frame_pose=");
      builder.append(this.control_frame_pose_);      builder.append(", ");
      builder.append("queueing_properties=");
      builder.append(this.queueing_properties_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ObjectDetectorResultPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "ObjectDetectorResultPacket" defined in "ObjectDetectorResultPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from ObjectDetectorResultPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit ObjectDetectorResultPacket_.idl instead.
*
*/
public class ObjectDetectorResultPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.ObjectDetectorResultPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::ObjectDetectorResultPacket_";

   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.ObjectDetectorResultPacket 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.ObjectDetectorResultPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += controller_msgs.msg.dds.HeatMapPacketPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += controller_msgs.msg.dds.BoundingBoxesPacketPubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ObjectDetectorResultPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ObjectDetectorResultPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += controller_msgs.msg.dds.HeatMapPacketPubSubType.getCdrSerializedSize(data.getHeatMap(), current_alignment);

      current_alignment += controller_msgs.msg.dds.BoundingBoxesPacketPubSubType.getCdrSerializedSize(data.getBoundingBoxes(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.ObjectDetectorResultPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      controller_msgs.msg.dds.HeatMapPacketPubSubType.write(data.getHeatMap(), cdr);
      controller_msgs.msg.dds.BoundingBoxesPacketPubSubType.write(data.getBoundingBoxes(), cdr);
   }

   public static void read(controller_msgs.msg.dds.ObjectDetectorResultPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      controller_msgs.msg.dds.HeatMapPacketPubSubType.read(data.getHeatMap(), cdr);	
      controller_msgs.msg.dds.BoundingBoxesPacketPubSubType.read(data.getBoundingBoxes(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.ObjectDetectorResultPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_a("heat_map", new controller_msgs.msg.dds.HeatMapPacketPubSubType(), data.getHeatMap());

      ser.write_type_a("bounding_boxes", new controller_msgs.msg.dds.BoundingBoxesPacketPubSubType(), data.getBoundingBoxes());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.ObjectDetectorResultPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_a("heat_map", new controller_msgs.msg.dds.HeatMapPacketPubSubType(), data.getHeatMap());

      ser.read_type_a("bounding_boxes", new controller_msgs.msg.dds.BoundingBoxesPacketPubSubType(), data.getBoundingBoxes());

   }

   public static void staticCopy(controller_msgs.msg.dds.ObjectDetectorResultPacket src, controller_msgs.msg.dds.ObjectDetectorResultPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.ObjectDetectorResultPacket createData()
   {
      return new controller_msgs.msg.dds.ObjectDetectorResultPacket();
   }
   @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.ObjectDetectorResultPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.ObjectDetectorResultPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.ObjectDetectorResultPacket src, controller_msgs.msg.dds.ObjectDetectorResultPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public ObjectDetectorResultPacketPubSubType newInstance()
   {
      return new ObjectDetectorResultPacketPubSubType();
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/clouds/detect/alg/ConfigSchnabel2007.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.clouds.detect.alg;

import java.util.ArrayList;
import java.util.List;

import georegression.fitting.cylinder.CodecCylinder3D_F64;
import georegression.fitting.cylinder.FitCylinderToPoints_F64;
import georegression.fitting.cylinder.ModelManagerCylinder3D_F64;
import georegression.fitting.plane.CodecPlaneGeneral3D_F64;
import georegression.fitting.plane.ModelManagerPlaneGeneral3D_F64;
import georegression.fitting.sphere.CodecSphere3D_F64;
import georegression.fitting.sphere.FitSphereToPoints_F64;
import georegression.fitting.sphere.ModelManagerSphere3D_F64;
import us.ihmc.sensorProcessing.bubo.clouds.detect.CloudShapeTypes;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.DistanceCylinderToPointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.DistancePlaneToPointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.DistanceSphereToPointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.GenerateCylinderPointVector;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.GeneratePlanePointVector;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.GenerateSpherePointVector;
import us.ihmc.sensorProcessing.bubo.clouds.detect.wrapper.PlaneGeneralSvd_to_ModelFitter;

/**
 * Tuning parameters for {@link PointCloudShapeDetectionSchnabel2007}
 *
 * @author <NAME>
 */
public class ConfigSchnabel2007 {
	/**
	 * description of the shapes it will search for
	 */
	public List<ShapeDescription> models;
	/**
	 * Maximum number of iterations it will perform while refining a shape after detection by RANSAC.
	 */
	public int localFitMaxIterations = 100;
	/**
	 * Exit criteria for shape refinement.  When the model parameters change less than this it exits.
	 */
	public double localFitChangeThreshold = 1e-8;
	/**
	 * Children will be created in an Octree node when the number of points contained inside exceed this number.
	 * Optimal value is data dependent.  Try 50.
	 */
	public int octreeSplit = 100;
	/**
	 * THe minimum number of points that are in the inlier set for a shape for the shape to be accepted.
	 */
	public int minModelAccept = 50;
	/**
	 * RANSAC will be extended by this many iterations each time a new best fit model is found.
	 */
	public int ransacExtension = 15;
	/**
	 * The total maximum number of allowed iterations to find a shape.  This is used to limit the time
	 * spent searching for shapes.  Try 1000
	 */
	public int maximumAllowedIterations = 1000;
	/**
	 * Random seed used in various places, such as RANSAC
	 */
	public long randomSeed = 0xDEADBEEF;

	/**
	 * Creates a default set of parameters which can detect
	 *
	 * @param fitIterations           Number of iterations when refining a shape
	 * @param angleTolerance          Tolerance in radians to reject a model from an initial sample set
	 * @param ransacDistanceThreshold Euclidean distance that RANSAC considers a point an inlier
	 * @param shapes                  A list of shape you wish to detect.  If empty or null then all possible shapes will be detected.
	 * @return ConfigSchnabel2007
	 */
	public static ConfigSchnabel2007 createDefault(int fitIterations,
												   double angleTolerance,
												   double ransacDistanceThreshold,
												   CloudShapeTypes... shapes) {
		if (shapes == null || shapes.length == 0) {
			shapes = CloudShapeTypes.values();
		}
		List<ShapeDescription> objects = new ArrayList<ShapeDescription>();

		int index = 0;
		for (CloudShapeTypes shape : shapes) {
			switch (shape) {
				case SPHERE: {
					ShapeDescription sphere = new ShapeDescription();
					sphere.modelManager = new ModelManagerSphere3D_F64();
					sphere.modelDistance = new DistanceSphereToPointVectorNN(angleTolerance);
					sphere.modelGenerator = new GenerateSpherePointVector(angleTolerance, ransacDistanceThreshold);
					sphere.modelFitter = new ModelFitter_P_to_PVNN(new FitSphereToPoints_F64(fitIterations));
					sphere.codec = new CodecSphere3D_F64();
					sphere.thresholdFit = ransacDistanceThreshold;
					objects.add(sphere);
				}
				break;

				case CYLINDER: {
					ShapeDescription cylinder = new ShapeDescription();
					cylinder.modelManager = new ModelManagerCylinder3D_F64();
					cylinder.modelDistance = new DistanceCylinderToPointVectorNN(angleTolerance);
					cylinder.modelGenerator = new GenerateCylinderPointVector(angleTolerance, ransacDistanceThreshold);
					cylinder.modelFitter = new ModelFitter_P_to_PVNN(new FitCylinderToPoints_F64(fitIterations));
					cylinder.codec = new CodecCylinder3D_F64();
					cylinder.thresholdFit = ransacDistanceThreshold;
					objects.add(cylinder);
				}
				break;

				case PLANE: {
					ShapeDescription plane = new ShapeDescription();
					plane.modelManager = new ModelManagerPlaneGeneral3D_F64();
					plane.modelDistance = new DistancePlaneToPointVectorNN(angleTolerance);
					plane.modelGenerator = new GeneratePlanePointVector(angleTolerance);
					plane.modelFitter = new ModelFitter_P_to_PVNN(new PlaneGeneralSvd_to_ModelFitter());
					plane.codec = new CodecPlaneGeneral3D_F64();
					plane.thresholdFit = ransacDistanceThreshold;
					objects.add(plane);
				}
				break;

				default:
					throw new IllegalArgumentException("Unsupported shape: " + shape);
			}
			index++;
		}

		ConfigSchnabel2007 config = new ConfigSchnabel2007();
		config.models = objects;

		return config;
	}

	/**
	 * Checks to see if the specified parameters are internally consistent
	 */
	public void checkConfig() {

	}
}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/SimpleVisibilityGraphsUI.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui;

import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.API;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.GoalEditModeEnabled;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.GoalPosition;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.PlanarRegionData;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.RandomizePlanarRegionIDRequest;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.ShowGoalPosition;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.ShowPlanarRegions;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.ShowStartPosition;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.StartEditModeEnabled;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.StartPosition;

import java.io.IOException;

import javafx.fxml.FXML;
import javafx.fxml.FXMLLoader;
import javafx.scene.Scene;
import javafx.scene.control.SplitPane;
import javafx.scene.layout.BorderPane;
import javafx.scene.layout.Pane;
import javafx.stage.Stage;
import us.ihmc.javaFXToolkit.messager.JavaFXMessager;
import us.ihmc.javaFXToolkit.messager.SharedMemoryJavaFXMessager;
import us.ihmc.javaFXToolkit.scenes.View3DFactory;
import us.ihmc.pathPlanning.visibilityGraphs.ui.controllers.DatasetNavigationAccordionController;
import us.ihmc.pathPlanning.visibilityGraphs.ui.controllers.SimpleUIMenuController;
import us.ihmc.pathPlanning.visibilityGraphs.ui.controllers.StartGoalAnchorPaneController;
import us.ihmc.pathPlanning.visibilityGraphs.ui.controllers.VisibilityGraphsAnchorPaneController;
import us.ihmc.pathPlanning.visibilityGraphs.ui.controllers.VisibilityGraphsDataExporterAnchorPaneController;
import us.ihmc.pathPlanning.visibilityGraphs.ui.controllers.VisibilityGraphsParametersAnchorPaneController;
import us.ihmc.pathPlanning.visibilityGraphs.ui.viewers.PlanarRegionViewer;
import us.ihmc.pathPlanning.visibilityGraphs.ui.viewers.StartGoalPositionViewer;
import us.ihmc.pathPlanning.visibilityGraphs.ui.viewers.VisibilityGraphsRenderer;

public class SimpleVisibilityGraphsUI
{
   private final JavaFXMessager messager = new SharedMemoryJavaFXMessager(API);
   private final Stage primaryStage;
   private final BorderPane mainPane;

   private final PlanarRegionViewer planarRegionViewer;
   private final StartGoalPositionEditor startGoalEditor;
   private final StartGoalPositionViewer startGoalViewer;
   private final VisibilityGraphsRenderer visibilityGraphsRenderer;
   private final VisibilityGraphsDataExporter dataExporter;
   private final PlanarRegionIDRandomizer planarRegionIDRandomizer;

   @FXML
   private SplitPane centerSplitPane;
   @FXML
   private SimpleUIMenuController simpleUIMenuController;
   @FXML
   private StartGoalAnchorPaneController startGoalAnchorPaneController;
   @FXML
   private VisibilityGraphsAnchorPaneController visibilityGraphsAnchorPaneController;
   @FXML
   private VisibilityGraphsDataExporterAnchorPaneController visibilityGraphsDataExporterAnchorPaneController;
   @FXML
   private VisibilityGraphsParametersAnchorPaneController visibilityGraphsParametersAnchorPaneController;
   @FXML
   private DatasetNavigationAccordionController datasetNavigationAccordionController;

   public SimpleVisibilityGraphsUI(Stage primaryStage) throws Exception
   {
      this.primaryStage = primaryStage;

      FXMLLoader loader = new FXMLLoader();
      loader.setController(this);
      loader.setLocation(getClass().getResource(getClass().getSimpleName() + ".fxml"));

      mainPane = loader.load();

      messager.startMessager();

      simpleUIMenuController.attachMessager(messager);
      simpleUIMenuController.setMainWindow(primaryStage);
      startGoalAnchorPaneController.attachMessager(messager);
      startGoalAnchorPaneController.bindControls();
      visibilityGraphsAnchorPaneController.attachMessager(messager);
      visibilityGraphsAnchorPaneController.bindControls();

      visibilityGraphsDataExporterAnchorPaneController.attachMessager(messager);
      visibilityGraphsParametersAnchorPaneController.attachMessager(messager);
      visibilityGraphsParametersAnchorPaneController.bindControls();

      datasetNavigationAccordionController.attachMessager(messager);
      datasetNavigationAccordionController.setMainWindow(primaryStage);
      datasetNavigationAccordionController.bindControls();
      datasetNavigationAccordionController.load();

      dataExporter = new VisibilityGraphsDataExporter(messager);
      planarRegionIDRandomizer = new PlanarRegionIDRandomizer(messager);
      planarRegionIDRandomizer.setTopics(RandomizePlanarRegionIDRequest, PlanarRegionData);
      planarRegionIDRandomizer.start();

      View3DFactory view3dFactory = View3DFactory.createSubscene();
      view3dFactory.addCameraController(true);
      view3dFactory.addWorldCoordinateSystem(0.3);
      Pane subScene = view3dFactory.getSubSceneWrappedInsidePane();
      centerSplitPane.getItems().add(subScene);

      planarRegionViewer = new PlanarRegionViewer(messager, PlanarRegionData, ShowPlanarRegions);
      view3dFactory.addNodeToView(planarRegionViewer.getRoot());
      planarRegionViewer.start();
      startGoalEditor = new StartGoalPositionEditor(messager, subScene, StartEditModeEnabled, GoalEditModeEnabled, StartPosition, GoalPosition);
      startGoalEditor.start();

      startGoalViewer = new StartGoalPositionViewer(messager);
      startGoalViewer.setEditStartGoalTopics(StartEditModeEnabled, GoalEditModeEnabled);
      startGoalViewer.setPositionStartGoalTopics(StartPosition, GoalPosition);
      startGoalViewer.setShowStartGoalTopics(ShowStartPosition, ShowGoalPosition);
      view3dFactory.addNodeToView(startGoalViewer.getRoot());
      startGoalViewer.start();
      visibilityGraphsRenderer = new VisibilityGraphsRenderer(messager);
      view3dFactory.addNodeToView(visibilityGraphsRenderer.getRoot());
      visibilityGraphsRenderer.start();

      primaryStage.setTitle(getClass().getSimpleName());
      primaryStage.setMaximized(true);
      Scene mainScene = new Scene(mainPane, 600, 400);

      primaryStage.setScene(mainScene);
      primaryStage.setOnCloseRequest(event -> stop());
   }

   public void show(boolean showPlanarRegionFileChooser) throws IOException
   {
      primaryStage.show();

      if (showPlanarRegionFileChooser)
         simpleUIMenuController.loadPlanarRegion();
   }

   public void stop()
   {
      try
      {
         messager.closeMessager();
      }
      catch (Exception e)
      {
         e.printStackTrace();
      }

      planarRegionViewer.stop();
      startGoalEditor.stop();
      startGoalViewer.stop();
      visibilityGraphsRenderer.stop();
      dataExporter.stop();
      planarRegionIDRandomizer.stop();
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/thread/DebugInterruptableWorker.java<|end_filename|>
package us.ihmc.tools.thread;

import java.util.Random;

public class DebugInterruptableWorker extends InterruptableWorker
{
   private int result;
   private int endNumber;
   private int countBy;

   public void reset(int startNumber, int endNumber, int countBy)
   {
      super.reset();
      this.endNumber = endNumber;
      this.countBy = countBy;
      result = startNumber;
   }

   @Override
   public STATE doWork()
   {
      while (!isInterrupted())
      {
         result = result + countBy;
         if (result == endNumber)
            return STATE.SUCCESSFULLY_COMPLETED;
         if (result > endNumber)
            return STATE.UNSUCCESSFULLY_COMPLETED;
         busyWork();

         result = result + countBy;
         if (result == endNumber)
            return STATE.SUCCESSFULLY_COMPLETED;
         if (result > endNumber)
            return STATE.UNSUCCESSFULLY_COMPLETED;
         busyWork();

         result = result + countBy;
         if (result == endNumber)
            return STATE.SUCCESSFULLY_COMPLETED;
         if (result > endNumber)
            return STATE.UNSUCCESSFULLY_COMPLETED;
         busyWork();
      }

      return STATE.INTERRUPTED;
   }

   private void busyWork()
   {
      Random random = new Random(result);
      int n = random.nextInt(10000000);
      for (int i = 0; i < 5000000 + n; i++)
      {
         @SuppressWarnings("unused")
         int x = n * n;
      }
   }

   @Override
   public Object getCurrentResult()
   {
      return result;
   }

   public static void main(String[] args)
   {
      // successful completion
      System.out.println("----------------------------------------------");
      System.out.println("creating worker");
      DebugInterruptableWorker worker = new DebugInterruptableWorker();

      System.out.println("resetting worker");
      worker.reset(0, 100, 1);

      System.out.println("starting worker");
      worker.startWorkOnANewThread();

      System.out.println("letting worker finish");
      worker.waitForResult(100000);

      System.out.println("result");
      System.out.println("Worker finished with a state = " + worker.getCurrentState());
      System.out.println("Worker final result = " + worker.getCurrentResult());

      // unsuccessful completion
      System.out.println("----------------------------------------------");
      System.out.println("resetting worker to a non-solution");
      worker.reset(1, 200, 2);

      System.out.println("starting worker");
      worker.startWorkOnANewThread();

      System.out.println("letting worker finish");
      worker.waitForResult(100000);

      System.out.println("result");
      System.out.println("Worker finished with a state = " + worker.getCurrentState());
      System.out.println("Worker final result = " + worker.getCurrentResult());

      // timeout
      System.out.println("----------------------------------------------");
      System.out.println("creating worker");
      worker = new DebugInterruptableWorker();

      System.out.println("resetting worker");
      worker.reset(0, 555, 5);

      System.out.println("starting worker");
      worker.startWorkOnANewThread();

      System.out.println("letting worker finish");
      worker.waitForResult(1000);

      System.out.println("result");
      System.out.println("Worker finished with a state = " + worker.getCurrentState());
      System.out.println("Worker final result = " + worker.getCurrentResult());

      // interruption
      System.out.println("----------------------------------------------");
      System.out.println("resetting worker");
      worker.reset(0, 100, 1);

      System.out.println("starting worker");
      worker.startWorkOnANewThread();

      System.out.println("wait a bit");

      while (((Integer) worker.getCurrentResult()) < 40)
      {
         System.out.println("result=" + worker.getCurrentResult());

         try
         {
            Thread.sleep(300);
         }
         catch (Exception e)
         {
            e.printStackTrace();
         }
      }

      System.out.println("interrupt");
      long t = System.currentTimeMillis();
      worker.stopWork();
      System.out.println("interrupt took " + (System.currentTimeMillis() - t));

      System.out.println("result");
      System.out.println("Worker finished with a state = " + worker.getCurrentState());
      System.out.println("Worker final result = " + worker.getCurrentResult());

      // try to finish before interruption to test thread safety
      System.out.println("----------------------------------------------");
      System.out.println("resetting worker");
      worker.reset(0, 1, 1);

      System.out.println("starting worker");
      worker.startWorkOnANewThread();

      Thread thread = new Thread(new SetState(worker), "IHMC-InterruptableWorker");
      thread.setDaemon(true);
      thread.start();

      System.out.println("interrupt");
      t = System.currentTimeMillis();
      worker.stopWork();
      System.out.println("interrupt took " + (System.currentTimeMillis() - t));

      System.out.println("result");
      System.out.println("Worker finished with a state = " + worker.getCurrentState());
      System.out.println("Worker final result = " + worker.getCurrentResult());
   }

   static class SetState implements Runnable
   {
      private InterruptableWorker worker;

      public SetState(InterruptableWorker worker)
      {
         this.worker = worker;
      }

      @Override
      public void run()
      {
         worker.setCurrentState(STATE.INTERRUPTED);

         try
         {
            Thread.sleep(100);
         }
         catch (Exception e)
         {
         }
      }
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/walking/EndOfScriptCommand.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.walking;

import us.ihmc.communication.packets.Packet;

/**
 * Created with IntelliJ IDEA. User: pneuhaus Date: 5/16/13 Time: 3:43 PM To change this template
 * use File | Settings | File Templates.
 */
public class EndOfScriptCommand extends Packet<EndOfScriptCommand>
{
   public EndOfScriptCommand()
   {

   }

   @Override
   public void set(EndOfScriptCommand other)
   {
      setPacketInformation(other);
   }

   public String toString()
   {
      return this.getClass().getSimpleName();
   }

   @Override
   public boolean epsilonEquals(EndOfScriptCommand other, double epsilon)
   {
      return true;
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/mapping/LogitechExtreme3DMapping.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick.mapping;

import java.util.ArrayList;
import java.util.List;

import org.apache.commons.lang3.SystemUtils;

import com.google.common.collect.BiMap;
import com.google.common.collect.HashBiMap;

import net.java.games.input.Component;
import net.java.games.input.Component.Identifier;
import us.ihmc.tools.inputDevices.joystick.JoystickCompatibilityFilter;
import net.java.games.input.Event;

public enum LogitechExtreme3DMapping implements JoystickMapping
{
   TRIGGER,
   THROTTLE,
   HAT,
   STICK_ROLL,
   STICK_PITCH,
   STICK_YAW,
   BUTTON_2,
   BUTTON_3,
   BUTTON_4,
   BUTTON_5,
   BUTTON_6,
   BUTTON_7,
   BUTTON_8,
   BUTTON_9,
   BUTTON_10,
   BUTTON_11,
   BUTTON_12;

   public static final LogitechExtreme3DMapping[] values = values();

   private static final BiMap<Identifier, LogitechExtreme3DMapping> windowsBiMap = HashBiMap.create(values.length);
   private static final BiMap<Identifier, LogitechExtreme3DMapping> macBiMap = HashBiMap.create(values.length);
   private static final BiMap<Identifier, LogitechExtreme3DMapping> linuxBiMap = HashBiMap.create(values.length);

   static
   {
      mapValues(TRIGGER, Identifier.Button._0, Identifier.Button._0, Identifier.Button.TRIGGER);
      mapValues(THROTTLE, Identifier.Axis.SLIDER, Identifier.Axis.SLIDER, Identifier.Axis.SLIDER);
      mapValues(HAT, Identifier.Axis.POV, Identifier.Axis.POV, Identifier.Axis.POV);
      mapValues(STICK_ROLL, Identifier.Axis.X, Identifier.Axis.X, Identifier.Axis.X);
      mapValues(STICK_PITCH, Identifier.Axis.Y, Identifier.Axis.Y, Identifier.Axis.Y);
      mapValues(STICK_YAW, Identifier.Axis.RZ, Identifier.Axis.RZ, Identifier.Axis.RZ);
      mapValues(BUTTON_2, Identifier.Button._1, Identifier.Button._1, Identifier.Button.THUMB);
      mapValues(BUTTON_3, Identifier.Button._2, Identifier.Button._2, Identifier.Button.THUMB2);
      mapValues(BUTTON_4, Identifier.Button._3, Identifier.Button._3, Identifier.Button.TOP);
      mapValues(BUTTON_5, Identifier.Button._4, Identifier.Button._4, Identifier.Button.TOP2);
      mapValues(BUTTON_6, Identifier.Button._5, Identifier.Button._5, Identifier.Button.PINKIE);
      mapValues(BUTTON_7, Identifier.Button._6, Identifier.Button._6, Identifier.Button.BASE);
      mapValues(BUTTON_8, Identifier.Button._7, Identifier.Button._7, Identifier.Button.BASE2);
      mapValues(BUTTON_9, Identifier.Button._8, Identifier.Button._8, Identifier.Button.BASE3);
      mapValues(BUTTON_10, Identifier.Button._9, Identifier.Button._9, Identifier.Button.BASE4);
      mapValues(BUTTON_11, Identifier.Button._10, Identifier.Button._10, Identifier.Button.BASE5);
      mapValues(BUTTON_12, Identifier.Button._11, Identifier.Button._11, Identifier.Button.BASE6);
   }

   private static void mapValues(LogitechExtreme3DMapping mapping, Identifier windowsIdentifier, Identifier macIdentifier, Identifier linuxIdentifier)
   {
      windowsBiMap.put(windowsIdentifier, mapping);
      macBiMap.put(macIdentifier, mapping);
      linuxBiMap.put(linuxIdentifier, mapping);
   }

   public static List<JoystickCompatibilityFilter> getCompatibilityFilters()
   {
      ArrayList<JoystickCompatibilityFilter> compatibilityFilters = new ArrayList<>();
      
      return compatibilityFilters;
   }

   @Override
   public Identifier getIdentifier()
   {
      if (SystemUtils.IS_OS_WINDOWS)
      {
         return windowsBiMap.inverse().get(this);
      }
      else if (SystemUtils.IS_OS_MAC)
      {
         return macBiMap.inverse().get(this);
      }
      else if (SystemUtils.IS_OS_LINUX)
      {
         return linuxBiMap.inverse().get(this);
      }
      else
      {
         return null;
      }
   }
   
   public static LogitechExtreme3DMapping getMapping(Identifier identifier)
   {
      if (SystemUtils.IS_OS_WINDOWS)
      {
         return windowsBiMap.get(identifier);
      }
      else if (SystemUtils.IS_OS_MAC)
      {
         return macBiMap.get(identifier);
      }
      else if (SystemUtils.IS_OS_LINUX)
      {
         return linuxBiMap.get(identifier);
      }
      else
      {
         return null;
      }
   }
   
   private static LogitechExtreme3DMapping getMapping(Component component)
   {
      return getMapping(component.getIdentifier());
   }

   public static LogitechExtreme3DMapping getMapping(Event event)
   {
      return getMapping(event.getComponent());
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tree/ParameterTree.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tree;

import java.util.HashMap;
import java.util.List;
import java.util.Map;

import javafx.collections.ObservableList;
import javafx.scene.Node;
import javafx.scene.control.MenuItem;
import javafx.scene.control.TreeItem;
import javafx.scene.control.TreeView;
import us.ihmc.parameterTuner.guiElements.GuiParameter;
import us.ihmc.parameterTuner.guiElements.GuiParameterStatus;
import us.ihmc.parameterTuner.guiElements.GuiRegistry;
import us.ihmc.parameterTuner.guiElements.tuners.Tuner;
import us.ihmc.simulationconstructionset.util.RegularExpression;

public class ParameterTree extends TreeView<ParameterTreeValue>
{
   private List<GuiRegistry> registries;
   private final Map<String, ParameterTreeItem> treeItemMap = new HashMap<>();

   public ParameterTree()
   {
      super();
      setCellFactory(param -> new ParameterTreeCell());
   }

   public void setRegistries(List<GuiRegistry> registries, Map<String, Tuner> tuningNodes)
   {
      this.registries = registries;
      treeItemMap.clear();
      registries.stream().forEach(registry -> createItemsRecursive(registry, tuningNodes));
   }

   private void createItemsRecursive(GuiRegistry registry, Map<String, Tuner> tuningNodes)
   {
      ParameterTreeItem registryItem = new ParameterTreeItem(new ParameterTreeRegistry(registry));
      treeItemMap.put(registry.getUniqueName(), registryItem);
      registry.getRegistries().stream().forEach(child -> {
         createItemsRecursive(child, tuningNodes);
      });
      ObservableList<TreeItem<ParameterTreeValue>> selectedItems = getSelectionModel().getSelectedItems();
      registry.getParameters().stream().forEach(parameter -> {
         Tuner tuner = tuningNodes.get(parameter.getUniqueName());
         Node inputNode = tuner.getSimpleInputNode();
         ParameterTreeItem parameterItem = new ParameterTreeItem(new ParameterTreeParameter(parameter, inputNode, selectedItems));
         treeItemMap.put(parameter.getUniqueName(), parameterItem);

         MenuItem revealInTree = new MenuItem("Reveal in Tree");
         revealInTree.setOnAction(event -> {
            getSelectionModel().clearSelection();
            getSelectionModel().select(parameterItem);
            scrollTo(getSelectionModel().getSelectedIndex());
            requestFocus();
         });
         tuner.getContextMenu().getItems().add(revealInTree);
      });
   }

   public void filterRegistries(boolean hideNamespaces, GuiParameterStatus status, String regexParameters, String regexNamespaces)
   {
      if (registries == null)
      {
         return;
      }

      treeItemMap.values().stream().forEach(item -> {
         item.getChildren().clear();
      });

      ParameterTreeItem root = new ParameterTreeItem(null);
      root.setExpanded(true);
      setShowRoot(false);
      setRoot(root);

      boolean searchingParameters = regexParameters != null && !regexParameters.isEmpty();
      boolean searchingNamespaces = regexNamespaces != null && !regexNamespaces.isEmpty();
      boolean searching = searchingParameters || searchingNamespaces || status != GuiParameterStatus.ANY;

      registries.stream().forEach(registry -> {
         if (hideNamespaces && searching)
         {
            addAllMatching(registry.getAllParameters(), root, regexParameters, status);
         }
         else if (hideNamespaces)
         {
            addAllMatching(registry.getAllParameters(), root, "", status);
         }
         else if (searching)
         {
            addMatchingRecursive(registry, root, regexParameters, regexNamespaces, status);
         }
         else
         {
            addRecursive(registry, root, status);
            root.expandChildrenForSmallRegistries();
         }
      });
   }

   private void addMatchingRecursive(GuiRegistry registry, ParameterTreeItem item, String regexParameters, String regexNamespaces, GuiParameterStatus status)
   {
      if (RegularExpression.check(registry.getName(), regexNamespaces))
      {
         ParameterTreeItem registryItem = treeItemMap.get(registry.getUniqueName());
         registryItem.setExpanded(true);
         addAllMatching(registry.getParameters(), registryItem, regexParameters, status);
         if (!registryItem.getChildren().isEmpty())
         {
            item.getChildren().add(registryItem);
         }
      }

      registry.getRegistries().stream().forEach(child -> {
         addMatchingRecursive(child, item, regexParameters, regexNamespaces, status);
      });
   }

   private void addRecursive(GuiRegistry registry, ParameterTreeItem item, GuiParameterStatus status)
   {
      ParameterTreeItem registryItem = treeItemMap.get(registry.getUniqueName());
      item.getChildren().add(registryItem);
      registry.getRegistries().stream().forEach(child -> {
         addRecursive(child, registryItem, status);
      });
      addAllMatching(registry.getParameters(), registryItem, "", status);
   }

   private void addAllMatching(List<GuiParameter> parameters, ParameterTreeItem item, String regex, GuiParameterStatus status)
   {
      parameters.stream().filter(parameter -> RegularExpression.check(parameter.getName(), regex) && status.matches(parameter)).forEach(parameter -> {
         ParameterTreeItem parameterItem = treeItemMap.get(parameter.getUniqueName());
         item.getChildren().add(parameterItem);
      });
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/environmentRobots/ContactableRobot.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.environmentRobots;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.GroundContactPoint;
import us.ihmc.simulationconstructionset.Joint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.Contactable;

public abstract class ContactableRobot extends Robot implements Contactable
{
   private final InternalSingleJointArticulatedContactable articulatedContactable;

   public ContactableRobot(String name)
   {
      super(name);
      articulatedContactable = new InternalSingleJointArticulatedContactable(name, this);
   }

   public abstract FloatingJoint getFloatingJoint();

   public abstract void setMass(double mass);

   public abstract void setMomentOfInertia(double Ixx, double Iyy, double Izz);

   private static class InternalSingleJointArticulatedContactable extends SingleJointArticulatedContactable
   {
      private final ContactableRobot contactableRobot;

      public InternalSingleJointArticulatedContactable(String name, ContactableRobot robot)
      {
         super(name, robot);
         this.contactableRobot = robot;
      }

      @Override
      public boolean isClose(Point3D pointInWorldToCheck)
      {
         return contactableRobot.isClose(pointInWorldToCheck);
      }

      @Override
      public boolean isPointOnOrInside(Point3D pointInWorldToCheck)
      {
         return contactableRobot.isPointOnOrInside(pointInWorldToCheck);
      }

      @Override
      public void closestIntersectionAndNormalAt(Point3D intersectionToPack, Vector3D normalToPack, Point3D pointInWorldToCheck)
      {
         contactableRobot.closestIntersectionAndNormalAt(intersectionToPack, normalToPack, pointInWorldToCheck);
      }

      @Override
      public Joint getJoint()
      {
         return contactableRobot.getFloatingJoint();
      }
   }

   public void createAvailableContactPoints(int groupIdentifier, int totalContactPointsAvailable, double forceVectorScale,
         boolean addYoGraphicForceVectorsForceVectors)
   {
      articulatedContactable.createAvailableContactPoints(groupIdentifier, totalContactPointsAvailable, forceVectorScale,
            addYoGraphicForceVectorsForceVectors);
   }

   @Override
   public int getAndLockAvailableContactPoint()
   {
      return articulatedContactable.getAndLockAvailableContactPoint();
   }

   @Override
   public void unlockContactPoint(GroundContactPoint groundContactPoint)
   {
      articulatedContactable.unlockContactPoint(groundContactPoint);
   }

   @Override
   public GroundContactPoint getLockedContactPoint(int contactPointIndex)
   {
      return articulatedContactable.getLockedContactPoint(contactPointIndex);
   }

   @Override
   public void updateContactPoints()
   {
      articulatedContactable.updateContactPoints();
   }

   public void setPosition(double x, double y, double z)
   {
      getFloatingJoint().setPosition(x, y, z);
   }

   public void setPosition(double[] position)
   {
      getFloatingJoint().setPosition(position[0], position[1], position[2]);
   }

   public void setPosition(Tuple3DBasics position)
   {
      getFloatingJoint().setPosition(position);
   }

   public void setYawPitchRoll(double yaw, double pitch, double roll)
   {
      getFloatingJoint().setYawPitchRoll(yaw, pitch, roll);
   }

   public void setVelocity(double xd, double yd, double zd)
   {
      getFloatingJoint().setVelocity(xd, yd, zd);
   }

   public void setVelocity(Tuple3DBasics velocity)
   {
      getFloatingJoint().setVelocity(velocity);
   }

   public void setPositionAndVelocity(double x, double y, double z, double dx, double dy, double dz)
   {
      getFloatingJoint().setPositionAndVelocity(x, y, z, dx, dy, dz);
   }

   public void setPositionAndVelocity(Point3D position, Vector3D velocity)
   {
      getFloatingJoint().setPositionAndVelocity(position, velocity);
   }

   public void setAngularVelocityInBody(Vector3D velocity)
   {
      getFloatingJoint().setAngularVelocityInBody(velocity);
   }

   public void getBodyTransformToWorld(RigidBodyTransform transformToWorld)
   {
      getFloatingJoint().getTransformToWorld(transformToWorld);
   }

   public void getVelocity(Tuple3DBasics velocityToPack)
   {
      getFloatingJoint().getVelocity(velocityToPack);
   }

   public void getPosition(Tuple3DBasics positionToPack)
   {
      getFloatingJoint().getPosition(positionToPack);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/HeightMapWithPoints.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.geometry.InclusionFunction;

public interface HeightMapWithPoints
{
   abstract double getHeightAtPoint(double x, double y);
   abstract boolean addPoint(double x, double y, double z);
   abstract boolean containsPoint(double x, double y);
   abstract void clear();
   abstract List<Point3D> getAllPointsWithinArea(double xCenter, double yCenter, double xExtent, double yExtent);
   abstract List<Point3D> getAllPointsWithinArea(double xCenter, double yCenter, double xExtent, double yExtent,
         InclusionFunction<Point3D> maskFunctionAboutCenter);
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/kinematicToolboxDiagnostics/AtlasKinematicToolboxDiagnosticEnvironment.java<|end_filename|>
package us.ihmc.atlas.kinematicToolboxDiagnostics;

import us.ihmc.atlas.AtlasRobotModel;
import us.ihmc.atlas.AtlasRobotVersion;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.avatar.networkProcessor.kinematicsToolboxModule.KinematicToolboxDiagnosticEnvironment;

public class AtlasKinematicToolboxDiagnosticEnvironment
{
   public static void main(String[] args)
   {
      AtlasRobotVersion atlasVersion = AtlasRobotVersion.ATLAS_UNPLUGGED_V5_DUAL_ROBOTIQ;
      RobotTarget target = RobotTarget.REAL_ROBOT;
      boolean headless = false;
      AtlasRobotModel atlasModel = new AtlasRobotModel(atlasVersion, target, headless);

      new KinematicToolboxDiagnosticEnvironment(atlasModel);
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/jointLimits/JointLimitsController.java<|end_filename|>
package us.ihmc.exampleSimulations.jointLimits;

import us.ihmc.commons.MathTools;
import us.ihmc.yoVariables.variable.YoDouble;
import us.ihmc.robotics.math.filters.AlphaFilteredYoVariable;
import us.ihmc.simulationConstructionSetTools.robotController.SimpleRobotController;

public class JointLimitsController extends SimpleRobotController
{
   private static final double maxAbsJointVelocity = 9.0; // rad/s
   private static final double jointLimitDistanceForMaxVelocity = 30.0; // degree

   private final JointLimitsRobot robot;

   private final YoDouble magnitude = new YoDouble("Magnitude", registry);
   private final YoDouble frequency = new YoDouble("Frequency", registry);
   private final YoDouble gain = new YoDouble("Gain", registry);

   private final YoDouble qDesired = new YoDouble("QDesired", registry);
   private final YoDouble qddDesired = new YoDouble("QDDDesired", registry);
   private final YoDouble qddDesiredLimited = new YoDouble("QDDDesiredLimited", registry);

   private final YoDouble lowerLimit = new YoDouble("LowerLimit", registry);
   private final YoDouble upperLimit = new YoDouble("UpperLimit", registry);

   private final YoDouble qddMax = new YoDouble("QDDMax", registry);
   private final YoDouble qddMin = new YoDouble("QDDMin", registry);
   private final YoDouble qddMaxAbs = new YoDouble("QDDMaxAbs", registry);

   private final YoDouble filterAlpha = new YoDouble("FilterAlpha", registry);
   private final AlphaFilteredYoVariable lowerLimitFiltered = new AlphaFilteredYoVariable("LowerLimitFiltered", registry, filterAlpha);
   private final AlphaFilteredYoVariable upperLimitFiltered = new AlphaFilteredYoVariable("UpperLimitFiltered", registry, filterAlpha);

   private final YoDouble slope = new YoDouble("Slope", registry);

   public JointLimitsController(JointLimitsRobot robot, double controlDT)
   {
      this.robot = robot;

      magnitude.set(0.75);
      frequency.set(1.0);
      gain.set(10.0);

      lowerLimit.set(robot.getLowerLimit());
      upperLimit.set(robot.getUpperLimit());

      filterAlpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(10.0, controlDT));
      slope.set(maxAbsJointVelocity / Math.pow((jointLimitDistanceForMaxVelocity * Math.PI/180.0), 2.0));

      qddMaxAbs.set(10.0);
   }

   @Override
   public void doControl()
   {
      double f = frequency.getDoubleValue();
      double m = magnitude.getDoubleValue();

      double q_d = m * Math.sin(f * robot.getTime());
      double qdd_ff = -f*f*m * Math.sin(f * robot.getTime());
      double qdd_fb = (q_d - robot.getQ()) * gain.getDoubleValue();

      qDesired.set(q_d);
      qddDesired.set(qdd_ff + qdd_fb);

      limitQdd();
      robot.setQdd(qddDesiredLimited.getDoubleValue());
   }

   private void limitQdd()
   {
      qddMax.set(qddMaxAbs.getDoubleValue());
      qddMin.set(-qddMaxAbs.getDoubleValue());

      // --- do limiting here ---
      double timeHorizon = 0.02;
      double absoluteMaximumJointAcceleration = qddMaxAbs.getDoubleValue();
      double maxBreakAcceleration = 4.0;
      if (!Double.isInfinite(lowerLimit.getDoubleValue()))
      {
         // if we enter the limit do not explode:
         double distance = robot.getQ() - lowerLimit.getDoubleValue();
         distance = Math.max(0.0, distance);

         double qDotMin = -Math.pow(distance, 2) * slope.getDoubleValue();
         double qDDotMin = (qDotMin - robot.getQd()) / timeHorizon;
         qDDotMin = MathTools.clamp(qDDotMin, -absoluteMaximumJointAcceleration, maxBreakAcceleration);

         lowerLimitFiltered.update(qDDotMin);
         qddMin.set(lowerLimitFiltered.getDoubleValue());
      }
      if (!Double.isInfinite(upperLimit.getDoubleValue()))
      {
         // if we enter the limit do not explode:
         double distance = upperLimit.getDoubleValue() - robot.getQ();
         distance = Math.max(0.0, distance);

         double qDotMax = Math.pow(distance, 2) * slope.getDoubleValue();
         double qDDotMax = (qDotMax - robot.getQd()) / timeHorizon;
         qDDotMax = MathTools.clamp(qDDotMax, -maxBreakAcceleration, absoluteMaximumJointAcceleration);

         upperLimitFiltered.update(qDDotMax);
         qddMax.set(upperLimitFiltered.getDoubleValue());
      }
      // ---

      double qddLimited = MathTools.clamp(qddDesired.getDoubleValue(), qddMin.getDoubleValue(), qddMax.getDoubleValue());
      qddDesiredLimited.set(qddLimited);
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/clouds/detect/PointCloudShapeFinder.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.clouds.detect;

import java.util.ArrayList;
import java.util.List;

import org.ddogleg.struct.GrowQueue_I32;

import georegression.struct.point.Point3D_F64;
import georegression.struct.shapes.Box3D_F64;

/**
 * High level interface for finding different primitive shapes inside a point cloud. A list of found shapes is returned
 * and points which were not assigned to any shapes.  For each shape the following information is returned, the
 * shape type, shape parameters,and set of points belonging to the shape.  Depending on the implementations
 * a single point can belong to one or more shapes.
 *
 * @author <NAME>
 */
public interface PointCloudShapeFinder {

	/**
	 * <p>
	 * Searches the point cloud for primitive shapes.
	 * </p>
	 * <p>
	 * WARNING: To reduce memory overhead, object which are passed have there reference saved.  This means you
	 * shouldn't modify the input if you care about the points returned with the output shape.
	 * </p>
	 *
	 * @param cloud       The point cloud which is to be searched
	 * @param boundingBox Optional bounding box which is known to contain all the points.
	 *                    If none is provided set to null.
	 */
	public void process(List<Point3D_F64> cloud, Box3D_F64 boundingBox);

	/**
	 * <p>
	 * List of shapes that were found in the point cloud.
	 * </p>
	 * <p>
	 * WARNING: All data structures returned are owned by this class.  They are subject to being modified
	 * the next time process is called.  The points might also be references to the input point cloud.
	 * </p>
	 *
	 * @return List of shapes
	 */
	public List<Shape> getFound();

	/**
	 * <p>
	 * A list of points which were not matched to any shape.  Calling this function can result in additional
	 * computations and should only be called once for each call to
	 * {@link #process(java.util.List, georegression.struct.shapes.Box3D_F64)};
	 * </p>
	 * <p/>
	 * <p>
	 * WARNING: All data structures returned are owned by this class.  They are subject to being modified
	 * the next time process is called.  The points might also be references to the input point cloud.
	 * </p>
	 *
	 * @param unmatched Storage for Point3D_F64 which matched the shape.
	 */
	public void getUnmatched(List<Point3D_F64> unmatched);

	/**
	 * Returns a list of shapes that it can find
	 *
	 * @return List of shape types
	 */
	public List<CloudShapeTypes> getShapesList();

	/**
	 * Can it detect multiple objects in the point cloud or does it just search for a single object?
	 *
	 * @return If true it supports finding multiple object.  If false then it will only search for a single object.
	 */
	public boolean isSupportMultipleObjects();

	/**
	 * Storage for output
	 */
	public static class Shape {
		/**
		 * The type of shape
		 */
		public CloudShapeTypes type;
		/**
		 * Parameters for the shape.  See {@link CloudShapeTypes} for data structute type.
		 */
		public Object parameters;
		/**
		 * List of points that matched this shape
		 */
		public List<Point3D_F64> points = new ArrayList<Point3D_F64>();
		/**
		 * Index of the point in the original cloud
		 */
		public GrowQueue_I32 indexes = new GrowQueue_I32();

		public <T>T getParameters() {
			return (T)parameters;
		}
	}
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/genericQuadruped/simulation/GenericQuadrupedGroundContactParameters.java<|end_filename|>
package us.ihmc.exampleSimulations.genericQuadruped.simulation;

import us.ihmc.quadrupedRobotics.simulation.GroundContactParameters;

public class GenericQuadrupedGroundContactParameters implements GroundContactParameters
{
   @Override
   public double getZStiffness()
   {
      return 5000.0;
   }
   
   @Override
   public double getZDamping()
   {
      return 600.0;
   }
   
   @Override
   public double getXYStiffness()
   {
      return 15000.0;
   }
   
   @Override
   public double getXYDamping()
   {
      return 700.0;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/StateEstimatorModePacket.java<|end_filename|>
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.
       */
public class StateEstimatorModePacket extends Packet<StateEstimatorModePacket> implements Settable<StateEstimatorModePacket>, EpsilonComparable<StateEstimatorModePacket>
{
   public static final byte NORMAL = (byte) 0;
   public static final byte FROZEN = (byte) 1;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte requested_state_estimator_mode_ = (byte) 255;

   public StateEstimatorModePacket()
   {
   }

   public StateEstimatorModePacket(StateEstimatorModePacket other)
   {
      this();
      set(other);
   }

   public void set(StateEstimatorModePacket other)
   {
      sequence_id_ = other.sequence_id_;

      requested_state_estimator_mode_ = other.requested_state_estimator_mode_;

   }

   /**
            * 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 void setRequestedStateEstimatorMode(byte requested_state_estimator_mode)
   {
      requested_state_estimator_mode_ = requested_state_estimator_mode;
   }
   public byte getRequestedStateEstimatorMode()
   {
      return requested_state_estimator_mode_;
   }


   public static Supplier<StateEstimatorModePacketPubSubType> getPubSubType()
   {
      return StateEstimatorModePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return StateEstimatorModePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(StateEstimatorModePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.requested_state_estimator_mode_, other.requested_state_estimator_mode_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof StateEstimatorModePacket)) return false;

      StateEstimatorModePacket otherMyClass = (StateEstimatorModePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.requested_state_estimator_mode_ != otherMyClass.requested_state_estimator_mode_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("StateEstimatorModePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("requested_state_estimator_mode=");
      builder.append(this.requested_state_estimator_mode_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/ReachabilityMapListener.java<|end_filename|>
package us.ihmc.avatar.reachabilityMap;

public interface ReachabilityMapListener
{
   public abstract void hasReachedNewConfiguration();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PilotInterfacePacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "PilotInterfacePacket" defined in "PilotInterfacePacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from PilotInterfacePacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit PilotInterfacePacket_.idl instead.
*
*/
public class PilotInterfacePacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.PilotInterfacePacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::PilotInterfacePacket_";

   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.PilotInterfacePacket 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.PilotInterfacePacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      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.PilotInterfacePacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.PilotInterfacePacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.PilotInterfacePacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_2(data.getBehaviourState());

      cdr.write_type_2(data.getRequestedBehaviorState());

      cdr.write_type_2(data.getDesiredStepType());

      cdr.write_type_2(data.getDesiredStepLengthType());

      cdr.write_type_2(data.getDesiredStepStairsType());

      cdr.write_type_7(data.getDesiredStepContinousWalk());

      cdr.write_type_2(data.getDesiredStepsToTake());

      cdr.write_type_7(data.getExecuteBehavior());

      cdr.write_type_2(data.getDesiredSlopeStepType());

      cdr.write_type_2(data.getCurrentPilotState());

   }

   public static void read(controller_msgs.msg.dds.PilotInterfacePacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setBehaviourState(cdr.read_type_2());
      	
      data.setRequestedBehaviorState(cdr.read_type_2());
      	
      data.setDesiredStepType(cdr.read_type_2());
      	
      data.setDesiredStepLengthType(cdr.read_type_2());
      	
      data.setDesiredStepStairsType(cdr.read_type_2());
      	
      data.setDesiredStepContinousWalk(cdr.read_type_7());
      	
      data.setDesiredStepsToTake(cdr.read_type_2());
      	
      data.setExecuteBehavior(cdr.read_type_7());
      	
      data.setDesiredSlopeStepType(cdr.read_type_2());
      	
      data.setCurrentPilotState(cdr.read_type_2());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.PilotInterfacePacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_2("behaviour_state", data.getBehaviourState());
      ser.write_type_2("requested_behavior_state", data.getRequestedBehaviorState());
      ser.write_type_2("desired_step_type", data.getDesiredStepType());
      ser.write_type_2("desired_step_length_type", data.getDesiredStepLengthType());
      ser.write_type_2("desired_step_stairs_type", data.getDesiredStepStairsType());
      ser.write_type_7("desired_step_continous_walk", data.getDesiredStepContinousWalk());
      ser.write_type_2("desired_steps_to_take", data.getDesiredStepsToTake());
      ser.write_type_7("execute_behavior", data.getExecuteBehavior());
      ser.write_type_2("desired_slope_step_type", data.getDesiredSlopeStepType());
      ser.write_type_2("current_pilot_state", data.getCurrentPilotState());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.PilotInterfacePacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setBehaviourState(ser.read_type_2("behaviour_state"));
      data.setRequestedBehaviorState(ser.read_type_2("requested_behavior_state"));
      data.setDesiredStepType(ser.read_type_2("desired_step_type"));
      data.setDesiredStepLengthType(ser.read_type_2("desired_step_length_type"));
      data.setDesiredStepStairsType(ser.read_type_2("desired_step_stairs_type"));
      data.setDesiredStepContinousWalk(ser.read_type_7("desired_step_continous_walk"));
      data.setDesiredStepsToTake(ser.read_type_2("desired_steps_to_take"));
      data.setExecuteBehavior(ser.read_type_7("execute_behavior"));
      data.setDesiredSlopeStepType(ser.read_type_2("desired_slope_step_type"));
      data.setCurrentPilotState(ser.read_type_2("current_pilot_state"));
   }

   public static void staticCopy(controller_msgs.msg.dds.PilotInterfacePacket src, controller_msgs.msg.dds.PilotInterfacePacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.PilotInterfacePacket createData()
   {
      return new controller_msgs.msg.dds.PilotInterfacePacket();
   }
   @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.PilotInterfacePacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.PilotInterfacePacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.PilotInterfacePacket src, controller_msgs.msg.dds.PilotInterfacePacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public PilotInterfacePacketPubSubType newInstance()
   {
      return new PilotInterfacePacketPubSubType();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WrenchTrajectoryPointMessage.java<|end_filename|>
package controller_msgs.msg.dds;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.euclid.interfaces.EpsilonComparable;
import java.util.function.Supplier;
import us.ihmc.pubsub.TopicDataType;

/**
       * This message is part of the IHMC whole-body controller API.
       * This class is used to build a wrench (force & moment) profile over time.
       * It holds the necessary information for one trajectory point.
       */
public class WrenchTrajectoryPointMessage extends Packet<WrenchTrajectoryPointMessage> implements Settable<WrenchTrajectoryPointMessage>, EpsilonComparable<WrenchTrajectoryPointMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public double time_;
   /**
            * Define the desired wrench (force & moment) to be achieved at this trajectory point.
            */
   public geometry_msgs.msg.dds.Wrench wrench_;

   public WrenchTrajectoryPointMessage()
   {
      wrench_ = new geometry_msgs.msg.dds.Wrench();
   }

   public WrenchTrajectoryPointMessage(WrenchTrajectoryPointMessage other)
   {
      this();
      set(other);
   }

   public void set(WrenchTrajectoryPointMessage other)
   {
      sequence_id_ = other.sequence_id_;

      time_ = other.time_;

      geometry_msgs.msg.dds.WrenchPubSubType.staticCopy(other.wrench_, wrench_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }

   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public void setTime(double time)
   {
      time_ = time;
   }
   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public double getTime()
   {
      return time_;
   }


   /**
            * Define the desired wrench (force & moment) to be achieved at this trajectory point.
            */
   public geometry_msgs.msg.dds.Wrench getWrench()
   {
      return wrench_;
   }


   public static Supplier<WrenchTrajectoryPointMessagePubSubType> getPubSubType()
   {
      return WrenchTrajectoryPointMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WrenchTrajectoryPointMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WrenchTrajectoryPointMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false;

      if (!this.wrench_.epsilonEquals(other.wrench_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WrenchTrajectoryPointMessage)) return false;

      WrenchTrajectoryPointMessage otherMyClass = (WrenchTrajectoryPointMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.time_ != otherMyClass.time_) return false;

      if (!this.wrench_.equals(otherMyClass.wrench_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WrenchTrajectoryPointMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("time=");
      builder.append(this.time_);      builder.append(", ");
      builder.append("wrench=");
      builder.append(this.wrench_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/PositionControlParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates;

public interface PositionControlParameters
{
   double getProportionalGain(int jointIndex);
   double getDerivativeGain(int jointIndex);
   double getIntegralGain(int jointIndex);

   double getProportionalGain(String jointName);
   double getDerivativeGain(String jointName);
   double getIntegralGain(String jointName);

   double getPositionControlMasterGain();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/hyperCubeTree/OctreeToQuadtreeListenerConverter.java<|end_filename|>
package us.ihmc.robotics.hyperCubeTree;

import us.ihmc.robotics.quadTree.Box;
import us.ihmc.robotics.quadTree.QuadTreeForGroundListener;

public class OctreeToQuadtreeListenerConverter implements HyperCubeTreeListener<Boolean, Void>
{
   private final QuadTreeForGroundListener quadListener;

   public OctreeToQuadtreeListenerConverter(QuadTreeForGroundListener quadListener)
   {
      this.quadListener = quadListener;
   }

   public void nodeAdded(String id, OneDimensionalBounds[] bounds, HyperCubeLeaf<Boolean> leaf)
   {
      Box boundryBox = new Box(bounds[0].min(), bounds[1].min(), bounds[0].max(), bounds[1].max());
      if (leaf != null)
         if (leaf.getValue())
            quadListener.nodeAdded(id, boundryBox, (float) leaf.getLocation()[0], (float) leaf.getLocation()[1], (float) leaf.getLocation()[2]);
   }

   public void nodeRemoved(String id)
   {
      quadListener.nodeRemoved(id);
   }

   public void leafAdded(HyperCubeLeaf<Boolean> leaf)
   {
      if (leaf != null)
         if (leaf.getValue())
         {
            double[] location = leaf.getLocation();
            quadListener.RawPointAdded((float) location[0], (float) location[1], (float) location[2]);
         }
   }

   public void treeCleared()
   {

   }

   public void metaDataUpdated(String id, OneDimensionalBounds[] bounds, Void data)
   {
   }

}


<|start_filename|>simulation-construction-set-tools/src/test/java/us/ihmc/simulationConstructionSetTools/robotController/TestMultiThreadedSimulation.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.robotController;

import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class TestMultiThreadedSimulation
{
   public TestMultiThreadedSimulation()
   {
      DoublePendulum doublePendulum = new DoublePendulum();
      SimulationConstructionSet scs = new SimulationConstructionSet(doublePendulum);

      DoublePendulumController controller = new DoublePendulumController(doublePendulum);
      
      MultiThreadedRobotController multiThreadedRobotController = new MultiThreadedRobotController("threadedController", doublePendulum, scs);
      multiThreadedRobotController.addController(controller, 10, false);
      multiThreadedRobotController.initialize();
      
      
      doublePendulum.setController(multiThreadedRobotController);
      
      scs.setDT(0.0001, 1);
      
      scs.startOnAThread();
   }

   public static void main(String[] args)
   {
      new TestMultiThreadedSimulation();
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/exploringSpatial/ExploringRigidBodyTools.java<|end_filename|>
package us.ihmc.manipulation.planning.exploringSpatial;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory.ConfigurationSpaceName;

public class ExploringRigidBodyTools
{
   public static RigidBodyTransform getLocalRigidBodyTransform(ConfigurationSpaceName configurationSpaceName, double... configuration)
   {
      RigidBodyTransform ret = new RigidBodyTransform();

      switch (configurationSpaceName)
      {
      case X:
         ret.appendTranslation(configuration[0], 0, 0);
         break;
      case Y:
         ret.appendTranslation(0, configuration[0], 0);
         break;
      case Z:
         ret.appendTranslation(0, 0, configuration[0]);
         break;
      case ROLL:
         ret.appendRollRotation(configuration[0]);
         break;
      case PITCH:
         ret.appendPitchRotation(configuration[0]);
         break;
      case YAW:
         ret.appendYawRotation(configuration[0]);
         break;
      case SO3:
         double theta1 = Math.PI * 2 * configuration[0];
         double theta2 = Math.acos(1 - 2 * configuration[1]) + Math.PI * 0.5;
         if (configuration[1] < 0.5)
            if (theta2 < Math.PI)
               theta2 = theta2 + Math.PI;
            else
               theta2 = theta2 - Math.PI;
         double theta3 = Math.PI * 2 * configuration[2] - Math.PI;

         ret.appendRollRotation(theta1);
         ret.appendPitchRotation(theta2);
         ret.appendYawRotation(theta3);
         break;
      }

      return ret;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/planarRegion/IntersectionEstimationParameters.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.planarRegion;

import java.util.Scanner;

public class IntersectionEstimationParameters
{
   private double maxDistanceToRegion;
   private int minRegionSize;
   private double minIntersectionLength;
   private double minRegionAngleDifference;
   private boolean addIntersectionsToRegions;

   public IntersectionEstimationParameters()
   {
      setDefaultParameters();
   }

   public IntersectionEstimationParameters(IntersectionEstimationParameters other)
   {
      set(other);
   }

   public void setDefaultParameters()
   {
      maxDistanceToRegion = 0.10;
      minRegionSize = 50;
      minIntersectionLength = 0.06;
      minRegionAngleDifference = Math.toRadians(15.0);
      addIntersectionsToRegions = true;
   }

   public void set(IntersectionEstimationParameters other)
   {
      maxDistanceToRegion = other.maxDistanceToRegion;
      minRegionSize = other.minRegionSize;
      minIntersectionLength = other.minIntersectionLength;
      minRegionAngleDifference = other.minRegionAngleDifference;
      addIntersectionsToRegions = other.addIntersectionsToRegions;
   }

   public void setMaxDistanceToRegion(double maxDistanceToRegion)
   {
      this.maxDistanceToRegion = maxDistanceToRegion;
   }

   public void setMinRegionSize(int minRegionSize)
   {
      this.minRegionSize = minRegionSize;
   }

   public void setMinIntersectionLength(double minIntersectionLength)
   {
      this.minIntersectionLength = minIntersectionLength;
   }

   public void setMinRegionAngleDifference(double minRegionAngleDifference)
   {
      this.minRegionAngleDifference = minRegionAngleDifference;
   }

   public void setAddIntersectionsToRegions(boolean addIntersectionsToRegions)
   {
      this.addIntersectionsToRegions = addIntersectionsToRegions;
   }

   public double getMaxDistanceToRegion()
   {
      return maxDistanceToRegion;
   }

   public int getMinRegionSize()
   {
      return minRegionSize;
   }

   public double getMinIntersectionLength()
   {
      return minIntersectionLength;
   }

   public double getMinRegionAngleDifference()
   {
      return minRegionAngleDifference;
   }

   public boolean isAddIntersectionsToRegions()
   {
      return addIntersectionsToRegions;
   }

   @Override
   public String toString()
   {
      return "maxDistanceToRegion: " + maxDistanceToRegion + ", minRegionSize: " + minRegionSize + ", minIntersectionLength: " + minIntersectionLength
            + ", minRegionAngleDifference: " + minRegionAngleDifference + ", addIntersectionsToRegions: " + addIntersectionsToRegions;
   }

   public static IntersectionEstimationParameters parse(String parametersAsString)
   {
      parametersAsString = parametersAsString.replace(",", "");
      Scanner scanner = new Scanner(parametersAsString);
      while (!scanner.hasNextDouble())
         scanner.next();
      double maxDistanceToRegion = scanner.nextDouble();
      while (!scanner.hasNextInt())
         scanner.next();
      int minRegionSize = scanner.nextInt();
      while (!scanner.hasNextDouble())
         scanner.next();
      double minIntersectionLength = scanner.nextDouble();
      while (!scanner.hasNextDouble())
         scanner.next();
      double minRegionAngleDifference = scanner.nextDouble();
      while (!scanner.hasNextBoolean())
         scanner.next();
      boolean addIntersectionsToRegions = scanner.nextBoolean();
      scanner.close();

      IntersectionEstimationParameters parameters = new IntersectionEstimationParameters();
      parameters.setMaxDistanceToRegion(maxDistanceToRegion);
      parameters.setMinRegionSize(minRegionSize);
      parameters.setMinIntersectionLength(minIntersectionLength);
      parameters.setMinRegionAngleDifference(minRegionAngleDifference);
      parameters.setAddIntersectionsToRegions(addIntersectionsToRegions);
      return parameters;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/OcTreeKeyMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import java.util.Arrays;

import us.ihmc.communication.packets.Packet;
import us.ihmc.jOctoMap.key.OcTreeKey;
import us.ihmc.jOctoMap.key.OcTreeKeyReadOnly;

public class OcTreeKeyMessage extends Packet<OcTreeKeyMessage>
{
   public int[] k;

   public OcTreeKeyMessage()
   {
   }

   @Override
   public void set(OcTreeKeyMessage other)
   {
      set(other.getKey(0), other.getKey(1), other.getKey(2));
      setPacketInformation(other);
   }

   public void set(OcTreeKeyReadOnly other)
   {
      set(other.getKey(0), other.getKey(1), other.getKey(2));
   }

   public void set(int k0, int k1, int k2)
   {
      if (k == null)
         k = new int[3];
      k[0] = k0;
      k[1] = k1;
      k[2] = k2;
   }

   public int getKey(int index)
   {
      return k[index];
   }

   @Override
   public boolean equals(Object obj)
   {
      if (obj instanceof OcTreeKey)
         return equals((OcTreeKey) obj);
      if (obj instanceof OcTreeKeyMessage)
         return equals((OcTreeKeyMessage) obj);
      return false;
   }

   public boolean equals(OcTreeKeyMessage other)
   {
      return Arrays.equals(k, other.k);
   }

   public boolean equals(OcTreeKey other)
   {
      return k[0] == other.getKey(0) && k[1] == other.getKey(1) && k[2] == other.getKey(2);
   }

   @Override
   public int hashCode()
   {
      return (int) (k[0] + 1447L * k[1] + 345637L * k[2]);
   }

   @Override
   public boolean epsilonEquals(OcTreeKeyMessage other, double epsilon)
   {
      return equals(other);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SE3TrajectoryMessage.java<|end_filename|>
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 carries the information to execute a trajectory in taskspace (position and orientation) by defining trajectory points.
       * A third order polynomial function is used to interpolate positions and a Hermite based curve (third order) is used to interpolate the orientations.
       * To execute a single straight line trajectory to reach a desired pose, set only one trajectory point with zero velocity and its time to be equal to the desired trajectory time.
       */
public class SE3TrajectoryMessage extends Packet<SE3TrajectoryMessage> implements Settable<SE3TrajectoryMessage>, EpsilonComparable<SE3TrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * List of trajectory points (in taskpsace) to go through while executing the trajectory.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.SE3TrajectoryPointMessage>  taskspace_trajectory_points_;
   /**
            * The selection matrix for each axis of the angular part of this trajectory.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage angular_selection_matrix_;
   /**
            * The selection matrix for each axis of the linear part of this trajectory.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage linear_selection_matrix_;
   /**
            * Frame information for this message.
            */
   public controller_msgs.msg.dds.FrameInformation frame_information_;
   /**
            * The weight matrix for each axis of the angular part of this trajectory.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage angular_weight_matrix_;
   /**
            * The weight matrix for each axis of the linear part of this trajectory.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage linear_weight_matrix_;
   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public boolean use_custom_control_frame_;
   /**
            * Pose of custom control frame expressed in the end-effector frame.
            * This is the frame attached to the rigid body that the taskspace trajectory is defined for.
            */
   public us.ihmc.euclid.geometry.Pose3D control_frame_pose_;
   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage queueing_properties_;

   public SE3TrajectoryMessage()
   {
      taskspace_trajectory_points_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.SE3TrajectoryPointMessage> (50, new controller_msgs.msg.dds.SE3TrajectoryPointMessagePubSubType());
      angular_selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      linear_selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      frame_information_ = new controller_msgs.msg.dds.FrameInformation();
      angular_weight_matrix_ = new controller_msgs.msg.dds.WeightMatrix3DMessage();
      linear_weight_matrix_ = new controller_msgs.msg.dds.WeightMatrix3DMessage();
      control_frame_pose_ = new us.ihmc.euclid.geometry.Pose3D();
      queueing_properties_ = new controller_msgs.msg.dds.QueueableMessage();

   }

   public SE3TrajectoryMessage(SE3TrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(SE3TrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      taskspace_trajectory_points_.set(other.taskspace_trajectory_points_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.angular_selection_matrix_, angular_selection_matrix_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.linear_selection_matrix_, linear_selection_matrix_);
      controller_msgs.msg.dds.FrameInformationPubSubType.staticCopy(other.frame_information_, frame_information_);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.staticCopy(other.angular_weight_matrix_, angular_weight_matrix_);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.staticCopy(other.linear_weight_matrix_, linear_weight_matrix_);
      use_custom_control_frame_ = other.use_custom_control_frame_;

      geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.control_frame_pose_, control_frame_pose_);
      controller_msgs.msg.dds.QueueableMessagePubSubType.staticCopy(other.queueing_properties_, queueing_properties_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * List of trajectory points (in taskpsace) to go through while executing the trajectory.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.SE3TrajectoryPointMessage>  getTaskspaceTrajectoryPoints()
   {
      return taskspace_trajectory_points_;
   }


   /**
            * The selection matrix for each axis of the angular part of this trajectory.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage getAngularSelectionMatrix()
   {
      return angular_selection_matrix_;
   }


   /**
            * The selection matrix for each axis of the linear part of this trajectory.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage getLinearSelectionMatrix()
   {
      return linear_selection_matrix_;
   }


   /**
            * Frame information for this message.
            */
   public controller_msgs.msg.dds.FrameInformation getFrameInformation()
   {
      return frame_information_;
   }


   /**
            * The weight matrix for each axis of the angular part of this trajectory.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage getAngularWeightMatrix()
   {
      return angular_weight_matrix_;
   }


   /**
            * The weight matrix for each axis of the linear part of this trajectory.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage getLinearWeightMatrix()
   {
      return linear_weight_matrix_;
   }

   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public void setUseCustomControlFrame(boolean use_custom_control_frame)
   {
      use_custom_control_frame_ = use_custom_control_frame;
   }
   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public boolean getUseCustomControlFrame()
   {
      return use_custom_control_frame_;
   }


   /**
            * Pose of custom control frame expressed in the end-effector frame.
            * This is the frame attached to the rigid body that the taskspace trajectory is defined for.
            */
   public us.ihmc.euclid.geometry.Pose3D getControlFramePose()
   {
      return control_frame_pose_;
   }


   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage getQueueingProperties()
   {
      return queueing_properties_;
   }


   public static Supplier<SE3TrajectoryMessagePubSubType> getPubSubType()
   {
      return SE3TrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SE3TrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SE3TrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.taskspace_trajectory_points_.size() != other.taskspace_trajectory_points_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.taskspace_trajectory_points_.size(); i++)
         {  if (!this.taskspace_trajectory_points_.get(i).epsilonEquals(other.taskspace_trajectory_points_.get(i), epsilon)) return false; }
      }

      if (!this.angular_selection_matrix_.epsilonEquals(other.angular_selection_matrix_, epsilon)) return false;
      if (!this.linear_selection_matrix_.epsilonEquals(other.linear_selection_matrix_, epsilon)) return false;
      if (!this.frame_information_.epsilonEquals(other.frame_information_, epsilon)) return false;
      if (!this.angular_weight_matrix_.epsilonEquals(other.angular_weight_matrix_, epsilon)) return false;
      if (!this.linear_weight_matrix_.epsilonEquals(other.linear_weight_matrix_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_custom_control_frame_, other.use_custom_control_frame_, epsilon)) return false;

      if (!this.control_frame_pose_.epsilonEquals(other.control_frame_pose_, epsilon)) return false;
      if (!this.queueing_properties_.epsilonEquals(other.queueing_properties_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SE3TrajectoryMessage)) return false;

      SE3TrajectoryMessage otherMyClass = (SE3TrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.taskspace_trajectory_points_.equals(otherMyClass.taskspace_trajectory_points_)) return false;
      if (!this.angular_selection_matrix_.equals(otherMyClass.angular_selection_matrix_)) return false;
      if (!this.linear_selection_matrix_.equals(otherMyClass.linear_selection_matrix_)) return false;
      if (!this.frame_information_.equals(otherMyClass.frame_information_)) return false;
      if (!this.angular_weight_matrix_.equals(otherMyClass.angular_weight_matrix_)) return false;
      if (!this.linear_weight_matrix_.equals(otherMyClass.linear_weight_matrix_)) return false;
      if(this.use_custom_control_frame_ != otherMyClass.use_custom_control_frame_) return false;

      if (!this.control_frame_pose_.equals(otherMyClass.control_frame_pose_)) return false;
      if (!this.queueing_properties_.equals(otherMyClass.queueing_properties_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SE3TrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("taskspace_trajectory_points=");
      builder.append(this.taskspace_trajectory_points_);      builder.append(", ");
      builder.append("angular_selection_matrix=");
      builder.append(this.angular_selection_matrix_);      builder.append(", ");
      builder.append("linear_selection_matrix=");
      builder.append(this.linear_selection_matrix_);      builder.append(", ");
      builder.append("frame_information=");
      builder.append(this.frame_information_);      builder.append(", ");
      builder.append("angular_weight_matrix=");
      builder.append(this.angular_weight_matrix_);      builder.append(", ");
      builder.append("linear_weight_matrix=");
      builder.append(this.linear_weight_matrix_);      builder.append(", ");
      builder.append("use_custom_control_frame=");
      builder.append(this.use_custom_control_frame_);      builder.append(", ");
      builder.append("control_frame_pose=");
      builder.append(this.control_frame_pose_);      builder.append(", ");
      builder.append("queueing_properties=");
      builder.append(this.queueing_properties_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/CylindricalCoordinatesCalculator.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.FrameQuaternion;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.commons.MathTools;
import us.ihmc.robotics.robotSide.RobotSide;

/**
 * @author twan
 *         Date: 5/21/13
 */
public class CylindricalCoordinatesCalculator
{

   private FramePoint3D position = new FramePoint3D();
   private final RotationMatrix preRotation = new RotationMatrix();
   private final RotationMatrix rotation = new RotationMatrix();
   private final FrameQuaternion orientation = new FrameQuaternion(ReferenceFrame.getWorldFrame());

   public FramePose3D getPoseFromCylindricalCoordinates(RobotSide robotSide, ReferenceFrame frame, double radiansFromYAxis, double radius, double z,
                                                              double outwardRotation, double pitchRotation)
   {
      getPosition(position, frame, radiansFromYAxis, radius, z);


      preRotation.setYawPitchRoll(0.0, Math.PI / 2.0, -Math.PI / 2.0);

      rotation.set(preRotation);
      rotation.appendRollRotation(robotSide.negateIfRightSide(Math.PI / 2.0) - radiansFromYAxis);
      rotation.appendYawRotation(robotSide.negateIfRightSide(outwardRotation));
      rotation.appendPitchRotation(pitchRotation);

      orientation.setIncludingFrame(frame, rotation);

      return new FramePose3D(position, orientation);
   }

   public static void getPosition(FramePoint3D pointToPack, ReferenceFrame frame, double angle, double radius, double z)
   {
      double x = radius * Math.cos(angle);
      double y = radius * Math.sin(angle);

      pointToPack.setIncludingFrame(frame, x, y, z);
   }

   public static void getVelocity(FrameVector3D velocityToPack, ReferenceFrame frame, double angle, double angleDot, double radius, double radiusDot, double zDot)
   {
      double cos = Math.cos(angle);
      double sin = Math.sin(angle);

      velocityToPack.setToZero(frame);

      double xDot = -radius * sin * angleDot + cos * radiusDot;
      double yDot =  radius * cos * angleDot + sin * radiusDot;

      velocityToPack.setX(xDot);
      velocityToPack.setY(yDot);
      velocityToPack.setZ(zDot);
   }

   public static void getAcceleration(FrameVector3D accelerationToPack, ReferenceFrame frame, double angle, double angleDot, double angleDDot, double radius, double radiusDot, double radiusDDot, double zDDot)
   {
      double cos = Math.cos(angle);
      double sin = Math.sin(angle);

      accelerationToPack.setToZero(frame);

      double xDDot = -cos * radius * MathTools.square(angleDot) - 2.0 * sin * angleDot * radiusDot - radius * sin * angleDDot + cos * radiusDDot;
      double yDDot = -sin * radius * MathTools.square(angleDot) + 2.0 * cos * angleDot * radiusDot + radius * cos * angleDDot + sin * radiusDDot;

      accelerationToPack.setX(xDDot);
      accelerationToPack.setY(yDDot);
      accelerationToPack.setZ(zDDot);
   }

   public static double getRadius(FramePoint3D position)
   {
      double x = position.getX();
      double y = position.getY();

      double radius = Math.hypot(x, y);
      return radius;
   }

   public static double getAngle(FramePoint3D position)
   {
      double x = position.getX();
      double y = position.getY();

      double angle = Math.atan2(y, x);
      return angle;
   }

   public static double getRadialVelocity(FramePoint3D position, FrameVector3D velocity)
   {
      position.checkReferenceFrameMatch(velocity);

      double x = position.getX();
      double y = position.getY();

      double xd = velocity.getX();
      double yd = velocity.getY();

      double radius = getRadius(position);
      return (x * xd + y * yd) / radius;
   }

   public static double getAngularVelocity(FramePoint3D position, FrameVector3D velocity)
   {
      position.checkReferenceFrameMatch(velocity);

      double x = position.getX();
      double y = position.getY();

      double xd = velocity.getX();
      double yd = velocity.getY();

      double radiusSquared = MathTools.square(x) + MathTools.square(y);
      return (x * yd - y * xd) / radiusSquared;
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/DoubleSpinner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import java.util.ArrayList;
import java.util.List;
import java.util.Optional;

import org.apache.commons.lang3.tuple.ImmutablePair;

import javafx.scene.control.MenuItem;
import javafx.scene.control.SeparatorMenuItem;
import javafx.scene.control.TextInputDialog;

public class DoubleSpinner extends NumericSpinner<Double>
{
   public DoubleSpinner()
   {
      super(new DoubleSpinnerValueFactory(0.1));
   }

   @Override
   public Double convertStringToNumber(String numberString)
   {
      if (numberString == null)
      {
         return 0.0;
      }
      if (numberString.endsWith("e") || numberString.endsWith("E"))
      {
         return Double.parseDouble(numberString.substring(0, numberString.length() - 1));
      }
      else if (numberString.endsWith("e-") || numberString.endsWith("E-"))
      {
         return Double.parseDouble(numberString.substring(0, numberString.length() - 2));
      }
      return Double.parseDouble(numberString);
   }

   @Override
   public String convertNumberToString(Double number)
   {
      return Double.toString(number);
   }

   @Override
   public List<MenuItem> getContextMenuOptions()
   {
      List<MenuItem> items = new ArrayList<>();
      for (ImmutablePair<String, String> option : getSpecialStringOptions())
      {
         MenuItem menuItem = new MenuItem(option.getLeft());
         Double number = getValueFactory().getConverter().fromString(option.getRight());
         menuItem.setOnAction(actionEvent -> setValue(number));
         items.add(menuItem);
      }
      items.add(new SeparatorMenuItem());
      items.add(getAngleConversionOption());
      return items;
   }

   private List<ImmutablePair<String, String>> getSpecialStringOptions()
   {
      List<ImmutablePair<String, String>> ret = new ArrayList<>();
      ret.add(new ImmutablePair<String, String>("Infinity", convertNumberToString(Double.POSITIVE_INFINITY)));
      ret.add(new ImmutablePair<String, String>("Negative Infinity", convertNumberToString(Double.NEGATIVE_INFINITY)));
      return ret;
   }

   private MenuItem getAngleConversionOption()
   {
      MenuItem menuItem = new MenuItem("Input Angle");

      menuItem.setOnAction(actionEvent -> {
         TextInputDialog dialog = new TextInputDialog();
         dialog.setTitle("Convert Angle");
         dialog.setHeaderText("Convert Degree to Radian");
         dialog.setContentText("Enter Angle in Degrees:");

         Optional<String> result = dialog.showAndWait();
         if (result.isPresent() && isValidString(result.get()))
         {
            Double number = getValueFactory().getConverter().fromString(result.get());
            setValue(Math.toRadians(number));
         }
      });

      return menuItem;
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/scripts/engine/ScriptTools.java<|end_filename|>
package us.ihmc.humanoidBehaviors.behaviors.scripts.engine;

import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;

import com.thoughtworks.xstream.io.StreamException;

import controller_msgs.msg.dds.FootTrajectoryMessage;
import controller_msgs.msg.dds.FootstepDataListMessage;
import controller_msgs.msg.dds.PauseWalkingMessage;
import us.ihmc.commons.PrintTools;
import us.ihmc.humanoidRobotics.communication.packets.walking.EndOfScriptCommand;

/**
 * Created with IntelliJ IDEA.
 * User: pneuhaus
 * Date: 5/31/13
 * Time: 3:07 PM
 * To change this template use File | Settings | File Templates.
 */
public class ScriptTools
{

   public static void removeAllUnnecessaryScripObjects(ArrayList<ScriptObject> listToEdit)
   {
      removeAllEndOfScripts(listToEdit);
      removeAllPauses(listToEdit);
      removeAllZeroStepLists(listToEdit);
      //removeAllFootPosePackets(listToEdit);
   }

   private static void removePausesAndZeroStepLists(ArrayList<ScriptObject> listToEdit)
   {
      int beforeSize, afterSize;
      do
      {
         beforeSize = listToEdit.size();
         removePausesAndZeroStepListsInnnerOpeartion(listToEdit);
         afterSize = listToEdit.size();
      }
      while (afterSize < beforeSize);
   }

   private static void removeAllEndOfScripts(ArrayList<ScriptObject> listToEdit)
   {
      for (int i = listToEdit.size() - 1; i >= 0; i--)
      {
         if (listToEdit.get(i).getScriptObject() instanceof EndOfScriptCommand)
            listToEdit.remove(i);
      }
   }

   private static void removeAllFootPosePackets(ArrayList<ScriptObject> listToEdit)
   {
      for (int i = listToEdit.size() - 1; i >= 0; i--)
      {
         if (listToEdit.get(i).getScriptObject() instanceof FootTrajectoryMessage)
            listToEdit.remove(i);
      }
   }


   private static void removeAllPauses(ArrayList<ScriptObject> listToEdit)
   {
      for (int i = listToEdit.size() - 1; i >= 0; i--)
      {
         if (listToEdit.get(i).getScriptObject() instanceof PauseWalkingMessage)
            listToEdit.remove(i);
      }
   }

   private static void removeAllZeroStepLists(ArrayList<ScriptObject> listToEdit)
   {
      for (int i = listToEdit.size() - 1; i >= 0; i--)
      {
         if (listToEdit.get(i).getScriptObject() instanceof FootstepDataListMessage)
         {
            if (((FootstepDataListMessage) listToEdit.get(i).getScriptObject()).getFootstepDataList().size() == 0)
               listToEdit.remove(i);
         }
      }
   }

   private static void removePausesAndZeroStepListsInnnerOpeartion(ArrayList<ScriptObject> listToEdit)
   {
      boolean previousWasPause = false;
      ArrayList<Integer> elementsToRemove = new ArrayList<Integer>();

      for (int i = 0; i < listToEdit.size(); i++)
      {
         ScriptObject scriptObject = listToEdit.get(i);

         if (scriptObject.getScriptObject() instanceof PauseWalkingMessage)
         {
            if (((PauseWalkingMessage) scriptObject.getScriptObject()).getPause())
            {
               if (previousWasPause)
               {
                  elementsToRemove.add(i);
               }
               else
               {
                  previousWasPause = true;
               }
            }
            else
            {
               previousWasPause = false;
            }

         }
         else
            previousWasPause = false;

         if (scriptObject.getScriptObject() instanceof FootstepDataListMessage)
         {
            if (((FootstepDataListMessage) scriptObject.getScriptObject()).getFootstepDataList().size() == 0)
               elementsToRemove.add(i);
         }
      }

      Collections.reverse(elementsToRemove);

      for (Integer integer : elementsToRemove)
      {
         listToEdit.remove(integer.intValue());
      }
   }

   public static void main(String[] args) //throws IOException
   {
      File file = ScriptFileSelector.getScriptFileFromUserSelection(ScriptEngineSettings.extension);

      System.out.println("Loading file: " + file.getAbsolutePath());

      ScriptFileLoader loader = null;
      try
      {
         loader = new ScriptFileLoader(file);
      }
      catch (StreamException | IOException e)
      {
         PrintTools.error(e.getMessage());
         System.exit(-1);
      }
      
      ArrayList<ScriptObject> scriptObjects = loader.readIntoList();
      loader.close();

      System.out.println("Before:");
      System.out.println(ScriptObject.getListInfo(scriptObjects));

      ScriptTools.removeAllUnnecessaryScripObjects(scriptObjects);

      //ScriptTools.removePausesAndZeroStepLists(scriptObjects);
      //ScriptTools.removeAllEndOfScripts(scriptObjects);

      System.out.println("\nAfter:");
      System.out.println(ScriptObject.getListInfo(scriptObjects));

      System.exit(-1);
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/DefaultFootContactPoints.java<|end_filename|>
package us.ihmc.wholeBodyController;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.robotics.partNames.LeggedJointNameMap;
import us.ihmc.robotics.robotSide.RobotSegment;
import us.ihmc.robotics.robotSide.SegmentDependentList;

public class DefaultFootContactPoints<E extends Enum<E> & RobotSegment<E>> implements FootContactPoints<E>
{
   private final E[] robotSegments;
   public DefaultFootContactPoints(E[] robotSegments)
   {
      this.robotSegments = robotSegments;
   }

   @Override
   public Map<String, List<Tuple3DBasics>> getSimulationContactPoints(double footLength, double footWidth, double toeWidth, LeggedJointNameMap<E> jointMap,
         SegmentDependentList<E, RigidBodyTransform> soleToAnkleFrameTransforms)
   {
      HashMap<String, List<Tuple3DBasics>> ret = new HashMap<>();

      for (E segment : robotSegments)
      {
         ArrayList<Tuple3DBasics> footContactPoints = new ArrayList<>();
         String parentJointName = jointMap.getJointBeforeFootName(segment);

         //SCS Sim contactPoints
         int nContactPointsX = 2;
         int nContactPointsY = 2;

         double dx = 1.01 * footLength / (nContactPointsX - 1.0);
         double xOffset = 1.01 * footLength / 2.0;

         for (int ix = 1; ix <= nContactPointsX; ix++)
         {
            double alpha = (ix - 1.0) / (nContactPointsX - 1.0);
            double footWidthAtCurrentX = (1.0 - alpha) * 1.01 * footWidth + alpha * 1.01 * toeWidth;
            double dy = footWidthAtCurrentX / (nContactPointsY - 1.0);
            double yOffset = footWidthAtCurrentX / 2.0;

            for (int iy = 1; iy <= nContactPointsY; iy++)
            {
               double x = (ix - 1.0) * dx - xOffset;
               double y = (iy - 1.0) * dy - yOffset;
               RigidBodyTransform transformToParentJointFrame = soleToAnkleFrameTransforms.get(segment);

               Point3D contactPoint = new Point3D(x, y, 0.0);
               transformToParentJointFrame.transform(contactPoint);
               footContactPoints.add(contactPoint);
            }
         }

         ret.put(parentJointName, footContactPoints);
      }

      return ret;
   }

   @Override
   public SegmentDependentList<E, List<Tuple2DBasics>> getControllerContactPoints(double footLength, double footWidth, double toeWidth)
   {
      SegmentDependentList<E, List<Tuple2DBasics>> ret = new SegmentDependentList<>(robotSegments[0].getClassType());

      for (E segment : robotSegments)
      {
         ArrayList<Tuple2DBasics> contactPoints = new ArrayList<>();
         contactPoints.add(new Point2D(-footLength / 2.0, -footWidth / 2.0));
         contactPoints.add(new Point2D(-footLength / 2.0, footWidth / 2.0));
         contactPoints.add(new Point2D(footLength / 2.0, -toeWidth / 2.0));
         contactPoints.add(new Point2D(footLength / 2.0, toeWidth / 2.0));
         ret.put(segment, contactPoints);
      }

      return ret;
   }

   @Override
   public SegmentDependentList<E, Tuple2DBasics> getToeOffContactPoints(double footLength, double footWidth, double toeWidth)
   {
      SegmentDependentList<E, Tuple2DBasics> ret = new SegmentDependentList<>(robotSegments[0].getClassType());

      for (E segment : robotSegments)
         ret.put(segment, new Point2D(footLength / 2.0, 0.0));

      return ret;
   }

   @Override
   public SegmentDependentList<E, LineSegment2D> getToeOffContactLines(double footLength, double footWidth, double toeWidth)
   {
      SegmentDependentList<E, LineSegment2D> ret = new SegmentDependentList<>(robotSegments[0].getClassType());

      double footForward = footLength / 2.0;
      double halfToeWidth = toeWidth / 2.0;

      for (E segment : robotSegments)
         ret.put(segment, new LineSegment2D(new Point2D(footForward, -halfToeWidth), new Point2D(footForward, halfToeWidth)));

      return ret;
   }

   @Override
   public boolean useSoftContactPointParameters()
   {
      return false;
   }

}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/stickRobot/StickRobotOrderedJointMap.java<|end_filename|>
package us.ihmc.exampleSimulations.stickRobot;

import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class StickRobotOrderedJointMap
{
   public final static int LeftHipYaw= 0;
   public final static int LeftHipRoll= 1;
   public final static int LeftHipPitch= 2;
   public final static int LeftKneePitch= 3;
   public final static int LeftAnklePitch= 4;
   public final static int LeftAnkleRoll= 5;
   public final static int RightHipYaw= 6;
   public final static int RightHipRoll= 7;
   public final static int RightHipPitch= 8;
   public final static int RightKneePitch= 9;
   public final static int RightAnklePitch= 10;
   public final static int RightAnkleRoll= 11;
   public final static int TorsoYaw= 12;
   public final static int TorsoPitch= 13;
   public final static int TorsoRoll= 14;
   public final static int LeftShoulderPitch= 15;
   public final static int LeftShoulderRoll= 16;
   public final static int LeftShoulderYaw= 17;
   public final static int LeftElbowPitch= 18;
   public final static int LeftForearmYaw= 19;
   public final static int LeftWristRoll= 20;
   public final static int LeftWristPitch= 21;
   public final static int NeckYaw= 22;
   public final static int NeckRoll= 23;
   public final static int NeckPitch= 24;
   public final static int RightShoulderPitch= 25;
   public final static int RightShoulderRoll= 26;
   public final static int RightShoulderYaw= 27;
   public final static int RightElbowPitch= 28;
   public final static int RightForearmYaw= 29;
   public final static int RightWristRoll= 30;
   public final static int RightWristPitch= 31;
   
   // since the count begins from 0 and RightWristPitch being the last joint in the chain
   public final static int numberOfJoints = RightWristPitch + 1;

   public static String[]  jointNames = new String[numberOfJoints];
   static
   {
      jointNames[LeftHipYaw] = "leftHipYaw";
      jointNames[LeftHipRoll] = "leftHipRoll";
      jointNames[LeftHipPitch] = "leftHipPitch";
      jointNames[LeftKneePitch] = "leftKneePitch";
      jointNames[LeftAnklePitch] = "leftAnklePitch";
      jointNames[LeftAnkleRoll] = "leftAnkleRoll";
      jointNames[RightHipYaw] = "rightHipYaw";
      jointNames[RightHipRoll] = "rightHipRoll";
      jointNames[RightHipPitch] = "rightHipPitch";
      jointNames[RightKneePitch] = "rightKneePitch";
      jointNames[RightAnklePitch] = "rightAnklePitch";
      jointNames[RightAnkleRoll] = "rightAnkleRoll";
      jointNames[TorsoYaw] = "torsoYaw";
      jointNames[TorsoPitch] = "torsoPitch";
      jointNames[TorsoRoll] = "torsoRoll";
      jointNames[LeftShoulderPitch] = "leftShoulderPitch";
      jointNames[LeftShoulderRoll] = "leftShoulderRoll";
      jointNames[LeftShoulderYaw] = "leftShoulderYaw";
      jointNames[LeftElbowPitch] = "leftElbowPitch";
      jointNames[LeftForearmYaw] = "leftForearmYaw";
      jointNames[LeftWristRoll] = "leftWristRoll";
      jointNames[LeftWristPitch] = "leftWristPitch";
      jointNames[NeckYaw] = "neckYaw";
      jointNames[NeckRoll] = "neckRoll";
      jointNames[NeckPitch] = "neckPitch";
      jointNames[RightShoulderPitch] = "rightShoulderPitch";
      jointNames[RightShoulderRoll] = "rightShoulderRoll";
      jointNames[RightShoulderYaw] = "rightShoulderYaw";
      jointNames[RightElbowPitch] = "rightElbowPitch";
      jointNames[RightForearmYaw] = "rightForearmYaw";
      jointNames[RightWristRoll] = "rightWristRoll";
      jointNames[RightWristPitch] = "rightWristPitch";
   }

   public static final SideDependentList<String[]> forcedSideDependentJointNames = new SideDependentList<String[]>();
   static
   {
      String[] jointNamesRight = new String[numberOfJoints];
      jointNamesRight[LeftHipYaw] = "rightHipYaw";
      jointNamesRight[LeftHipRoll] = "rightHipRoll";
      jointNamesRight[LeftHipPitch] = "rightHipPitch";
      jointNamesRight[LeftKneePitch] = "rightKneePitch";
      jointNamesRight[LeftAnklePitch] = "rightAnklePitch";
      jointNamesRight[LeftAnkleRoll] = "rightAnkleRoll";
      jointNamesRight[RightHipYaw] = "rightHipYaw";
      jointNamesRight[RightHipRoll] = "rightHipRoll";
      jointNamesRight[RightHipPitch] = "rightHipPitch";
      jointNamesRight[RightKneePitch] = "rightKneePitch";
      jointNamesRight[RightAnklePitch] = "rightAnklePitch";
      jointNamesRight[RightAnkleRoll] = "rightAnkleRoll";
      jointNamesRight[TorsoYaw] = "torsoYaw";
      jointNamesRight[TorsoPitch] = "torsoPitch";
      jointNamesRight[TorsoRoll] = "torsoRoll";
      jointNamesRight[LeftShoulderPitch] = "rightShoulderPitch";
      jointNamesRight[LeftShoulderRoll] = "rightShoulderRoll";
      jointNamesRight[LeftShoulderYaw] = "rightShoulderYaw";
      jointNamesRight[LeftElbowPitch] = "rightElbowPitch";
      jointNamesRight[LeftForearmYaw] = "rightForearmYaw";
      jointNamesRight[LeftWristRoll] = "rightWristRoll";
      jointNamesRight[LeftWristPitch] = "rightWristPitch";
      jointNamesRight[NeckYaw] = "neckYaw";
      jointNamesRight[NeckRoll] = "neckRoll";
      jointNamesRight[NeckPitch] = "neckPitch";
      jointNamesRight[RightShoulderPitch] = "rightShoulderPitch";
      jointNamesRight[RightShoulderRoll] = "rightShoulderRoll";
      jointNamesRight[RightShoulderYaw] = "rightShoulderYaw";
      jointNamesRight[RightElbowPitch] = "rightElbowPitch";
      jointNamesRight[RightForearmYaw] = "rightForearmYaw";
      jointNamesRight[RightWristRoll] = "rightWristRoll";
      jointNamesRight[RightWristPitch] = "rightWristPitch";
      
      forcedSideDependentJointNames.put(RobotSide.RIGHT, jointNamesRight);

      String[] jointNamesLeft = new String[numberOfJoints];
      jointNamesLeft[LeftHipYaw] = "leftHipYaw";
      jointNamesLeft[LeftHipRoll] = "leftHipRoll";
      jointNamesLeft[LeftHipPitch] = "leftHipPitch";
      jointNamesLeft[LeftKneePitch] = "leftKneePitch";
      jointNamesLeft[LeftAnklePitch] = "leftAnklePitch";
      jointNamesLeft[LeftAnkleRoll] = "leftAnkleRoll";
      jointNamesLeft[RightHipYaw] = "leftHipYaw";
      jointNamesLeft[RightHipRoll] = "leftHipRoll";
      jointNamesLeft[RightHipPitch] = "leftHipPitch";
      jointNamesLeft[RightKneePitch] = "leftKneePitch";
      jointNamesLeft[RightAnklePitch] = "leftAnklePitch";
      jointNamesLeft[RightAnkleRoll] = "leftAnkleRoll";
      jointNamesLeft[TorsoYaw] = "torsoYaw";
      jointNamesLeft[TorsoPitch] = "torsoPitch";
      jointNamesLeft[TorsoRoll] = "torsoRoll";
      jointNamesLeft[LeftShoulderPitch] = "leftShoulderPitch";
      jointNamesLeft[LeftShoulderRoll] = "leftShoulderRoll";
      jointNamesLeft[LeftShoulderYaw] = "leftShoulderYaw";
      jointNamesLeft[LeftElbowPitch] = "leftElbowPitch";
      jointNamesLeft[LeftForearmYaw] = "leftForearmYaw";
      jointNamesLeft[LeftWristRoll] = "leftWristRoll";
      jointNamesLeft[LeftWristPitch] = "leftWristPitch";
      jointNamesLeft[NeckYaw] = "neckYaw";
      jointNamesLeft[NeckRoll] = "neckRoll";
      jointNamesLeft[NeckPitch] = "neckPitch";
      jointNamesLeft[RightShoulderPitch] = "leftShoulderPitch";
      jointNamesLeft[RightShoulderRoll] = "leftShoulderRoll";
      jointNamesLeft[RightShoulderYaw] = "leftShoulderYaw";
      jointNamesLeft[RightElbowPitch] = "leftElbowPitch";
      jointNamesLeft[RightForearmYaw] = "leftForearmYaw";
      jointNamesLeft[RightWristRoll] = "leftWristRoll";
      jointNamesLeft[RightWristPitch] = "leftWristPitch";
      
      forcedSideDependentJointNames.put(RobotSide.LEFT, jointNamesLeft);
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/InverseDynamicsJointStateCopier.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public class InverseDynamicsJointStateCopier extends AbstractInverseDynamicsCopier
{

   public InverseDynamicsJointStateCopier(RigidBodyBasics originalBody, RigidBodyBasics targetBody)
   {
      super(originalBody, targetBody);
   }

   @Override
   protected void copyJoint(JointBasics originalJoint, JointBasics targetJoint)
   {
      targetJoint.setJointConfiguration(originalJoint);
      targetJoint.setJointTwist(originalJoint);
      targetJoint.setJointAcceleration(originalJoint);
   }  

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/Ray3d.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;

public class Ray3d 
{
   private final Point3D point;
   private final Vector3D vector;
   
   public static Ray3d transformRay3d(Ray3d ray3d, RigidBodyTransform transformToHere)
   {
      Point3D transformedPoint = new Point3D();
      Vector3D transformedVector = new Vector3D();
      transformToHere.transform(ray3d.getPoint(), transformedPoint);
      transformToHere.transform(ray3d.getVector(), transformedVector);

      return new Ray3d(transformedPoint, transformedVector);
   }

   public Ray3d(Point3DReadOnly point, Vector3DReadOnly vector)
   {
      this.point = new Point3D(point);
      this.vector = new Vector3D(vector);
   }

   public Point3D getPoint()
   {
      return point;
   }

   public Vector3D getVector()
   {
      return vector;
   }

   public void transformInPlace(RigidBodyTransform transformToHere)
   {
      this.set(transformRay3d(this, transformToHere));
   }

   public void set(Ray3d ray3d)
   {
      this.getPoint().set(ray3d.getPoint());
      this.getVector().set(ray3d.getVector());
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PointCloudWorldPacket.java<|end_filename|>
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 obsolete
       */
public class PointCloudWorldPacket extends Packet<PointCloudWorldPacket> implements Settable<PointCloudWorldPacket>, EpsilonComparable<PointCloudWorldPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public long timestamp_;
   public us.ihmc.idl.IDLSequence.Float  ground_quad_tree_support_;
   public us.ihmc.idl.IDLSequence.Float  decaying_world_scan_;
   public float default_ground_height_;

   public PointCloudWorldPacket()
   {
      ground_quad_tree_support_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5");

      decaying_world_scan_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5");

   }

   public PointCloudWorldPacket(PointCloudWorldPacket other)
   {
      this();
      set(other);
   }

   public void set(PointCloudWorldPacket other)
   {
      sequence_id_ = other.sequence_id_;

      timestamp_ = other.timestamp_;

      ground_quad_tree_support_.set(other.ground_quad_tree_support_);
      decaying_world_scan_.set(other.decaying_world_scan_);
      default_ground_height_ = other.default_ground_height_;

   }

   /**
            * 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 void setTimestamp(long timestamp)
   {
      timestamp_ = timestamp;
   }
   public long getTimestamp()
   {
      return timestamp_;
   }


   public us.ihmc.idl.IDLSequence.Float  getGroundQuadTreeSupport()
   {
      return ground_quad_tree_support_;
   }


   public us.ihmc.idl.IDLSequence.Float  getDecayingWorldScan()
   {
      return decaying_world_scan_;
   }

   public void setDefaultGroundHeight(float default_ground_height)
   {
      default_ground_height_ = default_ground_height;
   }
   public float getDefaultGroundHeight()
   {
      return default_ground_height_;
   }


   public static Supplier<PointCloudWorldPacketPubSubType> getPubSubType()
   {
      return PointCloudWorldPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PointCloudWorldPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PointCloudWorldPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.timestamp_, other.timestamp_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.ground_quad_tree_support_, other.ground_quad_tree_support_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.decaying_world_scan_, other.decaying_world_scan_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.default_ground_height_, other.default_ground_height_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PointCloudWorldPacket)) return false;

      PointCloudWorldPacket otherMyClass = (PointCloudWorldPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.timestamp_ != otherMyClass.timestamp_) return false;

      if (!this.ground_quad_tree_support_.equals(otherMyClass.ground_quad_tree_support_)) return false;
      if (!this.decaying_world_scan_.equals(otherMyClass.decaying_world_scan_)) return false;
      if(this.default_ground_height_ != otherMyClass.default_ground_height_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PointCloudWorldPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("timestamp=");
      builder.append(this.timestamp_);      builder.append(", ");
      builder.append("ground_quad_tree_support=");
      builder.append(this.ground_quad_tree_support_);      builder.append(", ");
      builder.append("decaying_world_scan=");
      builder.append(this.decaying_world_scan_);      builder.append(", ");
      builder.append("default_ground_height=");
      builder.append(this.default_ground_height_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/StandReadyControllerState.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates;

import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;

public class StandReadyControllerState extends HoldPositionControllerState
{
   private static final HighLevelControllerName controllerState = HighLevelControllerName.STAND_READY;

   public StandReadyControllerState(OneDoFJointBasics[] controlledJoints, HighLevelControllerParameters highLevelControllerParameters,
                                    JointDesiredOutputListReadOnly highLevelControllerOutput)
   {
      super(controllerState, controlledJoints, highLevelControllerParameters, highLevelControllerOutput);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/NumericalMovingReferenceFrame.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.spatial.Twist;

/**
 * {@code NumericalMovingReferenceFrame} can be used to extend a simple {@code ReferenceFrame} to a
 * {@code MovingReferenceFrame} by computing the frame velocity by using finite difference on its
 * transform.
 * <p>
 * For this to work properly, the update method has to be called at a constant rate consistent with
 * the {@code updateDT} given at construction time.
 * </p>
 * <p>
 * Note that it is preferable to use this class only for test purposes or environment for which the
 * noise on the frame transform is really small. The resulting velocity is not filtered, so any
 * noise in the transform will result in important peaks in the computed velocity.
 * </p>
 */
public class NumericalMovingReferenceFrame extends MovingReferenceFrame
{

   private final ReferenceFrame originalFrame;

   private final Quaternion previousRotation = new Quaternion();
   private final Vector3D previousTranslation = new Vector3D();

   private final Quaternion rotation = new Quaternion();
   private final Vector3D translation = new Vector3D();

   private final Vector3D angularVelocity = new Vector3D();
   private final Vector3D linearVelocity = new Vector3D();

   private final double updateDT;

   public NumericalMovingReferenceFrame(ReferenceFrame originalFrame, double updateDT)
   {
      this("", originalFrame, updateDT);
   }

   public NumericalMovingReferenceFrame(String nameSuffix, ReferenceFrame originalFrame, double updateDT)
   {
      super(originalFrame.getName() + nameSuffix, originalFrame.getRootFrame());
      this.originalFrame = originalFrame;
      this.updateDT = updateDT;

      previousRotation.setToNaN();
      previousTranslation.setToNaN();
   }

   @Override
   protected void updateTransformToParent(RigidBodyTransform transformToParent)
   {
      transformToParent.set(originalFrame.getTransformToRoot());
   }

   @Override
   protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack)
   {
      RigidBodyTransform transformToRoot = originalFrame.getTransformToRoot();

      if (previousRotation.containsNaN() || previousTranslation.containsNaN())
      {
         transformToRoot.get(previousRotation, previousTranslation);

         angularVelocity.setToZero();
         linearVelocity.setToZero();
      }
      else
      {
         transformToRoot.get(rotation, translation);
         rotation.multiplyConjugateOther(previousRotation);
         rotation.getRotationVector(angularVelocity);
         angularVelocity.scale(1.0 / updateDT);
         linearVelocity.sub(translation, previousTranslation);
         linearVelocity.scale(1.0 / updateDT);

         transformToRoot.get(previousRotation, previousTranslation);

         transformToRoot.inverseTransform(angularVelocity);
         transformToRoot.inverseTransform(linearVelocity);
      }

      twistRelativeToParentToPack.setIncludingFrame(this, getParent(), this, angularVelocity, linearVelocity);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ManualHandControlPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "ManualHandControlPacket" defined in "ManualHandControlPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from ManualHandControlPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit ManualHandControlPacket_.idl instead.
*
*/
public class ManualHandControlPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.ManualHandControlPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::ManualHandControlPacket_";

   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.ManualHandControlPacket 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.ManualHandControlPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 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 += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ManualHandControlPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ManualHandControlPacket data, int current_alignment)
   {
      int initial_alignment = 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 += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.ManualHandControlPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_9(data.getRobotSide());

      cdr.write_type_6(data.getIndex());

      cdr.write_type_6(data.getMiddle());

      cdr.write_type_6(data.getThumb());

      cdr.write_type_6(data.getSpread());

      cdr.write_type_2(data.getControlType());

   }

   public static void read(controller_msgs.msg.dds.ManualHandControlPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setRobotSide(cdr.read_type_9());
      	
      data.setIndex(cdr.read_type_6());
      	
      data.setMiddle(cdr.read_type_6());
      	
      data.setThumb(cdr.read_type_6());
      	
      data.setSpread(cdr.read_type_6());
      	
      data.setControlType(cdr.read_type_2());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.ManualHandControlPacket 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("index", data.getIndex());
      ser.write_type_6("middle", data.getMiddle());
      ser.write_type_6("thumb", data.getThumb());
      ser.write_type_6("spread", data.getSpread());
      ser.write_type_2("control_type", data.getControlType());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.ManualHandControlPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setRobotSide(ser.read_type_9("robot_side"));
      data.setIndex(ser.read_type_6("index"));
      data.setMiddle(ser.read_type_6("middle"));
      data.setThumb(ser.read_type_6("thumb"));
      data.setSpread(ser.read_type_6("spread"));
      data.setControlType(ser.read_type_2("control_type"));
   }

   public static void staticCopy(controller_msgs.msg.dds.ManualHandControlPacket src, controller_msgs.msg.dds.ManualHandControlPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.ManualHandControlPacket createData()
   {
      return new controller_msgs.msg.dds.ManualHandControlPacket();
   }
   @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.ManualHandControlPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.ManualHandControlPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.ManualHandControlPacket src, controller_msgs.msg.dds.ManualHandControlPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public ManualHandControlPacketPubSubType newInstance()
   {
      return new ManualHandControlPacketPubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/partNames/SpineJointName.java<|end_filename|>
package us.ihmc.robotics.partNames;

import org.apache.commons.lang3.StringUtils;

public enum SpineJointName
{
   SPINE_PITCH, SPINE_YAW, SPINE_ROLL;

   public static final SpineJointName[] values = values();

   public String getCamelCaseNameForStartOfExpression()
   {
      switch (this)
      {
         case SPINE_PITCH :
         {
            return "spinePitch";
         }

         case SPINE_YAW :
         {
            return "spineYaw";
         }

         case SPINE_ROLL :
         {
            return "spineRoll";
         }

         default :
         {
            throw new RuntimeException("Should not get to here");
         }
      }
   }
   
   public String getPascalCaseName()
   {
      switch (this)
      {
         case SPINE_PITCH :
         {
            return "SpinePitch";
         }

         case SPINE_YAW :
         {
            return "SpineYaw";
         }

         case SPINE_ROLL :
         {
            return "SpineRoll";
         }

         default :
         {
            throw new RuntimeException("Should not get to here");
         }
      }
   }

   public String getCamelCaseNameForMiddleOfExpression()
   {
      return StringUtils.capitalize(getCamelCaseNameForStartOfExpression());
   }

   public String toString()
   {
      return getCamelCaseNameForMiddleOfExpression();
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTPlanner2D.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

/**
 * @method expandTreeGoal re-factor a double array to save every node data of the tree. 
 * 
 * @author InhoLee 170224
 *
 */

public class RRTPlanner2D extends RRTPlanner
{
   public RRTPlanner2D(RRTNode root, RRTNode goal, double stepLength)
   {
      super(root, goal, stepLength);
   }

   public boolean expandTreeGoal(double[] branchInfo)
   {
      if (getRRTTree().expandTree() == true)
      {
         for (int i = 0; i < getRRTTree().newNode.getDimensionOfNodeData(); i++)
         {
            branchInfo[i] = getRRTTree().newNode.getNodeData(i);
            branchInfo[i + getRRTTree().newNode.getDimensionOfNodeData()] = getRRTTree().nearNode.getNodeData(i);
         }

         if (getRRTTree().newNode.getDistance(getGoalNode()) < getRRTTree().getStepLength())
         {
            getRRTTree().newNode.addChildNode(getGoalNode());
            getRRTTree().updatePathNode(getGoalNode());
            setOptimalPath(getRRTTree().pathNode);

            return true;
         }
         return false;
      }
      return false;
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/CTTaskNodeTree.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import java.util.ArrayList;

import us.ihmc.commons.PrintTools;

public class CTTaskNodeTree
{
   private CTTaskNode rootNode;

   private CTTaskNode nearNode;

   private CTTaskNode newNode;
   public CTTaskNode randomNode;

   private ArrayList<CTTaskNode> path = new ArrayList<CTTaskNode>();

   private ArrayList<CTTaskNode> wholeNodes = new ArrayList<CTTaskNode>();
   private ArrayList<CTTaskNode> failNodes = new ArrayList<CTTaskNode>();

   private ArrayList<CTTaskNode> optimalPath = new ArrayList<CTTaskNode>();

   private TaskRegion nodeRegion;

   /**
    * If @param matricRatioTimeToTask is 0.3, the matric will be obtained as
    * much as (getNormalizedTimeGap*0.3 + getNormalizedTaskDisplacement*1.0).
    */
   private static double matricRatioTimeToTask = 0.2; // 0.3

   private double maximumDisplacementOfStep = 0.06;
   private double maximumTimeGapOfStep = 0.05;
   public static double dismissibleTimeGap = 0.01;

   private int dimensionOfTask;

   private ArrayList<String> taskNames;

   private double trajectoryTime;

   private double treeReachingTime = 0.0;

   public CTTaskNodeTree(CTTaskNode rootNode)
   {
      this.rootNode = new CTTaskNode(rootNode);
      this.wholeNodes.add(this.rootNode);

      this.dimensionOfTask = rootNode.getDimensionOfNodeData() - 1;

      this.taskNames = new ArrayList<String>();
      this.taskNames.add("time");
      for (int i = 1; i < this.dimensionOfTask + 1; i++)
         this.taskNames.add("Task_" + i + "_");

      this.treeReachingTime = 0.0;
   }

   public CTTaskNodeTree(CTTaskNode rootNode, String... taskNames)
   {
      this.rootNode = rootNode;
      this.wholeNodes.add(this.rootNode);

      this.dimensionOfTask = rootNode.getDimensionOfNodeData() - 1;

      this.taskNames = new ArrayList<String>();
      this.taskNames.add("time");
      if (this.dimensionOfTask != taskNames.length)
         PrintTools.warn("Task dimension is incorrect");
      else
         for (int i = 1; i < this.dimensionOfTask + 1; i++)
            this.taskNames.add("Task_" + i + "_" + taskNames[i - 1]);

      this.treeReachingTime = 0.0;
   }

   public void setTaskRegion(TaskRegion taskRegion)
   {
      this.nodeRegion = taskRegion;
   }

   public String getTaskName(int indexOfDimension)
   {
      return taskNames.get(indexOfDimension);
   }

   public int getDimensionOfTask()
   {
      return dimensionOfTask;
   }

   public double getTrajectoryTime()
   {
      trajectoryTime = nodeRegion.getTrajectoryTime();
      return trajectoryTime;
   }

   public void setMatricRatioTimeToTask(double ratio)
   {
      matricRatioTimeToTask = ratio;
   }

   private double getNormalizedTaskDisplacement(CTTaskNode nodeOne, CTTaskNode nodeTwo)
   {
      double squaredDisplacement = 0;

      for (int i = 1; i < rootNode.getDimensionOfNodeData(); i++)
      {
         double nodeOneValue = nodeOne.getNormalizedNodeData(i);
         double nodeTwoValue = nodeTwo.getNormalizedNodeData(i);
         squaredDisplacement = squaredDisplacement + (nodeOneValue - nodeTwoValue) * (nodeOneValue - nodeTwoValue);
      }

      return Math.sqrt(squaredDisplacement);
   }

   public double getMetricTaskOnly(CTTaskNode nodeOne, CTTaskNode nodeTwo)
   {
      double normalizedtimeGap = nodeOne.getNormalizedTimeGap(nodeTwo);
      double normalizedTaskDisplacement = getNormalizedTaskDisplacement(nodeOne, nodeTwo);

      if (normalizedtimeGap <= 0)
         return Double.MAX_VALUE;
      else
      {
         double matric = normalizedTaskDisplacement;

         return matric;
      }
   }

   public double getMetricTaskTime(CTTaskNode nodeOne, CTTaskNode nodeTwo)
   {
      double normalizedtimeGap = nodeOne.getNormalizedTimeGap(nodeTwo);
      double normalizedTaskDisplacement = getNormalizedTaskDisplacement(nodeOne, nodeTwo);

      if (normalizedtimeGap <= 0)
         return Double.MAX_VALUE;
      else
      {
         double matric = 0;

         matric = normalizedtimeGap * normalizedtimeGap * matricRatioTimeToTask + normalizedTaskDisplacement * normalizedTaskDisplacement * 1.0;
         matric = Math.sqrt(matric);
         return matric;
      }
   }

   public void updateRandomConfiguration()
   {
      CTTaskNode randomNode = new CTTaskNode(rootNode);
      CTTreeTools.setRandomNormalizedNodeData(randomNode, false, this.treeReachingTime);
      
      for(int i=0;i<rootNode.getDimensionOfNodeData();i++)
      {
         if(!nodeRegion.isEnable(i))
            randomNode.setNormalizedNodeData(i, 0.0);
      }
      
      this.randomNode = randomNode;
   }

   public void updateNearestNodeTaskOnly()
   {
      CTTaskNode nearNode = this.rootNode;
      CTTaskNode curNode;

      double optMatric = Double.MAX_VALUE;
      double curMatric;
      
      for (int i = 0; i < wholeNodes.size(); i++)
      {
         curNode = this.wholeNodes.get(i);
         curMatric = getMetricTaskOnly(curNode, randomNode);
         
         if (curMatric < optMatric)
         {
            optMatric = curMatric;
            nearNode = curNode;
         }
      }

      this.nearNode = nearNode;
   }
   
   public void updateNearestNodeTaskTime()
   {
      CTTaskNode nearNode = this.rootNode;
      CTTaskNode curNode;

      double optMatric = Double.MAX_VALUE;
      double curMatric;
      
      for (int i = 0; i < wholeNodes.size(); i++)
      {
         curNode = this.wholeNodes.get(i);
         curMatric = getMetricTaskTime(curNode, randomNode);
         if (curMatric < optMatric)
         {
            optMatric = curMatric;
            nearNode = curNode;
         }
      }
      
      this.nearNode = nearNode;
   }

   public void updateNewConfiguration()
   {
      CTTaskNode newNode = new CTTaskNode(rootNode);

      double timeGap = this.nearNode.getNormalizedTimeGap(this.randomNode);
      double displacement = getNormalizedTaskDisplacement(this.nearNode, this.randomNode);

      double expandedTime;
      double expandingTimeGap;
      double expandingDisplacement;

      /*
       * timeGap Clamping
       */
      if (timeGap > maximumTimeGapOfStep)
      {
         expandingTimeGap = maximumTimeGapOfStep;
      }
      else
      {
         expandingTimeGap = timeGap;
      }

      /*
       * maximumDisplacementOfStep check the expandedTime is over 1.0 and if
       * then, Clamping again.
       */
      expandedTime = nearNode.getNormalizedNodeData(0) + expandingTimeGap;
      if (expandedTime > 1.0)
      {
         expandedTime = 1.0;
         expandingTimeGap = expandedTime - nearNode.getNormalizedNodeData(0);
      }

      /*
       * 1st displacement Clamping under expandingTimeGap.
       */
      expandingDisplacement = displacement * expandingTimeGap / timeGap;

      /*
       * 2nd displacement Clamping
       */
      if (expandingDisplacement > maximumDisplacementOfStep)
      {
         expandingDisplacement = maximumDisplacementOfStep;
         expandingTimeGap = timeGap * maximumDisplacementOfStep / displacement;
      }

      /*
       * set
       */
      newNode.setNormalizedNodeData(0, nearNode.getNormalizedNodeData(0) + expandingTimeGap);
      for (int i = 1; i < newNode.getDimensionOfNodeData(); i++)
      {
         double iDisplacement = (this.randomNode.getNormalizedNodeData(i) - nearNode.getNormalizedNodeData(i)) / displacement * expandingDisplacement;
         newNode.setNormalizedNodeData(i, nearNode.getNormalizedNodeData(i) + iDisplacement);
      }

      this.newNode = newNode;
   }

   public void connectNewNode(boolean connect)
   {
      if (connect)
      {
         this.nearNode.addChildNode(this.newNode);
         this.wholeNodes.add(this.newNode);

         if (this.newNode.getNormalizedNodeData(0) > this.treeReachingTime)
            this.treeReachingTime = this.newNode.getNormalizedNodeData(0);
      }
      else
      {
         this.newNode.clearParentNode();
         this.failNodes.add(this.newNode);
      }
   }

   /*
    * update shortcut path (optimalPath)
    */
   public void updateOptimalPath()
   {
      if (optimalPath.size() == 0)
         PrintTools.info("The optimalPath is empty.");
      else if (optimalPath.size() == 2)
         PrintTools.info("The optimalPath has only one linear line.");
      else
      {
         /*
          * shortcut algorithm. The integer variable indexOfSelectedPath is 'k'
          * when, node^(k)-----------selected----------node^(k+1)
          */

      }
   }

   public TaskRegion getTaskNodeRegion()
   {
      return nodeRegion;
   }

   public CTTaskNode getNewNode()
   {
      return newNode;
   }

   public CTTaskNode getNearNode()
   {
      return nearNode;
   }

   public ArrayList<CTTaskNode> getPath()
   {
      return path;
   }

   public void addNodeOnPath(CTTaskNode node)
   {
      path.add(node);
   }

   public ArrayList<CTTaskNode> getWholeNodes()
   {
      return wholeNodes;
   }

   public ArrayList<CTTaskNode> getFailNodes()
   {
      return failNodes;
   }

   public ArrayList<CTTaskNode> getOptimalPath()
   {
      return optimalPath;
   }

   public double getTreeReachingTime()
   {
      return treeReachingTime;
   }

   /*
    * When the new configuration is valid, return true.
    */
   //   private boolean connectNewConfiguration()
   //   {
   //      this.newNode.convertNormalizedDataToData(nodeRegion);
   //      this.newNode.setParentNode(this.nearNode);
   //
   //      if (this.newNode.isValidNode())
   //      {
   //         nearNode.addChildNode(this.newNode);
   //         wholeNodes.add(newNode);
   //         //         PrintTools.info("this new Configuration is added on tree");      
   //
   //         return true;
   //      }
   //      else
   //      {
   //         this.newNode.clearParentNode();
   //         failNodes.add(this.newNode);
   //         //         PrintTools.info("this new Configuration cannot be added on tree");
   //         return false;
   //      }
   //   }

   //   public void expandTree(int numberOfExpanding)
   //   {
   //      this.rootNode.convertDataToNormalizedData(this.nodeRegion);
   //
   //      for (int i = 0; i < numberOfExpanding; i++)
   //      {
   //         PrintTools.info("expanding process " + i);
   //         if (expandingTree())
   //         {
   //            PrintTools.info("expanding is done " + i);
   //            ArrayList<CTTaskNode> revertedPath = new ArrayList<CTTaskNode>();
   //            CTTaskNode currentNode = newNode;
   //            revertedPath.add(currentNode);
   //
   //            while (true)
   //            {
   //               currentNode = currentNode.getParentNode();
   //               if (currentNode != null)
   //               {
   //                  revertedPath.add(currentNode);
   //               }
   //               else
   //                  break;
   //            }
   //
   //            int revertedPathSize = revertedPath.size();
   //
   //            path.clear();
   //            for (int j = 0; j < revertedPathSize; j++)
   //               path.add(revertedPath.get(revertedPathSize - 1 - j));
   //
   //            PrintTools.info("Constructed Tree size is " + revertedPathSize);
   //
   ////            optimalPath.clear();
   ////            for (int j = 0; j < path.size(); j++)
   ////            {
   ////               CTTaskNode node = path.get(i).createNodeCopy();
   ////               optimalPath.add(node);
   ////            }
   //
   //            break;
   //         }
   //      }
   //   }

   /*
    * As long as the tree does not reach the trajectory time, this method
    * returns false.
    */
   //   public boolean expandingTree()
   //   {
   //      updateRandomConfiguration();
   //      updateNearestNode();
   //      updateNewConfiguration();
   //      if (connectNewConfiguration())
   //      {
   //         if (this.newNode.getTime() == getTrajectoryTime())
   //            return true;
   //         else
   //            return false;
   //      }
   //      else
   //      {
   //         return false;
   //      }
   //   }
   
}

<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/StraightLineOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import us.ihmc.euclid.referenceFrame.FrameOrientation2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.commons.MathTools;

public class StraightLineOverheadPath extends OverheadPath
{
   private final FramePoint2D startPoint;
   private final FramePoint2D endPoint;
   private final FrameOrientation2D orientation;
   private final double distance;

   public StraightLineOverheadPath(FramePose2D startPose, FramePoint2D endPosition)
   {
      startPose.checkReferenceFrameMatch(endPosition);
      this.startPoint = new FramePoint2D(startPose.getPosition());
      this.endPoint = new FramePoint2D(endPosition);
      this.orientation = new FrameOrientation2D(startPose.getOrientation());
      this.distance = endPoint.distance(startPoint);
   }

   private FramePoint2D tempPosition;

   @Override
   public FramePose2D getPoseAtS(double pathVariableS)
   {
      pathVariableS = MathTools.clamp(pathVariableS, 0.0, 1.0);
      return getExtrapolatedPoseAtS(pathVariableS);
   }

   @Override
   public FramePose2D getExtrapolatedPoseAtS(double pathVariableS)
   {
      if (tempPosition == null)
         tempPosition = new FramePoint2D(orientation.getReferenceFrame());
      tempPosition.interpolate(startPoint, endPoint, pathVariableS);

      return new FramePose2D(tempPosition, orientation);
   }

   @Override
   public ReferenceFrame getReferenceFrame()
   {
      return orientation.getReferenceFrame();
   }

   @Override
   public StraightLineOverheadPath changeFrameCopy(ReferenceFrame desiredFrame)
   {
      FramePoint2D newStartPoint = new FramePoint2D(startPoint);
      newStartPoint.changeFrame(desiredFrame);
      FrameOrientation2D newStartOrientation = new FrameOrientation2D(orientation);
      newStartOrientation.changeFrame(desiredFrame);
      FramePose2D newStartPose = new FramePose2D(newStartPoint, newStartOrientation);
      FramePoint2D newEndPoint = new FramePoint2D(endPoint);
      newEndPoint.changeFrame(desiredFrame);

      return new StraightLineOverheadPath(newStartPose, newEndPoint);
   }

   public double getDistance()
   {
      return distance;
   }

}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/geometry/ConcaveHullPocket.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.geometry;

import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;

/**
 * A pocket is a concave region.
 * It has different properties:
 *  - It has a unique bridge
 *  - It has a deepest vertex (straight-line criterion)
 *  - It has a maximum depth. 
 * @author Sylvain
 *
 */
public class ConcaveHullPocket
{
   private int startBridgeIndex;
   private int endBridgeIndex;

   private Point2DReadOnly startBridgeVertex;
   private Point2DReadOnly endBridgeVertex;

   private int deepestVertexIndex;
   private Point2DReadOnly deepestVertex;

   private double maxDepth;

   public ConcaveHullPocket()
   {
      clear();
   }

   public void clear()
   {
      startBridgeIndex = -1;
      endBridgeIndex = -1;

      startBridgeVertex = null;
      endBridgeVertex = null;

      clearDepthParameters();
   }

   public void clearDepthParameters()
   {
      deepestVertexIndex = -1;
      deepestVertex = null;

      maxDepth = Double.NEGATIVE_INFINITY;
   }

   public void setBridgeIndices(int startBridgeIndex, int endBridgeIndex)
   {
      this.startBridgeIndex = startBridgeIndex;
      this.endBridgeIndex = endBridgeIndex;
   }

   public void setBridgeVertices(Point2DReadOnly startBridgeVertex, Point2DReadOnly endBridgeVertex)
   {
      this.startBridgeVertex = startBridgeVertex;
      this.endBridgeVertex = endBridgeVertex;
   }

   public void setDeepestVertexIndex(int deepestVertexIndex)
   {
      this.deepestVertexIndex = deepestVertexIndex;
   }

   public void setDeepestVertex(Point2DReadOnly deepestVertex)
   {
      this.deepestVertex = deepestVertex;
   }

   public void setMaxDepth(double maxDepth)
   {
      this.maxDepth = maxDepth;
   }

   public int getStartBridgeIndex()
   {
      return startBridgeIndex;
   }

   public int getEndBridgeIndex()
   {
      return endBridgeIndex;
   }

   public Point2DReadOnly getStartBridgeVertex()
   {
      return startBridgeVertex;
   }

   public Point2DReadOnly getEndBridgeVertex()
   {
      return endBridgeVertex;
   }

   public int getDeepestVertexIndex()
   {
      return deepestVertexIndex;
   }

   public Point2DReadOnly getDeepestVertex()
   {
      return deepestVertex;
   }

   public double getMaxDepth()
   {
      return maxDepth;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/footstepPlanning/ConcurrentPairList.java<|end_filename|>
package us.ihmc.avatar.footstepPlanning;

import org.apache.commons.lang3.tuple.ImmutablePair;
import us.ihmc.concurrent.ConcurrentCopier;
import us.ihmc.tools.lists.PairList;

class ConcurrentPairList<L, R> extends ConcurrentCopier<PairList<L, R>>
{
   public ConcurrentPairList()
   {
      super(PairList::new);
   }

   public boolean isEmpty()
   {
      PairList<L, R> readCopy = getCopyForReading();
      if (readCopy == null)
         return true;
      else
         return readCopy.isEmpty();
   }

   public void clear()
   {
      PairList<L, R> updatedList = getCopyForWriting();
      updatedList.clear();

      commit();
   }

   public void add(L leftObjectToAdd, R rightObjectToAdd)
   {
      PairList<L, R> existingList = getCopyForReading();
      PairList<L, R> updatedList = getCopyForWriting();
      updatedList.clear();
      if (existingList != null)
         updatedList.addAll(existingList);
      updatedList.add(new ImmutablePair<>(leftObjectToAdd, rightObjectToAdd));

      this.commit();
   }

   public ImmutablePair<L, R> remove(int indexToRemove)
   {
      PairList<L, R> existingList = getCopyForReading();
      PairList<L, R> updatedList = getCopyForWriting();
      updatedList.clear();
      if (existingList != null)
         updatedList.addAll(existingList);
      ImmutablePair<L, R> objectToReturn = updatedList.remove(indexToRemove);

      this.commit();

      return objectToReturn;
   }

   public Iterable<ImmutablePair<L, R>> iterable()
   {
      return getCopyForReading();
   }
}


<|start_filename|>ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/RemoteStandaloneFootstepPlannerUI.java<|end_filename|>
package us.ihmc.footstepPlanning.ui;

import javafx.application.Application;
import javafx.application.Platform;
import javafx.stage.Stage;
import us.ihmc.footstepPlanning.communication.FootstepPlannerMessagerAPI;
import us.ihmc.footstepPlanning.ui.components.FootstepPathCalculatorModule;
import us.ihmc.javaFXToolkit.messager.JavaFXMessager;
import us.ihmc.javaFXToolkit.messager.SharedMemoryJavaFXMessager;
import us.ihmc.pubsub.DomainFactory;

/**
 * This class provides a fully remote footstep planner. It includes the UI and the footstep planning modules, which are
 * published out using RTPS messages. They are converted from JavaFX messages internally and then broadcast out.
 */
public class RemoteStandaloneFootstepPlannerUI extends Application
{
   private FootstepPathCalculatorModule module;
   private JavaFXMessager messager;
   private RemotePlannerMessageConverter messageConverter;

   private FootstepPlannerUI ui;

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      messager = new SharedMemoryJavaFXMessager(FootstepPlannerMessagerAPI.API);
      messageConverter = RemotePlannerMessageConverter.createConverter(messager, "", DomainFactory.PubSubImplementation.INTRAPROCESS);
      module = new FootstepPathCalculatorModule(messager);

      messager.startMessager();
      module.start();

      ui = FootstepPlannerUI.createMessagerUI(primaryStage, messager);
      ui.show();
   }

   @Override
   public void stop() throws Exception
   {
      super.stop();

      module.stop();
      messager.closeMessager();
      messageConverter.destroy();
      ui.stop();

      Platform.exit();
   }

   public static void main(String[] args)
   {
      launch(args);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/DoubleHashHeightMap.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.geometry.InclusionFunction;

public class DoubleHashHeightMap implements HeightMapWithPoints
{
   private HashMap<Integer, LinkedHashMap<Integer, Double>> rows;
   private final double gridSize;

   public DoubleHashHeightMap(double gridSize)
   {
      this.gridSize = gridSize;
      rows = new LinkedHashMap<Integer, LinkedHashMap<Integer, Double>>();
   }

   public double getHeightAtPoint(double x, double y)
   {
      return heightAtPoint(index(x), index(y));
   }

   public boolean containsPoint(double x, double y)
   {
      return containsPoint(index(x), index(y));
   }

   private boolean containsPoint(int x, int y)
   {
      if (!rows.containsKey(x))
         return false;
      HashMap<Integer, Double> row = rows.get(x);
      if (!row.containsKey(y))
         return false;
      Double height = row.get(y);
      if (!Double.isFinite(height))
         return false;

      return true;

   }

   private double heightAtPoint(int x, int y)
   {
      if (!rows.containsKey(x))
         return Double.NaN;
      HashMap<Integer, Double> row = rows.get(x);
      if (!row.containsKey(y))
         return Double.NaN;
      Double height = row.get(y);

      return height;

   }

   private boolean addPoint(int xIndex, int yIndex, double z)
   {

      synchronized (rows)
      {
         if (Double.isFinite(z))
         {
            if (!rows.containsKey(xIndex))
            {
               rows.put(xIndex, new LinkedHashMap<Integer, Double>());
            }


            HashMap<Integer, Double> xRow = rows.get(xIndex);
            xRow.put(yIndex, z);
         }
      }

      return true;
   }

   public boolean addPoint(double x, double y, double z)
   {
      return addPoint(index(x), index(y), z);
   }

   private int index(double coordinate)
   {
      double quotient = coordinate / gridSize;
      int closestIndex = (int) Math.round(quotient);

      return closestIndex;
   }

   public double gridSize()
   {
      return this.gridSize;
   }

   private List<Point3D> getAllPointsWithin(int xMin, int xMax, int yMin, int yMax)
   {
      ArrayList<Point3D> points = new ArrayList<Point3D>();
      if ((xMax >= xMin) && (yMax >= yMin))
      {
         if (rows.values().size() < (xMax - xMin+1))
         {
            iterateOverKeysToPackAllRowsWithin(xMin, xMax, yMin, yMax, points);
         }
         else
         {
            iterateOverValuesToPackAllRowsWithin(xMin, xMax, yMin, yMax, points);
         }
      }

      return points;
   }

   public void clear()
   {
      synchronized (rows)
      {
         rows.clear();
      }
   }

   private void iterateOverKeysToPackAllRowsWithin(int xMin, int xMax, int yMin, int yMax, ArrayList<Point3D> points)
   {
      synchronized (rows)
      {
         for (int x : rows.keySet())
         {
            if ((xMin <= x) && (xMax >= x))
            {
               LinkedHashMap<Integer, Double> row = new LinkedHashMap<Integer, Double>(rows.get(x));
               getAllPointsInRow(yMin, yMax, points, x, row);
            }
         }
      }
   }

   private void iterateOverValuesToPackAllRowsWithin(int xMin, int xMax, int yMin, int yMax, ArrayList<Point3D> points)
   {
      synchronized (rows)
      {
         for (int x = xMin; x <= xMax; x++)
         {
            if (rows.containsKey(x))
            {
               HashMap<Integer, Double> row = rows.get(x);
               getAllPointsInRow(yMin, yMax, points, x, row);
            }
         }
      }
   }

   private void getAllPointsInRow(int yMin, int yMax, ArrayList<Point3D> points, int x, HashMap<Integer, Double> row)
   {
      if (row.values().size() < (yMax - yMin+1))
      {
         iterateOverKeysetToPackAllPoints(yMin, yMax, points, x, row);
      }
      else
      {
         iterateOverColumnsToPackAllPoints(yMin, yMax, points, x, row);
      }
   }

   private void iterateOverColumnsToPackAllPoints(int yMin, int yMax, ArrayList<Point3D> points, int x, HashMap<Integer, Double> row)
   {
      for (int y = yMin; y <= yMax; y++)
      {
         if (row.containsKey(y))
         {
            points.add(new Point3D(x * gridSize, y * gridSize, row.get(y)));
         }
      }
   }

   private void iterateOverKeysetToPackAllPoints(int yMin, int yMax, ArrayList<Point3D> points, int x, HashMap<Integer, Double> row)
   {
      for (int y : row.keySet())
      {
         if ((yMin <= y) && (yMax >= y))
         {
            points.add(new Point3D(x * gridSize, y * gridSize, row.get(y)));
         }
      }
   }

   public List<Point3D> getAllPointsWithinArea(double xCenter, double yCenter, double xExtent, double yExtent)
   {
      return getAllPointsWithinAreaMinMax(xCenter - (xExtent * 0.5), xCenter + (xExtent * 0.5), yCenter - (yExtent * 0.5), yCenter + (yExtent * 0.5));
   }

   private List<Point3D> getAllPointsWithinAreaMinMax(double xMin, double xMax, double yMin, double yMax)
   {
      return getAllPointsWithin(index(xMin), index(xMax), index(yMin), index(yMax));
   }

   public List<Point3D> getAllPointsWithinArea(double xCenter, double yCenter, double xExtent, double yExtent,
         InclusionFunction<Point3D> maskFunctionAboutCenter)
   {
      List<Point3D> allPointsInArea = getAllPointsWithinArea(xCenter, yCenter, xExtent, yExtent);
      List<Point3D> pointsToReturn = new ArrayList<Point3D>();
      for (Point3D point : allPointsInArea)
      {
           if (maskFunctionAboutCenter.isIncluded(point))
            pointsToReturn.add(point);
      }
      return pointsToReturn;
   }


}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/producers/VideoDataServer.java<|end_filename|>
package us.ihmc.communication.producers;

import us.ihmc.communication.interfaces.Connectable;
import us.ihmc.communication.video.VideoCallback;

public interface VideoDataServer extends Connectable, VideoCallback
{
   
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/joystick/BooleanYoVariableJoystickEventListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.joystick;

import net.java.games.input.Component;
import net.java.games.input.Event;
import us.ihmc.yoVariables.variable.YoBoolean;
import us.ihmc.tools.inputDevices.joystick.JoystickEventListener;

public class BooleanYoVariableJoystickEventListener implements JoystickEventListener
{
   private final YoBoolean variable;
   private final Component component;
   private final boolean flip;
   private final boolean toggle;
   
   public BooleanYoVariableJoystickEventListener(YoBoolean variable, Component component, boolean toggle)
   {
      this(variable, component, toggle, false);
   }

   public BooleanYoVariableJoystickEventListener(YoBoolean variable, Component component, boolean toggle,  boolean flip)
   {
      if (component.isAnalog())
         throw new RuntimeException("component is analog; should be digital (i.e. an on/off button)");
      this.variable = variable;
      this.component = component;
      this.flip = flip;
      this.toggle=toggle;
   }

   @Override
   public void processEvent(Event event)
   {
      if (event.getComponent() == component)
      {
         boolean value = event.getValue() == 1.0f;
         if(toggle)
         {
            if(value)
               variable.set(!variable.getBooleanValue());
         }
         else
         {
            variable.set(value ^ flip);
         }
      }
   }
}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/properties/YawProperty.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui.properties;

import javafx.beans.property.Property;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.robotEnvironmentAwareness.ui.properties.ParametersProperty;

public class YawProperty extends ParametersProperty<Quaternion>
{
   private final DoubleField yaw = new DoubleField(Quaternion::getYaw, (p, v) -> p.setYawPitchRoll(v, 0.0, 0.0));

   public YawProperty(Object bean, String name)
   {
      this(bean, name, 0.0);
   }

   public YawProperty(Object bean, String name, double initialValue)
   {
      super(bean, name, new Quaternion(initialValue, 0.0, 0.0));
   }

   public void bindBidirectionalYaw(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, yaw);
   }


   @Override
   protected Quaternion getValueCopy(Quaternion valueToCopy)
   {
      return new Quaternion(valueToCopy);
   }

}


<|start_filename|>ihmc-path-planning/src/test/java/us/ihmc/pathPlanning/visibilityGraphs/visualizer/WalkerCollisionsViewer.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.visualizer;

import java.util.List;
import java.util.concurrent.atomic.AtomicReference;

import javafx.animation.AnimationTimer;
import javafx.scene.Group;
import javafx.scene.Node;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.MeshView;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.javaFXToolkit.messager.JavaFXMessager;
import us.ihmc.javaFXToolkit.shapes.JavaFXMeshBuilder;
import us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics;

public class WalkerCollisionsViewer extends AnimationTimer
{
   private static final double COLLISION_SIZE = 0.1;
   private final Group root = new Group();
   private final AtomicReference<MeshView> collisionGraphics = new AtomicReference<>(null);
   private final AtomicReference<Boolean> reset;

   public WalkerCollisionsViewer(JavaFXMessager messager)
   {
      messager.registerTopicListener(UIVisibilityGraphsTopics.WalkerCollisionLocations, this::processCollisions);
      reset = messager.createInput(UIVisibilityGraphsTopics.GlobalReset, false);
   }

   @Override
   public void handle(long now)
   {
      if (reset.getAndSet(false))
         root.getChildren().clear();

      MeshView meshView = collisionGraphics.getAndSet(null);

      if (meshView != null)
      {
         root.getChildren().add(meshView);
      }
   }

   private void processCollisions(List<Point3D> collisions)
   {
      JavaFXMeshBuilder meshBuilder = new JavaFXMeshBuilder();

      for (Point3D collision : collisions)
         meshBuilder.addTetrahedron(COLLISION_SIZE, collision);

      MeshView meshView = new MeshView(meshBuilder.generateMesh());
      meshView.setMaterial(new PhongMaterial(Color.RED));
      collisionGraphics.set(meshView);
   }

   public Node getRoot()
   {
      return root;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WeightMatrix3DMessage.java<|end_filename|>
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 allows the user to precisely choose the priority of each component of a taskspace command.
       */
public class WeightMatrix3DMessage extends Packet<WeightMatrix3DMessage> implements Settable<WeightMatrix3DMessage>, EpsilonComparable<WeightMatrix3DMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The ID of the reference frame defining the weight frame.
            * This reference frame defines the x axis, y axis, z axis for the weights.
            * This frame is optional. It is preferable to provide it when possible, but when it is absent, i.e. equal to 0,
            * the weight matrix will then be generated regardless to what frame is it used in.
            */
   public long weight_frame_id_;
   /**
            * Specifies the qp weight for the x-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public double x_weight_ = -1.0;
   /**
            * Specifies the qp weight for the y-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public double y_weight_ = -1.0;
   /**
            * Specifies the qp weight for the z-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public double z_weight_ = -1.0;

   public WeightMatrix3DMessage()
   {
   }

   public WeightMatrix3DMessage(WeightMatrix3DMessage other)
   {
      this();
      set(other);
   }

   public void set(WeightMatrix3DMessage other)
   {
      sequence_id_ = other.sequence_id_;

      weight_frame_id_ = other.weight_frame_id_;

      x_weight_ = other.x_weight_;

      y_weight_ = other.y_weight_;

      z_weight_ = other.z_weight_;

   }

   /**
            * 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_;
   }

   /**
            * The ID of the reference frame defining the weight frame.
            * This reference frame defines the x axis, y axis, z axis for the weights.
            * This frame is optional. It is preferable to provide it when possible, but when it is absent, i.e. equal to 0,
            * the weight matrix will then be generated regardless to what frame is it used in.
            */
   public void setWeightFrameId(long weight_frame_id)
   {
      weight_frame_id_ = weight_frame_id;
   }
   /**
            * The ID of the reference frame defining the weight frame.
            * This reference frame defines the x axis, y axis, z axis for the weights.
            * This frame is optional. It is preferable to provide it when possible, but when it is absent, i.e. equal to 0,
            * the weight matrix will then be generated regardless to what frame is it used in.
            */
   public long getWeightFrameId()
   {
      return weight_frame_id_;
   }

   /**
            * Specifies the qp weight for the x-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public void setXWeight(double x_weight)
   {
      x_weight_ = x_weight;
   }
   /**
            * Specifies the qp weight for the x-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public double getXWeight()
   {
      return x_weight_;
   }

   /**
            * Specifies the qp weight for the y-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public void setYWeight(double y_weight)
   {
      y_weight_ = y_weight;
   }
   /**
            * Specifies the qp weight for the y-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public double getYWeight()
   {
      return y_weight_;
   }

   /**
            * Specifies the qp weight for the z-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public void setZWeight(double z_weight)
   {
      z_weight_ = z_weight;
   }
   /**
            * Specifies the qp weight for the z-axis, if set to NaN or to a negative value the controller will use the default weight for this axis. The weight is -1 by default.
            */
   public double getZWeight()
   {
      return z_weight_;
   }


   public static Supplier<WeightMatrix3DMessagePubSubType> getPubSubType()
   {
      return WeightMatrix3DMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WeightMatrix3DMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WeightMatrix3DMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.weight_frame_id_, other.weight_frame_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.x_weight_, other.x_weight_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.y_weight_, other.y_weight_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.z_weight_, other.z_weight_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WeightMatrix3DMessage)) return false;

      WeightMatrix3DMessage otherMyClass = (WeightMatrix3DMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.weight_frame_id_ != otherMyClass.weight_frame_id_) return false;

      if(this.x_weight_ != otherMyClass.x_weight_) return false;

      if(this.y_weight_ != otherMyClass.y_weight_) return false;

      if(this.z_weight_ != otherMyClass.z_weight_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WeightMatrix3DMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("weight_frame_id=");
      builder.append(this.weight_frame_id_);      builder.append(", ");
      builder.append("x_weight=");
      builder.append(this.x_weight_);      builder.append(", ");
      builder.append("y_weight=");
      builder.append(this.y_weight_);      builder.append(", ");
      builder.append("z_weight=");
      builder.append(this.z_weight_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTValidConnection.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

import java.util.ArrayList;

import us.ihmc.commons.PrintTools;

public class RRTValidConnection
{
   private RRTNode parentNode;
   private RRTNode childNode;
   private RRTNode nodeCreator;

   private ArrayList<RRTNode> nodes = new ArrayList<RRTNode>();

   public int numberOfPiece = 20;

   public RRTValidConnection(RRTNode parentNode, RRTNode childNode)
   {
      this.parentNode = parentNode;
      this.childNode = childNode;
      this.nodeCreator = parentNode.createNode();
      initialize();
   }

   private void initialize()
   {
      double length = parentNode.getDistance(childNode);

      if (length == 0)
      {
         nodes.add(parentNode);
      }
      else
      {
         for (int i = 1; i < numberOfPiece-1; i++)
         {
            RRTNode aNode = nodeCreator.createNode();

            for (int j = 0; j < parentNode.getDimensionOfNodeData(); j++)
            {
               double parentData = parentNode.getNodeData(j);
               double childData = childNode.getNodeData(j);
               double aNodeData;
               aNodeData = (parentData - childData) * i / (numberOfPiece - 1) + childData;
               aNode.setNodeData(j, aNodeData);
            }

            nodes.add(aNode);
         }
      }
   }

   public boolean isValidConnection()
   {
      for (int i = 1; i < nodes.size()-1; i++)
      {
         if (nodes.get(i).isValidNode() == false)
         {
            return false;
         }
      }
      return true;
   }
   /*
    * numberOfPiece should larger than 2
    */
   public void reInitialize(int numberOfPiece)
   {
      nodes.clear();
      this.numberOfPiece = numberOfPiece;
      initialize();
   }   
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/remote/ReflectiveTCPServer.java<|end_filename|>
package us.ihmc.communication.remote;

import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.io.Serializable;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;
import java.net.Inet4Address;
import java.net.InetAddress;
import java.net.NetworkInterface;
import java.net.ServerSocket;
import java.net.Socket;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.util.Enumeration;
import java.util.Vector;

public class ReflectiveTCPServer
{
   private Object reflectiveObject;
   protected ServerSocket serverSocket = null;
   protected int port;
   protected String host;
   private boolean DEBUG = false;

   public ReflectiveTCPServer(Object reflectiveObject)
   {
      this.reflectiveObject = reflectiveObject;
   }

   public void init()
   {
      // NO port specified
      init(null, 0);
   }

   public void init(int port)
   {
      // NO port specified
      init(null, port);
   }

   public void initByInterface(String interfaceName, int iPort)
   {
      try
      {
//       Enumeration inets = NetworkInterface.getNetworkInterfaces();
//       while(inets.hasMoreElements())
//       {
//           System.out.println(inets.nextElement());
//           System.out.println("--------------------------------");
//       }

         NetworkInterface myNetworkInterface = NetworkInterface.getByName(interfaceName);
         if (DEBUG)
            System.out.println("interfaceName = " + myNetworkInterface);

         // Find IP address category 4 for this network interface
         boolean foundHost = false;
         String host = null;
         if (myNetworkInterface != null)
         {
            for (Enumeration<InetAddress> myAddresses = myNetworkInterface.getInetAddresses(); myAddresses.hasMoreElements() &&!foundHost; )
            {
               InetAddress currentInetAddress = myAddresses.nextElement();

               if (currentInetAddress instanceof Inet4Address)
               {
                  host = ((Inet4Address) currentInetAddress).getHostAddress();
                  foundHost = true;

                  if (DEBUG)
                     System.out.println("Found address for " + interfaceName + ": " + currentInetAddress.getHostAddress());
               }
            }
         }

         init(host, iPort);
      }
      catch (SocketException ex)
      {
         ex.printStackTrace();
      }
   }

   /**
    * Tries to start a socketserver on the specified port.
    *
    * @param host - host name (IP) to use
    * @param port - port to use for opened socket
    */
   public void init(String host, int port)
   {
      if (host != null)
         this.host = host;
      else
         try
         {
            this.host = InetAddress.getLocalHost().getHostAddress();
         }
         catch (UnknownHostException xcp)
         {
            xcp.printStackTrace();
            this.host = null;
         }

      // TODO: Inet4Address hostInet4Address = new Inet4Address(null, textToNumericFormat(_host));

      // Try to create server socket
      try
      {
         serverSocket = new ServerSocket(port);    // TODO: use hostInet4Address
      }
      catch (IOException xcp)
      {
         if (DEBUG)
            System.out.println(".init: Error Creating Server Socket on port: " + port);

         xcp.printStackTrace();
         serverSocket = null;
         this.port = -1;

         return;
      }

      // Get port being used for server socket
      this.port = serverSocket.getLocalPort();

      // Create a server thread and start it listening
      Thread svrThrd = new Thread(new ServerThread());
      svrThrd.setDaemon(true);
      svrThrd.start();

      if (DEBUG)
         System.out.println(".init: comm server started listening on:\n" + "\thost = " + this.host + "\n\tport = " + this.port);
   }

   /**
    * Tries to close server socket
    */
   public void close()
   {
      try
      {
         serverSocket.close();
      }
      catch (Exception xcp)
      {
         // Do nothing
      }

      serverSocket = null;
      port = -1;
      host = null;
   }

   /**
    * Returns the hostname of this server.
    */
   public String getHost()
   {
      return host;
   }

   /**
    * Returns the port number of this server.
    */
   public int getPort()
   {
      return port;
   }

   // ------------- Server Thread - Listens for new connections and add them to the list of connections to handle
   public class ServerThread implements Runnable
   {
      @Override
      public void run()
      {
         while ((serverSocket != null) &&!serverSocket.isClosed())
         {
            try
            {
               // Wait for client requests
               if (DEBUG)
                  System.out.println(".ServerThread_" + port + ": Waiting for next client");

               Socket clientSocket = serverSocket.accept();

               // Create a handler for new clients
               if (DEBUG)
                  System.out.println(".ServerThread_" + port + ": Got a connection handling on Port: " + clientSocket.getPort());

               ClientHandler ch = new ClientHandler(clientSocket);

               ch.start();

            }
            catch (SocketException xcp)
            {
               if (!xcp.getMessage().equals("socket closed"))
                  xcp.printStackTrace();
            }
            catch (IOException xcp)
            {
               xcp.printStackTrace();
            }
         }

         if (DEBUG)
            System.out.println(".ServerThread: ServerAccept Thread has Stopped.");
      }
   }


   // -------- Client Thread
   public class ClientHandler implements Runnable
   {
      protected Socket _clientSocket = null;
      protected ObjectInputStream _ois = null;
      protected ObjectOutputStream _oos = null;

      protected boolean _disconnected = false;
      protected int _clientPort = -1;
      protected String _clientHost = null;

      /**
       * ClientHandler sets the socket
       *
       * @param clientSocket      the socket for the client handler to read from
       */
      public ClientHandler(Socket clientSocket)
      {
         _clientSocket = clientSocket;
         _clientPort = clientSocket.getPort();
      }

      /**
       * starts the client handler
       */
      public void start()
      {
         if (_clientSocket == null)
         {
            _disconnected = true;
            if (DEBUG)
               System.out.println("client socket is null");

            return;
         }

         Thread thrd = new Thread(this);
         thrd.setDaemon(true);
         thrd.start();
      }

      /**
       * stop - sets client handler to disconnected which will terminate the "run" while loop
       */
      public void stop()
      {
         _disconnected = true;
      }

      /**
       * run - while connected, continues to read the object input stream and adds all objects to the newObjects vector
       */
      @Override
      public void run()
      {
         try
         {
            _ois = new ObjectInputStream(_clientSocket.getInputStream());
            _oos = new ObjectOutputStream(_clientSocket.getOutputStream());

         }
         catch (IOException xcp)
         {
            _ois = null;
            _disconnected = true;
            xcp.printStackTrace();

            return;
         }

         while (!_disconnected)
         {
            try
            {
               long start = System.currentTimeMillis();
               Object obj = _ois.readObject();
               long read = 0, invoke = 0, reply = 0;
               read = System.currentTimeMillis() - start;
               if (DEBUG)
                  System.out.println(".ClientHandler: received object " + obj);

               // Update message listeners
               if (obj instanceof RemoteRequest)
               {
                  RemoteRequest remoteRequest = (RemoteRequest) obj;
                  if (DEBUG)
                     System.out.println(".ClientHandler: received remote request " + remoteRequest);

                  try
                  {
                     Object result = invokeMethod(remoteRequest.methodName, remoteRequest.parameters);
                     invoke = System.currentTimeMillis() - (start + read);

                     if (result instanceof Serializable)
                     {
                        if (DEBUG)
                           System.out.println(".ClientHandler: returning " + result);

                        _oos.writeObject(result);

//                      _oos.flush();
                        reply = System.currentTimeMillis() - (start + read + invoke);
                     }
                     else
                     {
                        if (result != null)
                           System.out.println(remoteRequest.methodName + " cannot return non serializable: " + result.getClass());
                     }
                  }
                  catch (Exception ex)
                  {
                     ex.printStackTrace();
                  }
               }
               else
               {
                  if (DEBUG)
                     System.out.println("received obj which is not a RemoteRequest: " + obj.getClass());
               }

               if(DEBUG) System.out.println("read=" + read + "    invoke=" + invoke + "   reply=" + reply + "    obj=" + obj);
            }
            catch (IOException xcp)
            {
               _disconnected = true;
            }
            catch (ClassNotFoundException xcp)
            {
               _disconnected = true;
            }
         }

         try
         {
            _clientSocket.close();
         }
         catch (IOException xcp)
         {
         }

         _ois = null;
         _clientSocket = null;
      }

      /**
       * isConnected
       *
       * @return      a boolean true = connected, false = NOT connected
       */
      public boolean isConnected()
      {
         return !_disconnected;
      }

      /**
       * getHost
       *
       * @return      host name
       */
      public String getHost()
      {
         return _clientHost;
      }

      /**
       * getPort
       *
       * @return      port number
       */
      public int getPort()
      {
         return _clientPort;
      }
   }


   public Object invokeMethod(String methodName, Vector<?> args) throws Exception
   {
      try
      {
         if (args != null)
         {
            Object argObjects[] = new Object[args.size()];
            for (int i = 0; i < args.size(); i++)
            {
               argObjects[i] = args.elementAt(i);
            }

            // We have to check this the "hard way" because Class.getMethod(name, args[]) does not work if the args are subclasses of the parameter classes

            // Find the method matching the method name requested
            Method methods[] = reflectiveObject.getClass().getMethods();
            boolean foundMethod = false;

            for (int i = 0; i < methods.length; i++)
            {
               if (methods[i].getName().equals(methodName))
               {
                  // Check to see if the parameter classes match the arguments
                  Class<?> parameters[] = methods[i].getParameterTypes();
                  boolean classesMatch = false;
                  if (parameters.length == argObjects.length)
                  {
                     classesMatch = true;

                     for (int j = 0; j < parameters.length; j++)
                     {
                        // bug fix: check for null case
                        // if the arg is null, or if it is an instance of the parameter's class,
                        // we are OK; otherwise break
                        if ((argObjects[j] != null) &&!parameters[j].isInstance(argObjects[j]))
                        {
                           classesMatch = false;

                           break;
                        }
                     }
                  }

                  // If we found the right method, execute it
                  if (classesMatch)
                  {
                     foundMethod = true;
                     Object result = methods[i].invoke(reflectiveObject, argObjects);

                     return result;
                  }
               }
            }

            // If we didn't find the method, throw an exception
            if (!foundMethod)
               throw new NoSuchMethodException(methodName);
         }
         else    // if args == null, invoke method with no args
         {
            Object result = reflectiveObject.getClass().getMethod(methodName, (Class[]) null).invoke(reflectiveObject, (Object[]) null);

            return result;
         }
      }
      catch (InvocationTargetException inex)
      {
         throw(Exception) inex.getTargetException();
      }

      return null;
   }

   public static void main(String[] args)
   {
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/InputNode.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import javafx.scene.Node;

public interface InputNode
{
   /**
    * Creates a linked duplicate of the main input node.
    */
   Node getSimpleInputNode(double width, double height);

   /**
    * Retries the full tuning node with all additional options such as min and max value.
    */
   Node getFullInputNode();

   /**
    * Sets the value of the parameter associated with this input node based on the provided
    * percent value.
    */
   void setValueFromPercent(double percent);

   /**
    * Gets the value of the parameter as a percent value.
    */
   double getValuePercent();
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/TurnStraightTurnOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FrameOrientation2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.robotics.geometry.AngleTools;

public class TurnStraightTurnOverheadPath extends CompositeOverheadPath
{
   private TurningOverheadPath turningPath;
   private StraightLineOverheadPath straightLinePath;
   private TurningOverheadPath endTurningPath;
   private final static double defaultNoTranslationTolerance = 1e-14;

   public TurnStraightTurnOverheadPath(FramePose2D startPose, FramePose2D endPose, double headingOffset)
   {
      this(startPose, endPose, headingOffset, defaultNoTranslationTolerance);
   }

   public TurnStraightTurnOverheadPath(FramePose2D startPose, FramePose2D endPose, double headingOffset, double noTranslationTolerance)
   {
      super(calculatePaths(startPose, endPose, headingOffset, noTranslationTolerance));
      turningPath = (TurningOverheadPath) this.paths.get(0);
      straightLinePath = (StraightLineOverheadPath) this.paths.get(1);
      endTurningPath = (TurningOverheadPath) this.paths.get(2);
   }

   private static List<OverheadPath> calculatePaths(FramePose2D startPose, FramePose2D endPose, double headingOffset, double noTranslationTolerance)
   {
      startPose.checkReferenceFrameMatch(endPose);
      FramePoint2D endPosition = new FramePoint2D(endPose.getPosition());
      double heading = AngleTools.calculateHeading(startPose, endPosition, headingOffset, noTranslationTolerance);
      FrameOrientation2D intermediateOrientation = new FrameOrientation2D(startPose.getReferenceFrame(), heading);
      TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation);
      FramePose2D intermediatePose = turningPath.getPoseAtS(1.0);
      StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPosition);
      intermediatePose = straightPath.getPoseAtS(1.0);
      FrameOrientation2D endOrientation = new FrameOrientation2D(endPose.getOrientation());
      TurningOverheadPath endTurningPath = new TurningOverheadPath(intermediatePose, endOrientation);

      List<OverheadPath> paths = new ArrayList<OverheadPath>();
      paths.add(turningPath);
      paths.add(straightPath);
      paths.add(endTurningPath);

      return paths;
   }

   public double getSignedTurningAmountInitialTurn()
   {
      return turningPath.getDeltaYaw();
   }

   public double getSignedTurningAmountFinalTurn()
   {
      return endTurningPath.getDeltaYaw();
   }

   public double getDistance()
   {
      return straightLinePath.getDistance();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/StateMachineTools.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.yoVariables.variable.YoEnum;

/**
 * @author twan
 *         Date: 5/30/13
 */
@Deprecated
public class StateMachineTools
{
   @SafeVarargs
   public static <T extends Enum<T>> void addRequestedStateTransition(final YoEnum<T> requestedState, boolean waitUntilDone,
           final FinishableState<T> initialState, final FinishableState<T>... statePath)
   {
      addRequestedStateTransition(requestedState, waitUntilDone, new ArrayList<StateTransitionAction>(), initialState, statePath);
   }

   @SafeVarargs
   public static <T extends Enum<T>> void addRequestedStateTransition(final YoEnum<T> requestedState, boolean waitUntilDone,
         StateTransitionAction stateTransitionAction, final FinishableState<T> initialState, final FinishableState<T>... statePath)
   {
      addRequestedStateTransition(requestedState, waitUntilDone, createListWithOneElement(stateTransitionAction), initialState, statePath);
   }

   @SafeVarargs
   public static <T extends Enum<T>> void addRequestedStateTransition(final YoEnum<T> requestedState, boolean waitUntilDone,
           List<? extends StateTransitionAction> stateTransitionActions, final FinishableState<T> initialState, final FinishableState<T>... statePath)
   {
      final State<T> finalState = statePath[statePath.length - 1];
      FinishableState<T> fromState = initialState;

      List<StateTransitionAction> stateTransitionActionsCopy = new ArrayList<StateTransitionAction>(stateTransitionActions);

      for (FinishableState<T> toState : statePath)
      {
         StateTransitionCondition stateTransitionCondition = new RequestedStateTransitionCondition<T>(fromState, finalState.getStateEnum(), requestedState,
                                                                waitUntilDone);

         StateTransition<T> transition;
         if (toState == finalState)
         {
            StateTransitionAction stateTransitionAction = new StateTransitionAction()
            {
               public void doTransitionAction()
               {
                  requestedState.set(null);
               }
            };
            stateTransitionActionsCopy.add(stateTransitionAction);
         }

         transition = new StateTransition<T>(toState.getStateEnum(), stateTransitionCondition, stateTransitionActionsCopy);


         fromState.addStateTransition(transition);

         fromState = toState;
      }
   }

   public static <T extends Enum<T>> StateTransition<T> buildRequestableStateTransition(final YoEnum<T> requestedState, final T finalStateEnum)
   {
      return buildRequestableStateTransition(requestedState, new ArrayList<>(), finalStateEnum);
   }

   public static <T extends Enum<T>> StateTransition<T> buildRequestableStateTransition(final YoEnum<T> requestedState,
                                                                                      List<? extends StateTransitionAction> stateTransitionActions,
                                                                                      final T finalStateEnum)
   {
      List<StateTransitionAction> stateTransitionActionsCopy = new ArrayList<>(stateTransitionActions);
      StateTransitionCondition stateTransitionCondition = new RequestedStateTransitionCondition<T>(finalStateEnum, requestedState);
      StateTransitionAction stateTransitionAction = new StateTransitionAction()
      {
         public void doTransitionAction()
         {
            requestedState.set(null);
         }
      };
      stateTransitionActionsCopy.add(stateTransitionAction);

      return new StateTransition<T>(finalStateEnum, stateTransitionCondition, stateTransitionActionsCopy);
   }

   public static <T extends Enum<T>> StateTransition<T> buildFinishedStateTransition(final FinishableState<T> initialState, final T finalStateEnum)
   {
      return buildFinishedStateTransition(initialState, new ArrayList<>(), finalStateEnum);
   }

   public static <T extends Enum<T>> StateTransition<T> buildFinishedStateTransition(final FinishableState<T> initialState,
                                                                                     List<? extends StateTransitionAction> stateTransitionActions,
                                                                                     final T finalStateEnum)
   {
      List<StateTransitionAction> stateTransitionActionsCopy = new ArrayList<>(stateTransitionActions);
      StateTransitionCondition stateTransitionCondition = new FinishedStateTransitionCondition<>(initialState, finalStateEnum);
      StateTransitionAction stateTransitionAction = new StateTransitionAction()
      {
         public void doTransitionAction()
         {
         }
      };
      stateTransitionActionsCopy.add(stateTransitionAction);

      return new StateTransition<T>(finalStateEnum, stateTransitionCondition, stateTransitionActionsCopy);
   }

   private static class RequestedStateTransitionCondition<T extends Enum<T>> implements StateTransitionCondition
   {
      private final FinishableState<T> fromState;
      private final T requestedStateTrigger;
      private final boolean waitUntilDone;
      private final YoEnum<T> requestedState;

      public RequestedStateTransitionCondition(T requestedStateTrigger, YoEnum<T> requestedState)
      {
         this(null, requestedStateTrigger, requestedState, false);
      }

      public RequestedStateTransitionCondition(FinishableState<T> fromState, T requestedStateTrigger, YoEnum<T> requestedState, boolean waitUntilDone)
      {
         this.fromState = fromState;
         this.requestedStateTrigger = requestedStateTrigger;
         this.waitUntilDone = waitUntilDone;
         this.requestedState = requestedState;
      }

      public boolean checkCondition()
      {
         boolean done = !waitUntilDone || fromState.isDone();
         boolean transitionRequested = requestedStateTrigger.equals(requestedState.getEnumValue());

         return done && transitionRequested;
      }
   }

   private static class FinishedStateTransitionCondition<T extends Enum<T>> implements StateTransitionCondition
   {
      private final FinishableState<T> fromState;
      private final T nextState;

      public FinishedStateTransitionCondition(FinishableState<T> fromState, T nextState)
      {
         this.fromState = fromState;
         this.nextState = nextState;
      }

      public boolean checkCondition()
      {
         return fromState.isDone();
      }
   }

   private static <T> List<T> createListWithOneElement(T object)
   {
      ArrayList<T> ret = new ArrayList<>();
      ret.add(object);
      return ret;
   }
}


<|start_filename|>websitedocs/website/node_modules/feed/package.json<|end_filename|>
{
  "_from": "feed@^1.1.0",
  "_id": "feed@1.1.1",
  "_inBundle": false,
  "_integrity": "sha1-kUiXUX6U+jJ8xvc7tYWkfEqe0yE=",
  "_location": "/feed",
  "_phantomChildren": {},
  "_requested": {
    "type": "range",
    "registry": true,
    "raw": "feed@^1.1.0",
    "name": "feed",
    "escapedName": "feed",
    "rawSpec": "^1.1.0",
    "saveSpec": null,
    "fetchSpec": "^1.1.0"
  },
  "_requiredBy": [
    "/docusaurus"
  ],
  "_resolved": "https://registry.npmjs.org/feed/-/feed-1.1.1.tgz",
  "_shasum": "914897517e94fa327cc6f73bb585a47c4a9ed321",
  "_spec": "feed@^1.1.0",
  "_where": "C:\\Users\\tzheng\\ihmcstash\\ihmc-open-robotics-software\\websitedocs\\website\\node_modules\\docusaurus",
  "author": {
    "name": "<NAME>",
    "email": "<EMAIL>"
  },
  "bugs": {
    "url": "https://github.com/jpmonette/feed/issues"
  },
  "bundleDependencies": false,
  "contributors": [
    {
      "name": "<NAME>",
      "email": "<EMAIL>"
    },
    {
      "name": "<NAME>",
      "email": "<EMAIL>"
    }
  ],
  "dependencies": {
    "xml": "^1.0.1"
  },
  "deprecated": false,
  "description": "Feed is a RSS and Atom feed generator for Node.js, making content syndication simple and intuitive!",
  "devDependencies": {
    "babel-cli": "^6.24.1",
    "babel-preset-env": "^1.5.1",
    "codeclimate-test-reporter": "^0.5.0",
    "coveralls": "^2.13.1",
    "jest": "^20.0.4"
  },
  "engines": {
    "node": ">=0.4.0"
  },
  "homepage": "http://projets.jpmonette.net/en/feed",
  "jest": {
    "verbose": true,
    "collectCoverage": true,
    "collectCoverageFrom": [
      "**/src/*.{js}"
    ],
    "testMatch": [
      "**/*.spec.js"
    ]
  },
  "keywords": [
    "rss",
    "atom",
    "feed",
    "syndication",
    "xml",
    "wrapper",
    "blog"
  ],
  "license": "MIT",
  "main": "lib/feed.js",
  "name": "feed",
  "repository": {
    "type": "git",
    "url": "git+https://github.com/jpmonette/feed.git"
  },
  "scripts": {
    "build": "rm -rf lib/ && mkdir lib && babel -d lib/ src/ --ignore **/*.spec.js -s",
    "prepublish": "npm run build",
    "test": "export NODE_ENV=test && jest",
    "test-travis": "export NODE_ENV=test && jest --coverage"
  },
  "version": "1.1.1"
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/modules/uiConnector/PacketsForwardedToTheUi.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.modules.uiConnector;

import java.util.HashMap;

import controller_msgs.msg.dds.ArmTrajectoryMessage;
import controller_msgs.msg.dds.BDIBehaviorStatusPacket;
import controller_msgs.msg.dds.BehaviorControlModeResponsePacket;
import controller_msgs.msg.dds.BehaviorStatusPacket;
import controller_msgs.msg.dds.BlackFlyParameterPacket;
import controller_msgs.msg.dds.BoundingBoxesPacket;
import controller_msgs.msg.dds.CapturabilityBasedStatus;
import controller_msgs.msg.dds.ChestTrajectoryMessage;
import controller_msgs.msg.dds.ControllerCrashNotificationPacket;
import controller_msgs.msg.dds.DetectedObjectPacket;
import controller_msgs.msg.dds.DoorLocationPacket;
import controller_msgs.msg.dds.DrillDetectionPacket;
import controller_msgs.msg.dds.FisheyePacket;
import controller_msgs.msg.dds.FootstepDataListMessage;
import controller_msgs.msg.dds.FootstepPlanningToolboxOutputStatus;
import controller_msgs.msg.dds.FootstepStatusMessage;
import controller_msgs.msg.dds.HandJointAnglePacket;
import controller_msgs.msg.dds.HandTrajectoryMessage;
import controller_msgs.msg.dds.HeadTrajectoryMessage;
import controller_msgs.msg.dds.HeatMapPacket;
import controller_msgs.msg.dds.HeightQuadTreeMessage;
import controller_msgs.msg.dds.InvalidPacketNotificationPacket;
import controller_msgs.msg.dds.KinematicsToolboxOutputStatus;
import controller_msgs.msg.dds.LidarScanMessage;
import controller_msgs.msg.dds.LocalizationPointMapPacket;
import controller_msgs.msg.dds.ManipulationAbortedStatus;
import controller_msgs.msg.dds.NeckTrajectoryMessage;
import controller_msgs.msg.dds.PelvisHeightTrajectoryMessage;
import controller_msgs.msg.dds.PelvisOrientationTrajectoryMessage;
import controller_msgs.msg.dds.PelvisPoseErrorPacket;
import controller_msgs.msg.dds.PelvisTrajectoryMessage;
import controller_msgs.msg.dds.PlanarRegionsListMessage;
import controller_msgs.msg.dds.PointCloudWorldPacket;
import controller_msgs.msg.dds.RobotConfigurationData;
import controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket;
import controller_msgs.msg.dds.SnapFootstepPacket;
import controller_msgs.msg.dds.StereoVisionPointCloudMessage;
import controller_msgs.msg.dds.TextToSpeechPacket;
import controller_msgs.msg.dds.UIPositionCheckerPacket;
import controller_msgs.msg.dds.ValveLocationPacket;
import controller_msgs.msg.dds.VideoPacket;
import controller_msgs.msg.dds.WalkingStatusMessage;
import controller_msgs.msg.dds.WholeBodyTrajectoryMessage;
import controller_msgs.msg.dds.WholeBodyTrajectoryToolboxOutputStatus;

public class PacketsForwardedToTheUi
{
   public static final long UI_JOINT_CONFIGURATION_UPDATE_MILLIS = 100l;
   public static final long UI_WRIST_FEET_SENSORS_UPDATE_MILLIS = 500l;
   public static final long UI_MULTISENSE_IMU_CHECK_MILLIS=5000l;

   public final static Class<?>[] PACKETS_ALLOWED_TO_BE_SENT_TO_THE_USER_INTERFACE = {
      FootstepStatusMessage.class,
      PelvisPoseErrorPacket.class,
      BehaviorControlModeResponsePacket.class,
      BDIBehaviorStatusPacket.class,
      FootstepDataListMessage.class,
      PelvisHeightTrajectoryMessage.class,
      HeadTrajectoryMessage.class,
      NeckTrajectoryMessage.class,
      PelvisTrajectoryMessage.class,
      PelvisOrientationTrajectoryMessage.class,
      ChestTrajectoryMessage.class,
      SnapFootstepPacket.class,
      VideoPacket.class,
      HandTrajectoryMessage.class,
      ArmTrajectoryMessage.class,
      ValveLocationPacket.class,
      BehaviorStatusPacket.class,
      DoorLocationPacket.class,
      PointCloudWorldPacket.class,
      HandJointAnglePacket.class,
      WholeBodyTrajectoryMessage.class,
      ControllerCrashNotificationPacket.class,
      InvalidPacketNotificationPacket.class,
      DetectedObjectPacket.class,
      FisheyePacket.class,
      SimpleCoactiveBehaviorDataPacket.class,
      LocalizationPointMapPacket.class,
      BlackFlyParameterPacket.class,
      DrillDetectionPacket.class,
      ManipulationAbortedStatus.class,
      KinematicsToolboxOutputStatus.class,
      WholeBodyTrajectoryToolboxOutputStatus.class,
      FootstepPlanningToolboxOutputStatus.class,
      TextToSpeechPacket.class,
      UIPositionCheckerPacket.class,
      PlanarRegionsListMessage.class,
      HeightQuadTreeMessage.class,
      LidarScanMessage.class,
      HeatMapPacket.class,
      BoundingBoxesPacket.class,
      WalkingStatusMessage.class,
      StereoVisionPointCloudMessage.class
   };

   public static final HashMap<Class<?>, Long> PACKETS_ALLOWED_TO_BE_SENT_TO_THE_USER_INTERFACE_WITH_MINIMAL_INTERVALS = new HashMap<Class<?>, Long>();
   static {
//      PACKETS_ALLOWED_TO_BE_SENT_TO_THE_USER_INTERFACE_WITH_MINIMAL_INTERVALS.put(RobotPoseData.class, UI_JOINT_CONFIGURATION_UPDATE_MILLIS);
      PACKETS_ALLOWED_TO_BE_SENT_TO_THE_USER_INTERFACE_WITH_MINIMAL_INTERVALS.put(CapturabilityBasedStatus.class, UI_JOINT_CONFIGURATION_UPDATE_MILLIS);
      PACKETS_ALLOWED_TO_BE_SENT_TO_THE_USER_INTERFACE_WITH_MINIMAL_INTERVALS.put(RobotConfigurationData.class, UI_JOINT_CONFIGURATION_UPDATE_MILLIS);
   }


}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/HoldPositionControllerState.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates;

import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters;
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputBasics;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputReadOnly;
import us.ihmc.tools.lists.PairList;
import us.ihmc.yoVariables.variable.YoDouble;

public class HoldPositionControllerState extends HighLevelControllerState
{
   private final JointDesiredOutputListReadOnly highLevelControllerOutput;
   private final LowLevelOneDoFJointDesiredDataHolder lowLevelOneDoFJointDesiredDataHolder = new LowLevelOneDoFJointDesiredDataHolder();

   private final PairList<OneDoFJointBasics, YoDouble> jointSetpoints = new PairList<>();

   public HoldPositionControllerState(HighLevelControllerName stateEnum, OneDoFJointBasics[] controlledJoints,
                                      HighLevelControllerParameters highLevelControllerParameters, JointDesiredOutputListReadOnly highLevelControllerOutput)
   {
      super(stateEnum, highLevelControllerParameters, controlledJoints);

      this.highLevelControllerOutput = highLevelControllerOutput;
      String nameSuffix = "_" + stateEnum.name();

      for (OneDoFJointBasics controlledJoint : controlledJoints)
      {
         String jointName = controlledJoint.getName();

         YoDouble freezePosition = new YoDouble(jointName + nameSuffix + "_qDesired", registry);
         freezePosition.setToNaN();

         jointSetpoints.add(controlledJoint, freezePosition);
      }

      lowLevelOneDoFJointDesiredDataHolder.registerJointsWithEmptyData(controlledJoints);
   }

   public void setToCurrent()
   {
      for (int jointIndex = 0; jointIndex < jointSetpoints.size(); jointIndex++)
      {
         OneDoFJointBasics joint = jointSetpoints.get(jointIndex).getLeft();
         YoDouble setpoint = jointSetpoints.get(jointIndex).getRight();
         setpoint.set(joint.getQ());
      }
   }

   @Override
   public void onEntry()
   {
      for (int jointIndex = 0; jointIndex < jointSetpoints.size(); jointIndex++)
      {
         OneDoFJointBasics joint = jointSetpoints.get(jointIndex).getLeft();
         YoDouble setpoint = jointSetpoints.get(jointIndex).getRight();
         JointDesiredOutputReadOnly lowLevelJointData = highLevelControllerOutput.getJointDesiredOutput(joint);
         if (lowLevelJointData != null && lowLevelJointData.hasDesiredPosition())
            setpoint.set(lowLevelJointData.getDesiredPosition());
         else
            setpoint.set(joint.getQ());
      }
   }

   @Override
   public void doAction(double timeInState)
   {
      for (int jointIndex = 0; jointIndex < jointSetpoints.size(); jointIndex++)
      {
         OneDoFJointBasics joint = jointSetpoints.get(jointIndex).getLeft();
         YoDouble desiredPosition = jointSetpoints.get(jointIndex).getRight();

         JointDesiredOutputBasics lowLevelJointData = lowLevelOneDoFJointDesiredDataHolder.getJointDesiredOutput(joint);
         lowLevelJointData.clear();
         lowLevelJointData.setDesiredPosition(desiredPosition.getDoubleValue());
         lowLevelJointData.setDesiredVelocity(0.0);
         lowLevelJointData.setDesiredAcceleration(0.0);
      }

      lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings());
   }

   @Override
   public void onExit()
   {
      // Do nothing

   }

   @Override
   public JointDesiredOutputListReadOnly getOutputForLowLevelController()
   {
      return lowLevelOneDoFJointDesiredDataHolder;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/CenterOfMassDataHolderReadOnly.java<|end_filename|>
package us.ihmc.robotics.sensors;

import us.ihmc.euclid.referenceFrame.FrameVector3D;

public interface CenterOfMassDataHolderReadOnly
{
   public abstract void getCenterOfMassVelocity(FrameVector3D centerOfMassVelocityToPack);

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/CenterOfMassTrajectoryMessage.java<|end_filename|>
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.
       * Request the whole-body controller to track a desired trajectory for the center of mass.
       */
public class CenterOfMassTrajectoryMessage extends Packet<CenterOfMassTrajectoryMessage> implements Settable<CenterOfMassTrajectoryMessage>, EpsilonComparable<CenterOfMassTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * List of center of mass trajectory waypoints. Each waypoint contains the center of mass position and velocity at a given time.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage euclidean_trajectory_;

   public CenterOfMassTrajectoryMessage()
   {
      euclidean_trajectory_ = new controller_msgs.msg.dds.EuclideanTrajectoryMessage();
   }

   public CenterOfMassTrajectoryMessage(CenterOfMassTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(CenterOfMassTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.staticCopy(other.euclidean_trajectory_, euclidean_trajectory_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * List of center of mass trajectory waypoints. Each waypoint contains the center of mass position and velocity at a given time.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage getEuclideanTrajectory()
   {
      return euclidean_trajectory_;
   }


   public static Supplier<CenterOfMassTrajectoryMessagePubSubType> getPubSubType()
   {
      return CenterOfMassTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return CenterOfMassTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(CenterOfMassTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.euclidean_trajectory_.epsilonEquals(other.euclidean_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof CenterOfMassTrajectoryMessage)) return false;

      CenterOfMassTrajectoryMessage otherMyClass = (CenterOfMassTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.euclidean_trajectory_.equals(otherMyClass.euclidean_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("CenterOfMassTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("euclidean_trajectory=");
      builder.append(this.euclidean_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/walking/FootstepPlanRequestType.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.walking;

public enum FootstepPlanRequestType
{
   START_SEARCH, STOP_SEARCH, UPDATE_START;

   public static final FootstepPlanRequestType[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static FootstepPlanRequestType fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/ReachabilityMapTools.java<|end_filename|>
package us.ihmc.avatar.reachabilityMap;

import java.awt.Color;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.MeshDataGenerator;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;

public class ReachabilityMapTools
{

   public static Graphics3DObject createBoundingBoxGraphics(FramePoint3DReadOnly min, FramePoint3DReadOnly max)
   {
      double width = 0.01;
      AppearanceDefinition appearance = YoAppearance.LightBlue();
      Graphics3DObject boundingBox = new Graphics3DObject();
      FramePoint3D modifiableMin = new FramePoint3D(min);
      modifiableMin.changeFrame(ReferenceFrame.getWorldFrame());
      FramePoint3D modifiableMax = new FramePoint3D(max);
      modifiableMax.changeFrame(ReferenceFrame.getWorldFrame());
      double x0 = modifiableMin.getX();
      double y0 = modifiableMin.getY();
      double z0 = modifiableMin.getZ();
      double x1 = modifiableMax.getX();
      double y1 = modifiableMax.getY();
      double z1 = modifiableMax.getZ();
      // The three segments originating from min
      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y0, z0, x1, y0, z0, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y0, z0, x0, y1, z0, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y0, z0, x0, y0, z1, width), appearance);
      // The three segments originating from min
      boundingBox.addMeshData(MeshDataGenerator.Line(x1, y1, z1, x0, y1, z1, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x1, y1, z1, x1, y0, z1, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x1, y1, z1, x1, y1, z0, width), appearance);

      boundingBox.addMeshData(MeshDataGenerator.Line(x1, y0, z0, x1, y1, z0, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x1, y0, z0, x1, y0, z1, width), appearance);

      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y1, z0, x1, y1, z0, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y1, z0, x0, y1, z1, width), appearance);

      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y0, z1, x1, y0, z1, width), appearance);
      boundingBox.addMeshData(MeshDataGenerator.Line(x0, y0, z1, x0, y1, z1, width), appearance);

      return boundingBox;
   }

   public static Graphics3DObject createReachibilityColorScale()
   {
      Graphics3DObject voxelViz = new Graphics3DObject();
      double maxReachability = 0.7;
      double resolution = 0.1;
      voxelViz.translate(-1.0, -1.0, 0.0);

      for (double z = 0; z <= maxReachability; z += maxReachability * resolution)
      {
         AppearanceDefinition appearance = YoAppearance.RGBColorFromHex(Color.HSBtoRGB((float) z, 1.0f, 1.0f));
         voxelViz.translate(0.0, 0.0, resolution);
         voxelViz.addSphere(0.025, appearance);
      }

      return voxelViz;
   }
}


<|start_filename|>ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/YawDriftProvider.java<|end_filename|>
package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation;

public interface YawDriftProvider
{
   public double getYawBiasInWorldFrame();

   public void update();
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/CommandExecutionTimeComparator.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController;

import us.ihmc.communication.controllerAPI.command.Command;

import java.util.Comparator;

/**
 * Compares the execution times of two commands
 */
public class CommandExecutionTimeComparator implements Comparator<Command<?,?>>
{
   
   /**
    * compares the execution time of {@code commandA} command with {@code commandB }. 
    * @return If commandA's execution time is less than commandB's it returns -1
    * If commandA's execution time is greater than commandB's it returns 1
    * If they are equal it returns 0
    * 
    */
   @Override
   public int compare(Command<?, ?> commandA, Command<?, ?> commandB)
   {
      double diff = commandA.getExecutionTime() - commandB.getExecutionTime();
      
      if(diff < 0)
      {
         return -1;
      }
      
      if(diff > 0)
      {
         return 1;
      }
      
      return 0;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/FrameVertex2DSupplierList.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier;

public class FrameVertex2DSupplierList<T extends FramePoint2DReadOnly> extends Vertex2DSupplierList<T> implements FrameVertex2DSupplier
{
   @Override
   public T getVertex(int index)
   {
      return super.getVertex(index);
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/FootContactPoints.java<|end_filename|>
package us.ihmc.wholeBodyController;

import java.util.List;
import java.util.Map;

import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.robotics.partNames.LeggedJointNameMap;
import us.ihmc.robotics.robotSide.RobotSegment;
import us.ihmc.robotics.robotSide.SegmentDependentList;

public interface FootContactPoints<E extends Enum<E> & RobotSegment<E>>
{
   /**
    * Creates the contact points used by the simulation. Not necessarily limited to the feet. A map is returned that
    * can hold contact points for each rigid body. The key of the map is the name of the parent joint of that body.
    * The contact points are expected in body frame.
    *
    * @param footLength
    * @param footWidth
    * @param toeWidth
    * @param jointMap
    * @param soleToAnkleFrameTransforms
    * @return
    */
   public Map<String, List<Tuple3DBasics>> getSimulationContactPoints(double footLength, double footWidth, double toeWidth, LeggedJointNameMap<E> jointMap,
         SegmentDependentList<E, RigidBodyTransform> soleToAnkleFrameTransforms);

   /**
    * Creates contact points to be used inside the controller. The points are 2d and expected in sole frame (aligned
    * with the foot plane). A set of contact points has to be specified for each side of the robot.
    *
    * @param footLength
    * @param footWidth
    * @param toeWidth
    * @return
    */
   public SegmentDependentList<E, List<Tuple2DBasics>> getControllerContactPoints(double footLength, double footWidth, double toeWidth);

   /**
    * Creates the toe off contact point used by the controller for each robot side. The point is in 2d and expected
    * in sole frame.
    *
    * @param footLength
    * @param footWidth
    * @param toeWidth
    * @return
    */
   public SegmentDependentList<E, Tuple2DBasics> getToeOffContactPoints(double footLength, double footWidth, double toeWidth);

   /**
    * Creates the toe off contact line used by the controller for each robot side. The line is in 2d and expected
    * in sole frame.
    *
    * @param footLength
    * @param footWidth
    * @param toeWidth
    * @return
    */
   public SegmentDependentList<E, LineSegment2D> getToeOffContactLines(double footLength, double footWidth, double toeWidth);

   /**
    * Allows switching between different sets of ground parameters.
    *
    * @return
    */
   public boolean useSoftContactPointParameters();
}

<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/GuiParameterStatus.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements;

import us.ihmc.yoVariables.parameters.ParameterLoadStatus;

public enum GuiParameterStatus
{
   ANY,
   DEFAULT,
   LOADED,
   MODIFIED;

   public static GuiParameterStatus get(ParameterLoadStatus parameterLoadStatus)
   {
      switch (parameterLoadStatus)
      {
      case DEFAULT:
         return GuiParameterStatus.DEFAULT;
      case LOADED:
         return GuiParameterStatus.LOADED;
      default:
         throw new RuntimeException("Unknown parameter load status: " + parameterLoadStatus);
      }
   }

   public boolean matches(GuiParameter parameter)
   {
      return matches(parameter.getStatus());
   }

   public boolean matches(GuiParameterStatus other)
   {
      return this == ANY || other == this;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WaypointBasedTrajectoryMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "WaypointBasedTrajectoryMessage" defined in "WaypointBasedTrajectoryMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from WaypointBasedTrajectoryMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit WaypointBasedTrajectoryMessage_.idl instead.
*
*/
public class WaypointBasedTrajectoryMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.WaypointBasedTrajectoryMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::WaypointBasedTrajectoryMessage_";

   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.WaypointBasedTrajectoryMessage 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.WaypointBasedTrajectoryMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += geometry_msgs.msg.dds.QuaternionPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getWaypointTimes().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getWaypoints().size(); ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getWaypoints().get(i0), current_alignment);}

      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getCdrSerializedSize(data.getAngularSelectionMatrix(), current_alignment);

      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getCdrSerializedSize(data.getLinearSelectionMatrix(), current_alignment);

      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getControlFramePositionInEndEffector(), current_alignment);

      current_alignment += geometry_msgs.msg.dds.QuaternionPubSubType.getCdrSerializedSize(data.getControlFrameOrientationInEndEffector(), current_alignment);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_2(data.getEndEffectorHashCode());

      if(data.getWaypointTimes().size() <= 100)
      cdr.write_type_e(data.getWaypointTimes());else
          throw new RuntimeException("waypoint_times field exceeds the maximum length");

      if(data.getWaypoints().size() <= 100)
      cdr.write_type_e(data.getWaypoints());else
          throw new RuntimeException("waypoints field exceeds the maximum length");

      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.write(data.getAngularSelectionMatrix(), cdr);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.write(data.getLinearSelectionMatrix(), cdr);
      geometry_msgs.msg.dds.PointPubSubType.write(data.getControlFramePositionInEndEffector(), cdr);
      geometry_msgs.msg.dds.QuaternionPubSubType.write(data.getControlFrameOrientationInEndEffector(), cdr);
      cdr.write_type_6(data.getWeight());

   }

   public static void read(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setEndEffectorHashCode(cdr.read_type_2());
      	
      cdr.read_type_e(data.getWaypointTimes());	
      cdr.read_type_e(data.getWaypoints());	
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.read(data.getAngularSelectionMatrix(), cdr);	
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.read(data.getLinearSelectionMatrix(), cdr);	
      geometry_msgs.msg.dds.PointPubSubType.read(data.getControlFramePositionInEndEffector(), cdr);	
      geometry_msgs.msg.dds.QuaternionPubSubType.read(data.getControlFrameOrientationInEndEffector(), cdr);	
      data.setWeight(cdr.read_type_6());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_2("end_effector_hash_code", data.getEndEffectorHashCode());
      ser.write_type_e("waypoint_times", data.getWaypointTimes());
      ser.write_type_e("waypoints", data.getWaypoints());
      ser.write_type_a("angular_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getAngularSelectionMatrix());

      ser.write_type_a("linear_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getLinearSelectionMatrix());

      ser.write_type_a("control_frame_position_in_end_effector", new geometry_msgs.msg.dds.PointPubSubType(), data.getControlFramePositionInEndEffector());

      ser.write_type_a("control_frame_orientation_in_end_effector", new geometry_msgs.msg.dds.QuaternionPubSubType(), data.getControlFrameOrientationInEndEffector());

      ser.write_type_6("weight", data.getWeight());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setEndEffectorHashCode(ser.read_type_2("end_effector_hash_code"));
      ser.read_type_e("waypoint_times", data.getWaypointTimes());
      ser.read_type_e("waypoints", data.getWaypoints());
      ser.read_type_a("angular_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getAngularSelectionMatrix());

      ser.read_type_a("linear_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getLinearSelectionMatrix());

      ser.read_type_a("control_frame_position_in_end_effector", new geometry_msgs.msg.dds.PointPubSubType(), data.getControlFramePositionInEndEffector());

      ser.read_type_a("control_frame_orientation_in_end_effector", new geometry_msgs.msg.dds.QuaternionPubSubType(), data.getControlFrameOrientationInEndEffector());

      data.setWeight(ser.read_type_6("weight"));
   }

   public static void staticCopy(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage src, controller_msgs.msg.dds.WaypointBasedTrajectoryMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.WaypointBasedTrajectoryMessage createData()
   {
      return new controller_msgs.msg.dds.WaypointBasedTrajectoryMessage();
   }
   @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.WaypointBasedTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.WaypointBasedTrajectoryMessage src, controller_msgs.msg.dds.WaypointBasedTrajectoryMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public WaypointBasedTrajectoryMessagePubSubType newInstance()
   {
      return new WaypointBasedTrajectoryMessagePubSubType();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/IHMCROSTranslationRuntimeTools.java<|end_filename|>
package us.ihmc.avatar.ros;

import java.lang.reflect.InvocationTargetException;
import java.util.ArrayList;
import java.util.List;

import org.ros.internal.message.Message;
import org.ros.message.MessageFactory;

import controller_msgs.msg.dds.ArmTrajectoryMessage;
import controller_msgs.msg.dds.ChestTrajectoryMessage;
import controller_msgs.msg.dds.FootTrajectoryMessage;
import controller_msgs.msg.dds.FootstepDataListMessage;
import controller_msgs.msg.dds.FootstepDataMessage;
import controller_msgs.msg.dds.FrameInformation;
import controller_msgs.msg.dds.HandTrajectoryMessage;
import controller_msgs.msg.dds.HeadTrajectoryMessage;
import controller_msgs.msg.dds.PelvisTrajectoryMessage;
import controller_msgs.msg.dds.QueueableMessage;
import controller_msgs.msg.dds.SE3TrajectoryPointMessage;
import controller_msgs.msg.dds.WholeBodyTrajectoryMessage;
import geometry_msgs.Point;
import ihmc_msgs.ArmTrajectoryRosMessage;
import ihmc_msgs.ChestTrajectoryRosMessage;
import ihmc_msgs.FootTrajectoryRosMessage;
import ihmc_msgs.FootstepDataListRosMessage;
import ihmc_msgs.FootstepDataRosMessage;
import ihmc_msgs.FrameInformationRosMessage;
import ihmc_msgs.HandTrajectoryRosMessage;
import ihmc_msgs.HeadTrajectoryRosMessage;
import ihmc_msgs.PelvisTrajectoryRosMessage;
import ihmc_msgs.Point2dRosMessage;
import ihmc_msgs.QueueableRosMessage;
import ihmc_msgs.SE3TrajectoryPointRosMessage;
import ihmc_msgs.WholeBodyTrajectoryRosMessage;
import us.ihmc.communication.packets.MessageTools;
import us.ihmc.communication.packets.Packet;
import us.ihmc.communication.ros.generators.RosMessagePacket;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.utilities.ros.msgToPacket.converter.CustomFieldConversions;
import us.ihmc.utilities.ros.msgToPacket.converter.GenericROSTranslationTools;
import us.ihmc.utilities.ros.msgToPacket.converter.RosEnumConversionException;

/**
 * Provides generic and custom ROS<->Java translations.
 * <p>
 * If the {@link FootstepDataListMessage} or {@link WholeBodyTrajectoryMessage} definitions change,
 * you will need to update their custom translations here.
 * </p>
 * 
 */
public class IHMCROSTranslationRuntimeTools
{
   private static final MessageFactory messageFactory = GenericROSTranslationTools.getMessageFactory();
   private static final CustomFieldConversions customFieldConversions = CustomFieldConversions.getInstance();
   static
   {
      customFieldConversions.registerIHMCPacketFieldConverter(FootstepDataListMessage.class, IHMCROSTranslationRuntimeTools::customConvertToRosMessage);
      customFieldConversions.registerROSMessageFieldConverter(FootstepDataListRosMessage.class, IHMCROSTranslationRuntimeTools::customConvertToIHMCMessage);

      customFieldConversions.registerIHMCPacketFieldConverter(FootstepDataMessage.class, IHMCROSTranslationRuntimeTools::customConvertToRosMessage);
      customFieldConversions.registerROSMessageFieldConverter(FootstepDataRosMessage.class, IHMCROSTranslationRuntimeTools::customConvertToIHMCMessage);

      customFieldConversions.registerIHMCPacketFieldConverter(FrameInformation.class, IHMCROSTranslationRuntimeTools::convertFrameInformation);
      customFieldConversions.registerROSMessageFieldConverter(FrameInformationRosMessage.class,
                                                              IHMCROSTranslationRuntimeTools::convertFrameInformationRosMessage);

      customFieldConversions.registerIHMCPacketFieldConverter(WholeBodyTrajectoryMessage.class, IHMCROSTranslationRuntimeTools::customConvertToRosMessage);
      customFieldConversions.registerROSMessageFieldConverter(WholeBodyTrajectoryRosMessage.class, IHMCROSTranslationRuntimeTools::customConvertToIHMCMessage);
   }

   public static Message convertToRosMessage(Packet<?> ihmcMessage)
         throws InvocationTargetException, IllegalAccessException, NoSuchMethodException, ClassNotFoundException
   {
      if (ihmcMessage == null)
      {
         return null;
      }
      Class<? extends Packet> aClass = ihmcMessage.getClass();

      if (customFieldConversions.containsConverterFor(aClass))
      {
         return customFieldConversions.convert(ihmcMessage);
      }
      else
      {
         return GenericROSTranslationTools.convertIHMCMessageToRosMessage(ihmcMessage);
      }
   }

   public static Packet<?> convertToIHMCMessage(Message rosMessage) throws ClassNotFoundException, InvocationTargetException, IllegalAccessException,
         RosEnumConversionException, NoSuchFieldException, InstantiationException, IllegalArgumentException, NoSuchMethodException, SecurityException
   {
      if (rosMessage == null)
      {
         return null;
      }
      Class<?> aClass = Class.forName(rosMessage.toRawMessage().getType().replace("/", "."));

      if (customFieldConversions.containsConverterFor(aClass))
      {
         return customFieldConversions.convert(rosMessage);
      }
      else
      {
         return GenericROSTranslationTools.convertRosMessageToIHMCMessage(rosMessage);
      }
   }

   private static Packet customConvertToIHMCMessage(FootstepDataListRosMessage message)
   {
      FootstepDataListMessage footsteps = new FootstepDataListMessage();

      footsteps.setDefaultSwingDuration(message.getDefaultSwingDuration());
      footsteps.setDefaultTransferDuration(message.getDefaultTransferDuration());
      footsteps.setUniqueId(message.getUniqueId());
      try
      {
         footsteps.getQueueingProperties().set((QueueableMessage) convertToIHMCMessage(message.getQueueingProperties()));
      }
      catch (ClassNotFoundException | InvocationTargetException | IllegalAccessException | NoSuchFieldException | InstantiationException
            | RosEnumConversionException | IllegalArgumentException | NoSuchMethodException | SecurityException e1)
      {
         e1.printStackTrace();
      }
      footsteps.setFinalTransferDuration(message.getFinalTransferDuration());
      footsteps.setExecutionTiming(message.getExecutionTiming());
      footsteps.setOffsetFootstepsWithExecutionError(message.getOffsetFootstepsWithExecutionError());

      ArrayList<FootstepDataMessage> stepData = new ArrayList<>();
      for (FootstepDataRosMessage footstepDataRosMessage : message.getFootstepDataList())
      {
         try
         {
            stepData.add((FootstepDataMessage) convertToIHMCMessage(footstepDataRosMessage));
         }
         catch (ClassNotFoundException | InvocationTargetException | IllegalAccessException | RosEnumConversionException | NoSuchFieldException
               | InstantiationException | IllegalArgumentException | NoSuchMethodException | SecurityException e)
         {
            e.printStackTrace();
         }
      }

      MessageTools.copyData(stepData, footsteps.getFootstepDataList());

      return footsteps;
   }

   private static Packet customConvertToIHMCMessage(FootstepDataRosMessage message)
   {
      FootstepDataMessage ihmcMessage = new FootstepDataMessage();

      ihmcMessage.setRobotSide(message.getRobotSide());
      ihmcMessage.getLocation().set(new Point3D(GenericROSTranslationTools.convertPoint(message.getLocation())));
      ihmcMessage.getOrientation().set(new us.ihmc.euclid.tuple4D.Quaternion(GenericROSTranslationTools.convertQuaternion(message.getOrientation())));
      ihmcMessage.setSwingHeight(message.getSwingHeight());
      ihmcMessage.setTrajectoryType(message.getTrajectoryType());
      ihmcMessage.setUniqueId(message.getUniqueId());
      ihmcMessage.setSwingDuration(message.getSwingDuration());
      ihmcMessage.setTransferDuration(message.getTransferDuration());
      ihmcMessage.setTouchdownDuration(message.getTouchdownDuration());
      ihmcMessage.setSwingTrajectoryBlendDuration(message.getSwingTrajectoryBlendDuration());

      ArrayList<Point2D> predictedContactPoints = new ArrayList<>();
      for (Point2dRosMessage point2dRosMessage : message.getPredictedContactPoints())
      {
         predictedContactPoints.add(GenericROSTranslationTools.convertPoint2DRos(point2dRosMessage));
      }

      Point3D[] trajectoryWaypoints = new Point3D[message.getCustomPositionWaypoints().size()];
      for (int i = 0; i < message.getCustomPositionWaypoints().size(); i++)
      {
         trajectoryWaypoints[i] = new Point3D(GenericROSTranslationTools.convertPoint(message.getCustomPositionWaypoints().get(i)));
      }

      for (SE3TrajectoryPointRosMessage rosTrajectoryPoint : message.getSwingTrajectory())
      {
         try
         {
            ihmcMessage.getSwingTrajectory().add().set((SE3TrajectoryPointMessage) convertToIHMCMessage(rosTrajectoryPoint));
         }
         catch (ClassNotFoundException | InvocationTargetException | IllegalAccessException | NoSuchFieldException | InstantiationException
               | IllegalArgumentException | NoSuchMethodException | SecurityException | RosEnumConversionException e)
         {
            e.printStackTrace();
         }
      }

      HumanoidMessageTools.packPredictedContactPoints(predictedContactPoints, ihmcMessage);
      MessageTools.copyData(trajectoryWaypoints, ihmcMessage.getCustomPositionWaypoints());

      return ihmcMessage;
   }

   private static Packet customConvertToIHMCMessage(WholeBodyTrajectoryRosMessage message)
   {
      WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage = new WholeBodyTrajectoryMessage();

      wholeBodyTrajectoryMessage.setUniqueId(message.getUniqueId());
      try
      {
         wholeBodyTrajectoryMessage.getLeftArmTrajectoryMessage().set((ArmTrajectoryMessage) convertToIHMCMessage(message.getLeftArmTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getRightArmTrajectoryMessage().set((ArmTrajectoryMessage) convertToIHMCMessage(message.getRightArmTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getLeftHandTrajectoryMessage().set((HandTrajectoryMessage) convertToIHMCMessage(message.getLeftHandTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getRightHandTrajectoryMessage().set((HandTrajectoryMessage) convertToIHMCMessage(message.getRightHandTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getLeftFootTrajectoryMessage().set((FootTrajectoryMessage) convertToIHMCMessage(message.getLeftFootTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getRightFootTrajectoryMessage().set((FootTrajectoryMessage) convertToIHMCMessage(message.getRightFootTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getChestTrajectoryMessage().set((ChestTrajectoryMessage) convertToIHMCMessage(message.getChestTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getPelvisTrajectoryMessage().set((PelvisTrajectoryMessage) convertToIHMCMessage(message.getPelvisTrajectoryMessage()));
         wholeBodyTrajectoryMessage.getHeadTrajectoryMessage().set((HeadTrajectoryMessage) convertToIHMCMessage(message.getHeadTrajectoryMessage()));
      }
      catch (ClassNotFoundException | InvocationTargetException | IllegalAccessException | RosEnumConversionException | NoSuchFieldException
            | InstantiationException | IllegalArgumentException | NoSuchMethodException | SecurityException e)
      {
         e.printStackTrace();
      }

      return wholeBodyTrajectoryMessage;
   }

   private static Message customConvertToRosMessage(WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage)
   {
      Class<? extends Packet> ihmcMessageClass = WholeBodyTrajectoryMessage.class;
      String rosMessageClassNameFromIHMCMessage = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(ihmcMessageClass.getSimpleName());
      RosMessagePacket rosAnnotation = ihmcMessageClass.getAnnotation(RosMessagePacket.class);

      WholeBodyTrajectoryRosMessage message = messageFactory.newFromType(rosAnnotation.rosPackage() + "/" + rosMessageClassNameFromIHMCMessage);

      checkForNullComponents(wholeBodyTrajectoryMessage);
      message.setUniqueId(wholeBodyTrajectoryMessage.getUniqueId());
      try
      {
         message.setChestTrajectoryMessage((ChestTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getChestTrajectoryMessage()));
         message.setLeftArmTrajectoryMessage((ArmTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getLeftArmTrajectoryMessage()));
         message.setRightArmTrajectoryMessage((ArmTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getRightArmTrajectoryMessage()));
         message.setPelvisTrajectoryMessage((PelvisTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getPelvisTrajectoryMessage()));
         message.setLeftFootTrajectoryMessage((FootTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getLeftFootTrajectoryMessage()));
         message.setRightFootTrajectoryMessage((FootTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getRightFootTrajectoryMessage()));
         message.setLeftHandTrajectoryMessage((HandTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getLeftHandTrajectoryMessage()));
         message.setRightHandTrajectoryMessage((HandTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getRightHandTrajectoryMessage()));
         message.setHeadTrajectoryMessage((HeadTrajectoryRosMessage) convertToRosMessage(wholeBodyTrajectoryMessage.getHeadTrajectoryMessage()));
      }
      catch (InvocationTargetException | IllegalAccessException | NoSuchMethodException | ClassNotFoundException e)
      {
         e.printStackTrace();
      }

      return message;
   }

   private static void checkForNullComponents(WholeBodyTrajectoryMessage wholeBodyTrajectoryMessage)
   {
      if (wholeBodyTrajectoryMessage.getChestTrajectoryMessage() == null)
      {
         ChestTrajectoryMessage component = new ChestTrajectoryMessage();
         wholeBodyTrajectoryMessage.getChestTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getLeftArmTrajectoryMessage() == null)
      {
         ArmTrajectoryMessage component = new ArmTrajectoryMessage();
         component.setRobotSide(RobotSide.LEFT.toByte());
         wholeBodyTrajectoryMessage.getLeftArmTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getRightArmTrajectoryMessage() == null)
      {
         ArmTrajectoryMessage component = new ArmTrajectoryMessage();
         component.setRobotSide(RobotSide.RIGHT.toByte());
         wholeBodyTrajectoryMessage.getRightArmTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getPelvisTrajectoryMessage() == null)
      {
         PelvisTrajectoryMessage component = new PelvisTrajectoryMessage();
         wholeBodyTrajectoryMessage.getPelvisTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getLeftFootTrajectoryMessage() == null)
      {
         FootTrajectoryMessage component = new FootTrajectoryMessage();
         component.setRobotSide(RobotSide.LEFT.toByte());
         wholeBodyTrajectoryMessage.getLeftFootTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getRightFootTrajectoryMessage() == null)
      {
         FootTrajectoryMessage component = new FootTrajectoryMessage();
         component.setRobotSide(RobotSide.RIGHT.toByte());
         wholeBodyTrajectoryMessage.getRightFootTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getLeftHandTrajectoryMessage() == null)
      {
         HandTrajectoryMessage component = new HandTrajectoryMessage();
         component.setRobotSide(RobotSide.LEFT.toByte());
         wholeBodyTrajectoryMessage.getLeftHandTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getRightHandTrajectoryMessage() == null)
      {
         HandTrajectoryMessage component = new HandTrajectoryMessage();
         component.setRobotSide(RobotSide.RIGHT.toByte());
         wholeBodyTrajectoryMessage.getRightHandTrajectoryMessage().set(component);
      }
      if (wholeBodyTrajectoryMessage.getHeadTrajectoryMessage() == null)
      {
         HeadTrajectoryMessage component = new HeadTrajectoryMessage();
         wholeBodyTrajectoryMessage.getHeadTrajectoryMessage().set(component);
      }
   }

   private static Message customConvertToRosMessage(FootstepDataMessage footstep)
   {
      Class<? extends Packet> ihmcMessageClass = FootstepDataMessage.class;
      String rosMessageClassNameFromIHMCMessage = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(ihmcMessageClass.getSimpleName());
      RosMessagePacket rosAnnotation = ihmcMessageClass.getAnnotation(RosMessagePacket.class);

      FootstepDataRosMessage message = messageFactory.newFromType(rosAnnotation.rosPackage() + "/" + rosMessageClassNameFromIHMCMessage);

      message.setUniqueId(footstep.getUniqueId());
      message.setLocation(GenericROSTranslationTools.convertPoint3D(footstep.getLocation()));
      message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation()));
      message.setRobotSide(footstep.getRobotSide());
      message.setSwingHeight(footstep.getSwingHeight());
      message.setTrajectoryType(footstep.getTrajectoryType());
      message.setSwingDuration(footstep.getSwingDuration());
      message.setTransferDuration(footstep.getTransferDuration());
      message.setTouchdownDuration(footstep.getTouchdownDuration());
      message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration());

      List<Point2dRosMessage> predictedContatcPointsRos = new ArrayList<>();
      if (footstep.getPredictedContactPoints2d() != null)
      {
         for (Point2D predictedContactPoint : HumanoidMessageTools.unpackPredictedContactPoints(footstep))
         {
            predictedContatcPointsRos.add(GenericROSTranslationTools.convertPoint2d(predictedContactPoint));
         }
      }

      List<Point> positionWaypoints = new ArrayList<>();
      if (footstep.getCustomPositionWaypoints() != null)
      {
         List<Point3D> customPositionWaypoints = footstep.getCustomPositionWaypoints();
         for (int i = 0; i < customPositionWaypoints.size(); i++)
         {
            positionWaypoints.add(GenericROSTranslationTools.convertPoint3D(customPositionWaypoints.get(i)));
         }
      }

      List<SE3TrajectoryPointRosMessage> rosSwingTrajectory = new ArrayList<>();
      List<SE3TrajectoryPointMessage> swingTrajectory = footstep.getSwingTrajectory();
      for (int i = 0; i < swingTrajectory.size(); i++)
      {
         try
         {
            rosSwingTrajectory.add((SE3TrajectoryPointRosMessage) convertToRosMessage(swingTrajectory.get(i)));
         }
         catch (InvocationTargetException | IllegalAccessException | NoSuchMethodException | ClassNotFoundException e)
         {
            e.printStackTrace();
         }
      }
      message.setSwingTrajectory(rosSwingTrajectory);

      message.setPredictedContactPoints(predictedContatcPointsRos);
      message.setCustomPositionWaypoints(positionWaypoints);

      return message;
   }

   private static Message customConvertToRosMessage(FootstepDataListMessage footstepList)
   {
      Class<? extends Packet> ihmcMessageClass = FootstepDataListMessage.class;
      String rosMessageClassNameFromIHMCMessage = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(ihmcMessageClass.getSimpleName());
      RosMessagePacket rosAnnotation = ihmcMessageClass.getAnnotation(RosMessagePacket.class);

      FootstepDataListRosMessage message = messageFactory.newFromType(rosAnnotation.rosPackage() + "/" + rosMessageClassNameFromIHMCMessage);

      message.setDefaultSwingDuration(footstepList.getDefaultSwingDuration());
      message.setDefaultTransferDuration(footstepList.getDefaultTransferDuration());
      message.setUniqueId(footstepList.getUniqueId());
      message.setFinalTransferDuration(footstepList.getFinalTransferDuration());
      message.setOffsetFootstepsWithExecutionError(footstepList.getOffsetFootstepsWithExecutionError());
      message.setExecutionTiming(footstepList.getExecutionTiming());
      try
      {
         message.setQueueingProperties((QueueableRosMessage) convertToRosMessage(footstepList.getQueueingProperties()));
      }
      catch (InvocationTargetException | IllegalAccessException | NoSuchMethodException | ClassNotFoundException e1)
      {
         e1.printStackTrace();
      }

      List<FootstepDataRosMessage> convertedFootsteps = new ArrayList<>();
      List<FootstepDataMessage> footstepDataList = footstepList.getFootstepDataList();
      for (int i = 0; i < footstepDataList.size(); i++)
      {
         FootstepDataMessage footstepDataMessage = footstepDataList.get(i);
         try
         {
            convertedFootsteps.add((FootstepDataRosMessage) convertToRosMessage(footstepDataMessage));
         }
         catch (InvocationTargetException | IllegalAccessException | NoSuchMethodException | ClassNotFoundException e)
         {
            e.printStackTrace();
         }
      }

      message.setFootstepDataList(convertedFootsteps);

      return message;
   }

   private static FrameInformationRosMessage convertFrameInformation(FrameInformation frameInformation)
   {
      Class<?> ihmcClass = FrameInformation.class;
      String rosMessageClassNameFromIHMCClass = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(ihmcClass.getSimpleName());
      RosMessagePacket rosAnnotation = ihmcClass.getAnnotation(RosMessagePacket.class);

      FrameInformationRosMessage message = messageFactory.newFromType(rosAnnotation.rosPackage() + "/" + rosMessageClassNameFromIHMCClass);

      message.setDataReferenceFrameId(frameInformation.getDataReferenceFrameId());
      message.setTrajectoryReferenceFrameId(frameInformation.getTrajectoryReferenceFrameId());

      return message;
   }

   private static FrameInformation convertFrameInformationRosMessage(FrameInformationRosMessage message)
   {
      FrameInformation frameInformation = new FrameInformation();

      frameInformation.setDataReferenceFrameId(message.getDataReferenceFrameId());
      frameInformation.setTrajectoryReferenceFrameId(message.getTrajectoryReferenceFrameId());

      return frameInformation;
   }

   public static String getROSMessageTypeStringFromIHMCMessageClass(Class outputType)
   {
      String rosMessageClassNameFromIHMCMessage = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(outputType.getSimpleName());
      RosMessagePacket annotation = (RosMessagePacket) outputType.getAnnotation(RosMessagePacket.class);

      if (annotation == null)
      {
         return null;
      }

      return annotation.rosPackage() + "/" + rosMessageClassNameFromIHMCMessage;
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/producers/CompressedVideoDataClient.java<|end_filename|>
package us.ihmc.communication.producers;

import us.ihmc.communication.net.ConnectionStateListener;
import us.ihmc.communication.video.CompressedVideoCallback;

public interface CompressedVideoDataClient extends ConnectionStateListener, CompressedVideoCallback
{
   
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/initialSetup/SquaredUpDRCDemo01RobotOnPlatformsInitialSetup.java<|end_filename|>
package us.ihmc.avatar.initialSetup;

import us.ihmc.euclid.tuple3D.Vector3D;

public class SquaredUpDRCDemo01RobotOnPlatformsInitialSetup extends OffsetAndYawRobotInitialSetup
{
   private static final Vector3D firstPlatform = new Vector3D(-2.0, -3.6, 0.05);
   private static final Vector3D lastPlatform = new Vector3D(-6.9, -8.6, 0.4);
   
   private static final double offsetX = -1.8943, offsetY = -1.8745;
   private static final double yaw = Math.atan2(offsetY, offsetX);
   
   public SquaredUpDRCDemo01RobotOnPlatformsInitialSetup(Vector3D additionalOffset, double yaw)
   {
      super(additionalOffset, yaw);
   }

   public static SquaredUpDRCDemo01RobotOnPlatformsInitialSetup createInitialSetupOnNthPlatform(int nthPlatform)
   {
      double alpha = ((double) nthPlatform)/7.0;
      Vector3D startingLocation = morph(firstPlatform, lastPlatform, alpha);
      
      return new SquaredUpDRCDemo01RobotOnPlatformsInitialSetup(startingLocation, yaw);
   }
   
   public static Vector3D morph(Vector3D point1, Vector3D point2, double alpha)
   {
      Vector3D framePoint1 = new Vector3D(point1);
      Vector3D framePoint2 = new Vector3D(point2);

      framePoint1.scale(1.0 - alpha);
      framePoint2.scale(alpha);
      framePoint1.add(framePoint2);


      return framePoint1;
   }
   
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepSnapper/SimpleFootstepValueFunction.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepSnapper;

import controller_msgs.msg.dds.FootstepDataMessage;
import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;

/**
 * Created by agrabertilton on 1/20/15.
 */
public class SimpleFootstepValueFunction implements FootstepValueFunction
{
   private QuadTreeFootstepSnappingParameters parameters;
   private double slopeGain = -0.25; //more slope is bad
   private double areaGain = 100; //more area is good

   public SimpleFootstepValueFunction(QuadTreeFootstepSnappingParameters parameters)
   {
      this.parameters = parameters;
      updateFunction();
   }

   @Override
   public double getFootstepValue(FootstepDataMessage footstepData)
   {
      RotationMatrix rotationMatrix = new RotationMatrix();
      rotationMatrix.set(footstepData.getOrientation());
      Vector3D footstepNormal = new Vector3D();
      rotationMatrix.getColumn(2, footstepNormal);

      double offHorizontalAngle = Math.acos(footstepNormal.getZ());

      if (offHorizontalAngle > parameters.getMaxAngle() || footstepNormal.getZ() < 0)
         return Double.NEGATIVE_INFINITY;

      double value = slopeGain * offHorizontalAngle;
      if (footstepData.getPredictedContactPoints2d() == null || footstepData.getPredictedContactPoints2d().isEmpty())
         return Double.NEGATIVE_INFINITY;

      ConvexPolygon2D supportPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(HumanoidMessageTools.unpackPredictedContactPoints(footstepData)));
      supportPolygon.update();
      double inPlaneArea = supportPolygon.getArea();
      double horizonalArea = inPlaneArea * footstepNormal.getZ();

      if (horizonalArea < parameters.getMinArea())
         return Double.NEGATIVE_INFINITY;

      value += horizonalArea * areaGain;
      return value;
   }

   @Override
   public void updateFunction()
   {
   }
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/RosPointCloudVisualizer.java<|end_filename|>
package us.ihmc.ihmcPerception;

import java.util.ArrayList;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Point3D32;
import us.ihmc.ihmcPerception.depthData.PointCloudDataReceiverInterface;
import us.ihmc.ihmcPerception.depthData.PointCloudSource;
import us.ihmc.ihmcPerception.depthData.RosPointCloudReceiver;
import us.ihmc.jMonkeyEngineToolkit.jme.JMEGraphics3DAdapter;
import us.ihmc.jMonkeyEngineToolkit.jme.JMEGraphics3DWorld;
import us.ihmc.jMonkeyEngineToolkit.jme.JMERenderer;
import us.ihmc.jMonkeyEngineToolkit.jme.util.JMELidarSpriteGenerator;
import us.ihmc.utilities.ros.RosMainNode;

public class RosPointCloudVisualizer
{
   private final JMELidarSpriteGenerator lidarSpriteGenerator;
   private final RigidBodyTransform sensorTransform;
   private final RosPointCloudReceiver pointCloudReceiver;
   private final RosMainNode rosMainNode;

   public RosPointCloudVisualizer(String topicName, RosMainNode rosMainNode)
   {
      this(topicName, rosMainNode, null);
   }


   public RosPointCloudVisualizer(String topicName, RosMainNode rosMainNode, JMERenderer renderer)
   {
      this(topicName, rosMainNode, renderer, null);
   }

   public RosPointCloudVisualizer(String topicName, RosMainNode rosMainNode, JMERenderer renderer, RigidBodyTransform sensorTransform)
   {
      if(renderer == null)
      {
         JMEGraphics3DWorld pointCloudVisualizer = new JMEGraphics3DWorld("RosPointCloudVisualizer", new JMEGraphics3DAdapter(false));
         pointCloudVisualizer.startWithGui();

         renderer = pointCloudVisualizer.getGraphics3DAdapter().getRenderer();
      }

      this.rosMainNode = rosMainNode;
      this.sensorTransform = sensorTransform;

      lidarSpriteGenerator = new JMELidarSpriteGenerator(renderer);
      renderer.getZUpNode().attachChild(lidarSpriteGenerator);
      renderer.registerUpdatable(lidarSpriteGenerator);

      pointCloudReceiver = new RosPointCloudReceiver(topicName, rosMainNode, ReferenceFrame.getWorldFrame(),
            ReferenceFrame.getWorldFrame(), new PointCloudReceiver(), PointCloudSource.NEARSCAN);
   }

   public void detach()
   {
      lidarSpriteGenerator.clear();
      rosMainNode.removeSubscriber(pointCloudReceiver);
   }

   class PointCloudReceiver implements PointCloudDataReceiverInterface
   {
      @Override
      public void receivedPointCloudData(ReferenceFrame scanFrame, ReferenceFrame lidarFrame, long[] timestamps, ArrayList<Point3D> points,
            PointCloudSource... sources)
      {
         Point3D32[] point3fs = new Point3D32[points.size()];
         for(int i = 0; i < points.size(); i++)
         {
            Point3D point = points.get(i);
            if(sensorTransform != null)
               sensorTransform.transform(point);
            point3fs[i] = new Point3D32(point);
         }

         lidarSpriteGenerator.updatePoints(point3fs);
      }
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/DetectedObjectListener.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import us.ihmc.euclid.transform.RigidBodyTransform;

public interface DetectedObjectListener
{
   public abstract void updatePose(RigidBodyTransform pose, int id);
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullLeggedRobotModelFactory.java<|end_filename|>
package us.ihmc.robotModels;

import us.ihmc.robotics.robotSide.RobotSegment;

public interface FullLeggedRobotModelFactory<E extends Enum<E> & RobotSegment<E>> extends FullRobotModelFactory
{
   @Override
   FullLeggedRobotModel<E> createFullRobotModel();
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/parameters/ValkyrieLegConfigurationParameters.java<|end_filename|>
package us.ihmc.valkyrie.parameters;

import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.commonWalkingControlModules.configurations.LegConfigurationParameters;
import us.ihmc.commonWalkingControlModules.controlModules.legConfiguration.LegConfigurationGains;

public class ValkyrieLegConfigurationParameters extends LegConfigurationParameters
{
   private final RobotTarget target;

   public ValkyrieLegConfigurationParameters(RobotTarget target)
   {
      this.target = target;
   }

   /** {@inheritDoc} */
   @Override
   public LegConfigurationGains getBentLegGains()
   {
      LegConfigurationGains gains = new LegConfigurationGains();
      gains.setJointSpaceKp((target == RobotTarget.REAL_ROBOT) ? 40.0 : 150.0);
      gains.setJointSpaceKd(6.0);

      return gains;
   }

   /** {@inheritDoc} */
   public boolean attemptToStraightenLegs()
   {
      return false;
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTInterface.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;
/**
 * @method boolean isValidNode()
 * This method is for testing the node is valid.
 * 
 * @method RRTNode createNode()
 * This method is used for RRTTree, RRTPiecewisePath, RRTPlanner.
 * A class that inherits <RRTNode> must return a new constructor with overriding this method.
 * 
 * @author InhoLee 170224
 *
 */
public interface RRTInterface
{
   public abstract boolean isValidNode();
   public abstract RRTNode createNode();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/boards/BCF2000ControlMap.java<|end_filename|>
package us.ihmc.robotics.sliderboard.boards;

import us.ihmc.robotics.sliderboard.MidiControlMap;

/**
 * Implementation of the {@link MidiControlMap} for the BCF2000 sliderboard.
 * <p>
 * Indexing of the buttons and sliders on the board is as follows:</br>
 *  - The sliders 1-8 map to the physical sliders on the board.</br>
 *  - The sliders 9-16 map to the knobs on the board.</br>
 *  - The buttons 1-16 map to the buttons below the knobs on the board.</br>
 *  - The buttons 17-20 map the buttons in the bottom right of the board.</br>
 *  - The buttons 21-28 map the knobs as they can be used as buttons as well.
 * </p>
 *
 * @author <NAME>
 *
 */
public class BCF2000ControlMap implements MidiControlMap
{
   @Override
   public int getSliderChannel(int sliderIndex)
   {
      if (MidiControlMap.isInRange(sliderIndex, 1, 8))
      {
         return sliderIndex + 80;
      }
      // the knobs
      if (MidiControlMap.isInRange(sliderIndex, 9, 16))
      {
         return sliderIndex - 8;
      }

      return INVALID;
   }

   @Override
   public int getSliderIndex(int sliderChannel)
   {
      if (MidiControlMap.isInRange(sliderChannel, 81, 88))
      {
         return sliderChannel - 80;
      }
      // the knobs
      if (MidiControlMap.isInRange(sliderChannel, 1, 8))
      {
         return sliderChannel + 8;
      }

      return INVALID;
   }

   @Override
   public int getButtonChannel(int buttonIndex)
   {
      if (MidiControlMap.isInRange(buttonIndex, 1, 16))
      {
         return buttonIndex + 64;
      }
      // bottom right buttons
      if (MidiControlMap.isInRange(buttonIndex, 17, 20))
      {
         return buttonIndex + 72;
      }
      // knobs
      if (MidiControlMap.isInRange(buttonIndex, 21, 28))
      {
         return buttonIndex + 12;
      }

      return INVALID;
   }

   @Override
   public int getButtonIndex(int buttonChannel)
   {
      if (MidiControlMap.isInRange(buttonChannel, 65, 80))
      {
         return buttonChannel - 64;
      }
      // bottom right buttons
      if (MidiControlMap.isInRange(buttonChannel, 89, 92))
      {
         return buttonChannel - 72;
      }
      // knobs
      if (MidiControlMap.isInRange(buttonChannel, 33, 40))
      {
         return buttonChannel - 12;
      }

      return INVALID;
   }

   @Override
   public int getDelayVariationChannel()
   {
      return 94;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TimeIntervalMessage.java<|end_filename|>
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;

public class TimeIntervalMessage extends Packet<TimeIntervalMessage> implements Settable<TimeIntervalMessage>, EpsilonComparable<TimeIntervalMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public double start_time_;
   public double end_time_;

   public TimeIntervalMessage()
   {
   }

   public TimeIntervalMessage(TimeIntervalMessage other)
   {
      this();
      set(other);
   }

   public void set(TimeIntervalMessage other)
   {
      sequence_id_ = other.sequence_id_;

      start_time_ = other.start_time_;

      end_time_ = other.end_time_;

   }

   /**
            * 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 void setStartTime(double start_time)
   {
      start_time_ = start_time;
   }
   public double getStartTime()
   {
      return start_time_;
   }

   public void setEndTime(double end_time)
   {
      end_time_ = end_time;
   }
   public double getEndTime()
   {
      return end_time_;
   }


   public static Supplier<TimeIntervalMessagePubSubType> getPubSubType()
   {
      return TimeIntervalMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return TimeIntervalMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(TimeIntervalMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.start_time_, other.start_time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.end_time_, other.end_time_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof TimeIntervalMessage)) return false;

      TimeIntervalMessage otherMyClass = (TimeIntervalMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.start_time_ != otherMyClass.start_time_) return false;

      if(this.end_time_ != otherMyClass.end_time_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("TimeIntervalMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("start_time=");
      builder.append(this.start_time_);      builder.append(", ");
      builder.append("end_time=");
      builder.append(this.end_time_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/bipedSupportPolygons/ContactableFoot.java<|end_filename|>
package us.ihmc.humanoidRobotics.bipedSupportPolygons;

import us.ihmc.euclid.referenceFrame.FrameLineSegment2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.robotics.contactable.ContactablePlaneBody;

public interface ContactableFoot extends ContactablePlaneBody
{
   public abstract void getToeOffContactPoint(FramePoint2D contactPointToPack);

   public abstract void getToeOffContactLine(FrameLineSegment2D contactLineToPack);
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTPlanner.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

import java.util.ArrayList;

import us.ihmc.commons.PrintTools;

/**
 * This class provides only expanding tree, expanding tree until reaching goal node and updating an optimal path.
 * The optimal path is just displacement minimizing straight-line shortcut.
 *  
 * @author InhoLee 170224
 *
 */
public class RRTPlanner
{
   private RRTNode rootNode;
   private RRTNode goalNode;

   private RRTTree rrtTree;
   private RRTPiecewisePath rrtPiecewisePath;
   private ArrayList<RRTNode> optimalPath = new ArrayList<RRTNode>();

   public RRTPlanner(RRTNode root, RRTNode goal, double stepLength)
   {
      this.rootNode = root;
      this.goalNode = goal;

      rrtTree = new RRTTree(this.rootNode);
      rrtTree.setStepLength(stepLength);
   }

   public void expandTreeWhole(int numberOfExpanding)
   {
      for (int i = 0; i < numberOfExpanding; i++)
      {
         rrtTree.expandTree();
      }
   }

   public boolean expandTreeGoal(int maxNumberOfExpanding)
   {
      for (int i = 0; i < maxNumberOfExpanding; i++)
      {
         if (rrtTree.expandTree() == true)
         {
            if (rrtTree.newNode.getDistance(goalNode) < rrtTree.getStepLength())
            {
               rrtTree.newNode.addChildNode(goalNode);
               rrtTree.updatePathNode(goalNode);
               optimalPath = rrtTree.pathNode;

               return true;
            }
         }
      }
      return false;
   }

   public void updateOptimalPath(int sizeOfPiecewisePath)
   {
      if (optimalPath.size() > 1)
      {
         rrtPiecewisePath = new RRTPiecewisePath(optimalPath, sizeOfPiecewisePath);

         if (rrtPiecewisePath.updateShortCutPath() == true)
         {
            optimalPath = rrtPiecewisePath.getShortCutPath();
         }
      }
      else
      {
         //PrintTools.info("There is no path");
      }
   }

   public void updateOptimalPath(int sizeOfPiecewisePath, int numberOfIteration)
   {
      PrintTools.info("Buliding Started");
      for (int i = 0; i < numberOfIteration; i++)
      {
         updateOptimalPath(sizeOfPiecewisePath);
      }

      if (optimalPath.size() > 2)
      {
         ArrayList<RRTNode> tempPath = new ArrayList<RRTNode>();
         tempPath.add(optimalPath.get(0));
         for (int i = 1; i < optimalPath.size(); i++)
         {
            if (optimalPath.get(0).getDistance(optimalPath.get(i)) > 0)
            {
               tempPath.add(optimalPath.get(i));
            }
         }
         optimalPath = tempPath;
      }

      PrintTools.info("OptimalPath is Built");
   }

   public RRTTree getRRTTree()
   {
      return rrtTree;
   }

   public RRTPiecewisePath getPiecewisePath()
   {
      return rrtPiecewisePath;
   }

   public ArrayList<RRTNode> getOptimalPath()
   {
      return optimalPath;
   }

   public void setOptimalPath(ArrayList<RRTNode> path)
   {
      optimalPath = path;
   }

   public RRTNode getGoalNode()
   {
      return goalNode;
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/cart/CartRobotCollisionMeshDefinitionDataHolder.java<|end_filename|>
package us.ihmc.exampleSimulations.cart;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.BoxCollisionMeshDefinitionData;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.CollisionMeshDefinitionData;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.CollisionMeshDefinitionDataHolder;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.CylinderCollisionMeshDefinitionData;

public class CartRobotCollisionMeshDefinitionDataHolder extends CollisionMeshDefinitionDataHolder
{
   private final double xLengthBody = 0.8;
   private final double yLengthBody = 0.4;
   private final double zLengthBody = 0.1;

   private final double radiusWheel = 0.08;
   private final double lengthWheel = yLengthBody * 0.8;

   public CartRobotCollisionMeshDefinitionDataHolder()
   {
      RigidBodyTransform transformToBody = new RigidBodyTransform();
      CollisionMeshDefinitionData bodyCollisionMeshData = new BoxCollisionMeshDefinitionData("body", xLengthBody, yLengthBody, zLengthBody);
      bodyCollisionMeshData.setTransformToParentJoint(transformToBody);
      addCollisionMeshDefinitionData(bodyCollisionMeshData);

      RigidBodyTransform transformToFrontWheel = new RigidBodyTransform();
      transformToFrontWheel.appendRollRotation(Math.PI / 2.0);
      transformToFrontWheel.appendTranslation(0.0, 0.0, -lengthWheel / 2.0);
      CollisionMeshDefinitionData frontWheelCollisionMeshData = new CylinderCollisionMeshDefinitionData("frontWheel", radiusWheel, lengthWheel);
      frontWheelCollisionMeshData.setTransformToParentJoint(transformToFrontWheel);
      addCollisionMeshDefinitionData(frontWheelCollisionMeshData);

      RigidBodyTransform transformToRearWheel = new RigidBodyTransform();
      transformToRearWheel.appendRollRotation(Math.PI / 2.0);
      transformToRearWheel.appendTranslation(0.0, 0.0, -lengthWheel / 2.0);
      CollisionMeshDefinitionData rearWheelCollisionMeshData = new CylinderCollisionMeshDefinitionData("rearWheel", radiusWheel, lengthWheel);
      rearWheelCollisionMeshData.setTransformToParentJoint(transformToRearWheel);
      addCollisionMeshDefinitionData(rearWheelCollisionMeshData);
   }
}


<|start_filename|>websitedocs/website/node_modules/docusaurus/lib/core/metadata.js<|end_filename|>
/**
 * @generated
 */
module.exports = {
  "00-intro-control": {
    "title": "Introduction",
    "id": "00-intro-control",
    "source": "00-intro-control.md",
    "permalink": "docs/00-intro-control.html",
    "localized_id": "00-intro-control",
    "language": "en",
    "sidebar": "IHMCcontroller",
    "category": "IHMC Humanoid Controller",
    "next_id": "01-controllercore",
    "next": "01-controllercore",
    "previous_id": "000-summary-control",
    "previous": "000-summary-control",
    "previous_title": "Summary",
    "next_title": "Controller Core"
  },
  "00-pre-reqs-eclipse": {
    "title": "Prerequisites",
    "sidebar_label": "Prerequisites",
    "id": "00-pre-reqs-eclipse",
    "source": "00-pre-reqs-eclipse.md",
    "permalink": "docs/00-pre-reqs-eclipse.html",
    "localized_id": "00-pre-reqs-eclipse",
    "language": "en",
    "sidebar": "eclipsequickstart",
    "category": "Quick Start (Eclipse)",
    "next_id": "01-project-eclipse",
    "next": "01-project-eclipse",
    "next_title": "Create your First GradleProject with a build.gradle File"
  },
  "00-pre-reqs-intellij": {
    "title": "Prerequisites",
    "sidebar_label": "Prerequisites",
    "id": "00-pre-reqs-intellij",
    "source": "00-pre-reqs-intellij.md",
    "permalink": "docs/00-pre-reqs-intellij.html",
    "localized_id": "00-pre-reqs-intellij",
    "language": "en",
    "sidebar": "intellijquickstart",
    "category": "Quick Start (IntelliJ)",
    "next_id": "01-project-intellij",
    "next": "01-project-intellij",
    "next_title": "Create your First GradleProject with a build.gradle File"
  },
  "000-summary-control": {
    "title": "Summary",
    "id": "000-summary-control",
    "source": "000-summary-control.md",
    "permalink": "docs/000-summary-control.html",
    "localized_id": "000-summary-control",
    "language": "en",
    "sidebar": "IHMCcontroller",
    "category": "IHMC Humanoid Controller",
    "next_id": "00-intro-control",
    "next": "00-intro-control",
    "next_title": "Introduction"
  },
  "01-controllercore": {
    "title": "Controller Core",
    "id": "01-controllercore",
    "source": "01-controllercore.md",
    "permalink": "docs/01-controllercore.html",
    "localized_id": "01-controllercore",
    "language": "en",
    "sidebar": "IHMCcontroller",
    "category": "IHMC Humanoid Controller",
    "next_id": "02-yovariables",
    "next": "02-yovariables",
    "previous_id": "00-intro-control",
    "previous": "00-intro-control",
    "previous_title": "Introduction",
    "next_title": "Yo Variables"
  },
  "01-project-eclipse": {
    "title": "Create your First GradleProject with a build.gradle File",
    "sidebar_label": "Create your First GradleProject",
    "id": "01-project-eclipse",
    "source": "01-project-eclipse.md",
    "permalink": "docs/01-project-eclipse.html",
    "localized_id": "01-project-eclipse",
    "language": "en",
    "sidebar": "eclipsequickstart",
    "category": "Quick Start (Eclipse)",
    "next_id": "02-setup-gradle-project-eclipse",
    "next": "02-setup-gradle-project-eclipse",
    "previous_id": "00-pre-reqs-eclipse",
    "previous": "00-pre-reqs-eclipse",
    "previous_title": "Prerequisites",
    "next_title": "Import GradleProject"
  },
  "01-project-intellij": {
    "title": "Create your First GradleProject with a build.gradle File",
    "sidebar_label": "Create your First GradleProject",
    "id": "01-project-intellij",
    "source": "01-project-intellij.md",
    "permalink": "docs/01-project-intellij.html",
    "localized_id": "01-project-intellij",
    "language": "en",
    "sidebar": "intellijquickstart",
    "category": "Quick Start (IntelliJ)",
    "next_id": "02-setup-gradle-project-intellij",
    "next": "02-setup-gradle-project-intellij",
    "previous_id": "00-pre-reqs-intellij",
    "previous": "00-pre-reqs-intellij",
    "previous_title": "Prerequisites",
    "next_title": "Import GradleProject"
  },
  "02-setup-gradle-project-eclipse": {
    "title": "Import GradleProject",
    "sidebar_label": "Import GradleProject",
    "id": "02-setup-gradle-project-eclipse",
    "source": "02-setup-gradle-project-eclipse.md",
    "permalink": "docs/02-setup-gradle-project-eclipse.html",
    "localized_id": "02-setup-gradle-project-eclipse",
    "language": "en",
    "sidebar": "eclipsequickstart",
    "category": "Quick Start (Eclipse)",
    "next_id": "03-new-class-eclipse",
    "next": "03-new-class-eclipse",
    "previous_id": "01-project-eclipse",
    "previous": "01-project-eclipse",
    "previous_title": "Create your First GradleProject with a build.gradle File",
    "next_title": "Create The ValkyrieDemo Class"
  },
  "02-setup-gradle-project-intellij": {
    "title": "Import GradleProject",
    "sidebar_label": "Import GradleProject",
    "id": "02-setup-gradle-project-intellij",
    "source": "02-setup-gradle-project-intellij.md",
    "permalink": "docs/02-setup-gradle-project-intellij.html",
    "localized_id": "02-setup-gradle-project-intellij",
    "language": "en",
    "sidebar": "intellijquickstart",
    "category": "Quick Start (IntelliJ)",
    "next_id": "03-new-class-intellij",
    "next": "03-new-class-intellij",
    "previous_id": "01-project-intellij",
    "previous": "01-project-intellij",
    "previous_title": "Create your First GradleProject with a build.gradle File",
    "next_title": "Create The ValkyrieDemo Class"
  },
  "02-yovariables": {
    "title": "Yo Variables",
    "id": "02-yovariables",
    "source": "02-yovariables.md",
    "permalink": "docs/02-yovariables.html",
    "localized_id": "02-yovariables",
    "language": "en",
    "sidebar": "IHMCcontroller",
    "category": "IHMC Humanoid Controller",
    "previous_id": "01-controllercore",
    "previous": "01-controllercore",
    "previous_title": "Controller Core"
  },
  "03-new-class-eclipse": {
    "title": "Create The ValkyrieDemo Class",
    "sidebar_label": "Create the ValkyrieDemo Class",
    "id": "03-new-class-eclipse",
    "source": "03-new-class-eclipse.md",
    "permalink": "docs/03-new-class-eclipse.html",
    "localized_id": "03-new-class-eclipse",
    "language": "en",
    "sidebar": "eclipsequickstart",
    "category": "Quick Start (Eclipse)",
    "next_id": "04-run-valkyrie-eclipse",
    "next": "04-run-valkyrie-eclipse",
    "previous_id": "02-setup-gradle-project-eclipse",
    "previous": "02-setup-gradle-project-eclipse",
    "previous_title": "Import GradleProject",
    "next_title": "ValkyrieDemo Run Configuration"
  },
  "03-new-class-intellij": {
    "title": "Create The ValkyrieDemo Class",
    "sidebar_label": "Create the ValkyrieDemo Class",
    "id": "03-new-class-intellij",
    "source": "03-new-class-intellij.md",
    "permalink": "docs/03-new-class-intellij.html",
    "localized_id": "03-new-class-intellij",
    "language": "en",
    "sidebar": "intellijquickstart",
    "category": "Quick Start (IntelliJ)",
    "next_id": "04-run-valkyrie-intellij",
    "next": "04-run-valkyrie-intellij",
    "previous_id": "02-setup-gradle-project-intellij",
    "previous": "02-setup-gradle-project-intellij",
    "previous_title": "Import GradleProject",
    "next_title": "ValkyrieDemo Run Configuration"
  },
  "04-run-valkyrie-eclipse": {
    "title": "ValkyrieDemo Run Configuration",
    "sidebar_label": "ValkyrieDemo Run Configuration",
    "id": "04-run-valkyrie-eclipse",
    "source": "04-run-valkyrie-eclipse.md",
    "permalink": "docs/04-run-valkyrie-eclipse.html",
    "localized_id": "04-run-valkyrie-eclipse",
    "language": "en",
    "sidebar": "eclipsequickstart",
    "category": "Quick Start (Eclipse)",
    "next_id": "05-run-valkyrie-scs-eclipse",
    "next": "05-run-valkyrie-scs-eclipse",
    "previous_id": "03-new-class-eclipse",
    "previous": "03-new-class-eclipse",
    "previous_title": "Create The ValkyrieDemo Class",
    "next_title": "Run ValkyrieDemo"
  },
  "04-run-valkyrie-intellij": {
    "title": "ValkyrieDemo Run Configuration",
    "sidebar_label": "ValkyrieDemo Run Configuration",
    "id": "04-run-valkyrie-intellij",
    "source": "04-run-valkyrie-intellij.md",
    "permalink": "docs/04-run-valkyrie-intellij.html",
    "localized_id": "04-run-valkyrie-intellij",
    "language": "en",
    "sidebar": "intellijquickstart",
    "category": "Quick Start (IntelliJ)",
    "next_id": "05-run-valkyrie-scs",
    "next": "05-run-valkyrie-scs",
    "previous_id": "03-new-class-intellij",
    "previous": "03-new-class-intellij",
    "previous_title": "Create The ValkyrieDemo Class",
    "next_title": "Run ValkyrieDemo"
  },
  "05-run-valkyrie-scs-eclipse": {
    "title": "Run ValkyrieDemo",
    "sidebar_label": "Run ValkyrieDemo",
    "id": "05-run-valkyrie-scs-eclipse",
    "source": "05-run-valkyrie-scs-eclipse.md",
    "permalink": "docs/05-run-valkyrie-scs-eclipse.html",
    "localized_id": "05-run-valkyrie-scs-eclipse",
    "language": "en",
    "sidebar": "eclipsequickstart",
    "category": "Quick Start (Eclipse)",
    "previous_id": "04-run-valkyrie-eclipse",
    "previous": "04-run-valkyrie-eclipse",
    "previous_title": "ValkyrieDemo Run Configuration"
  },
  "05-run-valkyrie-scs": {
    "title": "Run ValkyrieDemo",
    "sidebar_label": "Run ValkyrieDemo",
    "id": "05-run-valkyrie-scs",
    "source": "05-run-valkyrie-scs.md",
    "permalink": "docs/05-run-valkyrie-scs.html",
    "localized_id": "05-run-valkyrie-scs",
    "language": "en",
    "sidebar": "intellijquickstart",
    "category": "Quick Start (IntelliJ)",
    "previous_id": "04-run-valkyrie-intellij",
    "previous": "04-run-valkyrie-intellij",
    "previous_title": "ValkyrieDemo Run Configuration"
  },
  "installgradle": {
    "title": "Installing Gradle",
    "id": "installgradle",
    "source": "installgradle.md",
    "permalink": "docs/installgradle.html",
    "localized_id": "installgradle",
    "language": "en"
  },
  "installjava": {
    "title": "Installing Java",
    "id": "installjava",
    "source": "installjava.md",
    "permalink": "docs/installjava.html",
    "localized_id": "installjava",
    "language": "en"
  },
  "open-software-home": {
    "id": "open-software-home",
    "title": "IHMC Open Robotics Software Documentatation",
    "source": "open-software-home.md",
    "permalink": "docs/open-software-home.html",
    "localized_id": "open-software-home",
    "language": "en"
  },
  "quickstarthome": {
    "id": "quickstarthome",
    "title": "Quick Start",
    "source": "quickstarthome.md",
    "permalink": "docs/quickstarthome.html",
    "localized_id": "quickstarthome",
    "language": "en"
  }
};


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/OneDoFJointTrajectoryMessage.java<|end_filename|>
package controller_msgs.msg.dds;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.euclid.interfaces.EpsilonComparable;
import java.util.function.Supplier;
import us.ihmc.pubsub.TopicDataType;

/**
       * This message is part of the IHMC whole-body controller API.
       * This class is used to build trajectory messages in jointspace
       * It holds all the trajectory points to go through with a one-dimensional trajectory.
       * A third order polynomial function is used to interpolate between trajectory points.
       */
public class OneDoFJointTrajectoryMessage extends Packet<OneDoFJointTrajectoryMessage> implements Settable<OneDoFJointTrajectoryMessage>, EpsilonComparable<OneDoFJointTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The list of trajectory points to go through while executing the trajectory.
            * The time has to be strictly increasing.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.TrajectoryPoint1DMessage>  trajectory_points_;
   /**
            * Weight used to encode the priority for achieving this trajectory:
            * - if too low, in the event the controller can't achieve all of the objectives it may lower the trajectory tracking quality.
            * - if too high, the controller will favor this trajectory over other objectives.
            * - if set to NaN or to a negative value, the controller will use the default weight for that trajectory.
            * The priority of this trajectory is determined from the relative weight of this trajectory and the weight of the other objectives.
            */
   public double weight_ = -1.0;

   public OneDoFJointTrajectoryMessage()
   {
      trajectory_points_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.TrajectoryPoint1DMessage> (50, new controller_msgs.msg.dds.TrajectoryPoint1DMessagePubSubType());

   }

   public OneDoFJointTrajectoryMessage(OneDoFJointTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(OneDoFJointTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      trajectory_points_.set(other.trajectory_points_);
      weight_ = other.weight_;

   }

   /**
            * 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_;
   }


   /**
            * The list of trajectory points to go through while executing the trajectory.
            * The time has to be strictly increasing.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.TrajectoryPoint1DMessage>  getTrajectoryPoints()
   {
      return trajectory_points_;
   }

   /**
            * Weight used to encode the priority for achieving this trajectory:
            * - if too low, in the event the controller can't achieve all of the objectives it may lower the trajectory tracking quality.
            * - if too high, the controller will favor this trajectory over other objectives.
            * - if set to NaN or to a negative value, the controller will use the default weight for that trajectory.
            * The priority of this trajectory is determined from the relative weight of this trajectory and the weight of the other objectives.
            */
   public void setWeight(double weight)
   {
      weight_ = weight;
   }
   /**
            * Weight used to encode the priority for achieving this trajectory:
            * - if too low, in the event the controller can't achieve all of the objectives it may lower the trajectory tracking quality.
            * - if too high, the controller will favor this trajectory over other objectives.
            * - if set to NaN or to a negative value, the controller will use the default weight for that trajectory.
            * The priority of this trajectory is determined from the relative weight of this trajectory and the weight of the other objectives.
            */
   public double getWeight()
   {
      return weight_;
   }


   public static Supplier<OneDoFJointTrajectoryMessagePubSubType> getPubSubType()
   {
      return OneDoFJointTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return OneDoFJointTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(OneDoFJointTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.trajectory_points_.size() != other.trajectory_points_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.trajectory_points_.size(); i++)
         {  if (!this.trajectory_points_.get(i).epsilonEquals(other.trajectory_points_.get(i), epsilon)) return false; }
      }

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.weight_, other.weight_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof OneDoFJointTrajectoryMessage)) return false;

      OneDoFJointTrajectoryMessage otherMyClass = (OneDoFJointTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.trajectory_points_.equals(otherMyClass.trajectory_points_)) return false;
      if(this.weight_ != otherMyClass.weight_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("OneDoFJointTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("trajectory_points=");
      builder.append(this.trajectory_points_);      builder.append(", ");
      builder.append("weight=");
      builder.append(this.weight_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PauseWalkingMessage.java<|end_filename|>
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 pauses the execution of a list of footsteps.
       * If this message is in the middle of executing a footstep, the robot will finish the step and pause when back in double support.
       */
public class PauseWalkingMessage extends Packet<PauseWalkingMessage> implements Settable<PauseWalkingMessage>, EpsilonComparable<PauseWalkingMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * True to pause walking, false to unpause and resume an existing footstep plan.
            */
   public boolean pause_;

   public PauseWalkingMessage()
   {
   }

   public PauseWalkingMessage(PauseWalkingMessage other)
   {
      this();
      set(other);
   }

   public void set(PauseWalkingMessage other)
   {
      sequence_id_ = other.sequence_id_;

      pause_ = other.pause_;

   }

   /**
            * 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_;
   }

   /**
            * True to pause walking, false to unpause and resume an existing footstep plan.
            */
   public void setPause(boolean pause)
   {
      pause_ = pause;
   }
   /**
            * True to pause walking, false to unpause and resume an existing footstep plan.
            */
   public boolean getPause()
   {
      return pause_;
   }


   public static Supplier<PauseWalkingMessagePubSubType> getPubSubType()
   {
      return PauseWalkingMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PauseWalkingMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PauseWalkingMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.pause_, other.pause_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PauseWalkingMessage)) return false;

      PauseWalkingMessage otherMyClass = (PauseWalkingMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.pause_ != otherMyClass.pause_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PauseWalkingMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("pause=");
      builder.append(this.pause_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/PlaneFitter.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.util.List;

import us.ihmc.euclid.geometry.Plane3D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;

public interface PlaneFitter
{
   /**
    *
    * @param pointList The list of points to fit
    * @param planeToPack the plane to pack
    * @return the average error of the points to the plane fit
    */
   public abstract double fitPlaneToPoints(List<? extends Point3DReadOnly> pointList, Plane3D planeToPack);

   /**
    *
    * @param center origin the plane at the center if possible
    * @param pointList The list of points to fit
    * @param planeToPack the plane to pack
    * @return the average error of the points to the plane fit
    */
   public abstract double fitPlaneToPoints(Point2DBasics center, List<? extends Point3DReadOnly> pointList, Plane3D planeToPack);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/PIDSE3Gains.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains;

/**
 * Interface for SE3 PID gains consisting of two PID gains in 3D. One for the
 * position control and one for the orientation control.
 */
public interface PIDSE3Gains extends PIDSE3GainsReadOnly
{
   /**
    * Returns the gains to be used for the position control.
    *
    * @return the position PID gains.
    */
   @Override
   public abstract PID3DGains getPositionGains();

   /**
    * Returns the gains to be used for the orientation control.
    *
    * @return the orientation PID gains.
    */
   @Override
   public abstract PID3DGains getOrientationGains();

   /**
    * Sets the position gains to the provided PID gains.
    *
    * @param positionGains the new gains for position control.
    */
   public default void setPositionGains(PID3DGainsReadOnly positionGains)
   {
      getPositionGains().set(positionGains);
   }

   /**
    * Sets the orientation gains to the provided PID gains.
    *
    * @param orientationGains the new gains for orientation control.
    */
   public default void setOrientationGains(PID3DGainsReadOnly orientationGains)
   {
      getOrientationGains().set(orientationGains);
   }

   /**
    * Sets gains in the position part of these SE3 gains.
    *
    * @param proportionalGainX the new proportional gain for the x direction.
    * @param proportionalGainY the new proportional gain for the y direction.
    * @param proportionalGainZ the new proportional gain for the z direction.
    * @see PID3DGains#setProportionalGains(double, double, double)
    */
   public default void setPositionProportionalGains(double proportionalGainX, double proportionalGainY, double proportionalGainZ)
   {
      getPositionGains().setProportionalGains(proportionalGainX, proportionalGainY, proportionalGainZ);
   }

   /**
    * Sets gains in the position part of these SE3 gains.
    *
    * @param derivativeGainX the new derivative gain for the x direction.
    * @param derivativeGainY the new derivative gain for the y direction.
    * @param derivativeGainZ the new derivative gain for the z direction.
    * @see PID3DGains#setDerivativeGains(double, double, double)
    */
   public default void setPositionDerivativeGains(double derivativeGainX, double derivativeGainY, double derivativeGainZ)
   {
      getPositionGains().setDerivativeGains(derivativeGainX, derivativeGainY, derivativeGainZ);
   }

   /**
    * Sets gains in the position part of these SE3 gains.
    *
    * @param integralGainX the new integral gain for the x direction.
    * @param integralGainY the new integral gain for the y direction.
    * @param integralGainZ the new integral gain for the z direction.
    * @param maxIntegralError the new maximum integral error.
    * @see PID3DGains#setIntegralGains(double, double, double)
    */
   public default void setPositionIntegralGains(double integralGainX, double integralGainY, double integralGainZ, double maxIntegralError)
   {
      getPositionGains().setIntegralGains(integralGainX, integralGainY, integralGainZ, maxIntegralError);
   }

   /**
    * Sets gains in the orientation part of these SE3 gains.
    *
    * @param proportionalGainX the new proportional gain for the x direction.
    * @param proportionalGainY the new proportional gain for the y direction.
    * @param proportionalGainZ the new proportional gain for the z direction.
    * @see PID3DGains#setProportionalGains(double, double, double)
    */
   public default void setOrientationProportionalGains(double proportionalGainX, double proportionalGainY, double proportionalGainZ)
   {
      getOrientationGains().setProportionalGains(proportionalGainX, proportionalGainY, proportionalGainZ);
   }

   /**
    * Sets gains in the orientation part of these SE3 gains.
    *
    * @param derivativeGainX the new derivative gain for the x direction.
    * @param derivativeGainY the new derivative gain for the y direction.
    * @param derivativeGainZ the new derivative gain for the z direction.
    * @see PID3DGains#setDerivativeGains(double, double, double)
    */
   public default void setOrientationDerivativeGains(double derivativeGainX, double derivativeGainY, double derivativeGainZ)
   {
      getOrientationGains().setDerivativeGains(derivativeGainX, derivativeGainY, derivativeGainZ);
   }

   /**
    * Sets gains in the orientation part of these SE3 gains.
    *
    * @param integralGainX the new integral gain for the x direction.
    * @param integralGainY the new integral gain for the y direction.
    * @param integralGainZ the new integral gain for the z direction.
    * @param maxIntegralError the new maximum integral error.
    * @see PID3DGains#setIntegralGains(double, double, double)
    */
   public default void setOrientationIntegralGains(double integralGainX, double integralGainY, double integralGainZ, double maxIntegralError)
   {
      getOrientationGains().setIntegralGains(integralGainX, integralGainY, integralGainZ, maxIntegralError);
   }

   /**
    * Sets gains in the position part of these SE3 gains.
    *
    * @param proportionalGainX the new proportional gain for the three dimensions.
    * @see PID3DGains#setProportionalGains(double)
    */
   public default void setPositionProportionalGains(double proportionalGain)
   {
      getPositionGains().setProportionalGains(proportionalGain);
   }

   /**
    * Sets gains in the position part of these SE3 gains.
    *
    * @param derivativeGainX the new derivative gain for the three dimensions.
    * @see PID3DGains#setDerivativeGains(double)
    */
   public default void setPositionDerivativeGains(double derivativeGain)
   {
      getPositionGains().setDerivativeGains(derivativeGain);
   }

   /**
    * Sets gains in the position part of these SE3 gains.
    *
    * @param integralGain the new integral gain for the three dimensions.
    * @param maxIntegralError the new maximum integral error.
    * @see PID3DGains#setIntegralGains(double, double)
    */
   public default void setPositionIntegralGains(double integralGain, double maxIntegralError)
   {
      getPositionGains().setIntegralGains(integralGain, maxIntegralError);
   }

   /**
    * Sets gains in the orientation part of these SE3 gains.
    *
    * @param proportionalGainX the new proportional gain for the three dimensions.
    * @see PID3DGains#setProportionalGains(double)
    */
   public default void setOrientationProportionalGains(double proportionalGain)
   {
      getOrientationGains().setProportionalGains(proportionalGain);
   }

   /**
    * Sets gains in the orientation part of these SE3 gains.
    *
    * @param derivativeGainX the new derivative gain for the three dimensions.
    * @see PID3DGains#setDerivativeGains(double)
    */
   public default void setOrientationDerivativeGains(double derivativeGain)
   {
      getOrientationGains().setDerivativeGains(derivativeGain);
   }

   /**
    * Sets gains in the orientation part of these SE3 gains.
    *
    * @param integralGainX the new integral gain for the three dimensions.
    * @param maxIntegralError the new maximum integral error.
    * @see PID3DGains#setIntegralGains(double, double)
    */
   public default void setOrientationIntegralGains(double integralGain, double maxIntegralError)
   {
      getOrientationGains().setIntegralGains(integralGain, maxIntegralError);
   }

   /**
    * Sets parameters in the position part of these SE3 gains.
    *
    * @param maxFeedback the new maximum feedback.
    * @param maxFeedbackRate the new maximum feedback rate.
    * @see PID3DGains#setMaxFeedbackAndFeedbackRate(double, double)
    */
   public default void setPositionMaxFeedbackAndFeedbackRate(double maxFeedback, double maxFeedbackRate)
   {
      getPositionGains().setMaxFeedbackAndFeedbackRate(maxFeedback, maxFeedbackRate);
   }

   /**
    * Sets parameters in the orientation part of these SE3 gains.
    *
    * @param maxFeedback the new maximum feedback.
    * @param maxFeedbackRate the new maximum feedback rate.
    * @see PID3DGains#setMaxFeedbackAndFeedbackRate(double, double)
    */
   public default void setOrientationMaxFeedbackAndFeedbackRate(double maxFeedback, double maxFeedbackRate)
   {
      getOrientationGains().setMaxFeedbackAndFeedbackRate(maxFeedback, maxFeedbackRate);
   }

   /**
    * Copies the gains and parameters from the provided {@link PIDSE3Gains}
    * parameters into this.
    *
    * @param other the new gains.
    */
   public default void set(PIDSE3GainsReadOnly other)
   {
      setOrientationGains(other.getOrientationGains());
      setPositionGains(other.getPositionGains());
   }
}

<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tree/ParameterTreeRegistry.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tree;

import org.apache.commons.lang3.StringUtils;

import javafx.application.Platform;
import javafx.geometry.Pos;
import javafx.scene.Node;
import javafx.scene.control.ContextMenu;
import javafx.scene.control.MenuItem;
import javafx.scene.control.Tooltip;
import javafx.scene.input.Clipboard;
import javafx.scene.input.ClipboardContent;
import javafx.scene.layout.HBox;
import javafx.scene.text.Text;
import us.ihmc.parameterTuner.guiElements.GuiElement;
import us.ihmc.parameterTuner.guiElements.GuiParameter;
import us.ihmc.parameterTuner.guiElements.GuiRegistry;

public class ParameterTreeRegistry implements ParameterTreeValue
{
   private final GuiRegistry registry;
   private Node node;

   public ParameterTreeRegistry(GuiRegistry registry)
   {
      this.registry = registry;
   }

   @Override
   public boolean isRegistry()
   {
      return true;
   }

   @Override
   public String getName()
   {
      return registry.getName();
   }

   @Override
   public Node getOrCreateNode()
   {
      if (node == null)
      {
         node = new RegistryNode(registry);
      }
      return node;
   }

   @Override
   public GuiParameter getParameter()
   {
      throw new RuntimeException("Is a registry!");
   }

   private class RegistryNode extends HBox
   {
      private final Text name = new Text();

      private final ContextMenu contextMenu = new ContextMenu();
      private final MenuItem makeRoot = new MenuItem("Make Root");
      private final MenuItem copyName = new MenuItem("Copy Name");
      private final Clipboard clipboard = Clipboard.getSystemClipboard();
      private final ClipboardContent content = new ClipboardContent();

      public RegistryNode(GuiRegistry registry)
      {
         // Creating this using fxml causes crazy slow down since it is done a lot.
         setSpacing(10.0);
         setAlignment(Pos.CENTER_LEFT);
         getChildren().add(name);
         setOnContextMenuRequested((event) -> contextMenu.show(name, event.getScreenX(), event.getScreenY()));

         // Setup context menu for copying name to system clipboard.
         contextMenu.getItems().add(copyName);
         copyName.setOnAction((event) -> {
            content.putString(registry.getName());
            clipboard.setContent(content);
         });

         // Setup context menu for making this a root registry.
         contextMenu.getItems().add(makeRoot);
         makeRoot.setOnAction(event -> Platform.runLater(() -> registry.makeRoot()));

         // Set up the css styles for the parameter status.
         registry.isRoot().addListener((observable, oldValue, newValue) -> updateStyle(newValue));
         updateStyle(registry.isRoot().get());

         name.setText(registry.getName());

         Tooltip tooltip = new Tooltip(StringUtils.replaceAll(registry.getUniqueName(), GuiElement.SEPERATOR, "\n"));
         Tooltip.install(this, tooltip);
      }

      private void updateStyle(boolean isRoot)
      {
         if (isRoot)
         {
            name.setId("root-registry-name-in-tree-view");
         }
         else
         {
            name.setId("registry-name-in-tree-view");
         }
      }
   }
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullHumanoidRobotModelFactory.java<|end_filename|>
package us.ihmc.robotModels;

import us.ihmc.robotics.robotSide.RobotSide;

public interface FullHumanoidRobotModelFactory extends FullLeggedRobotModelFactory<RobotSide>
{
   @Override
   FullHumanoidRobotModel createFullRobotModel();
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/vision/shapes/HoughCircleResult.java<|end_filename|>
package us.ihmc.ihmcPerception.vision.shapes;

import us.ihmc.euclid.tuple2D.Point2D;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class HoughCircleResult
{
   private final Point2D center;
   private final double radius;

   public HoughCircleResult(Point2D center, double radius)
   {
      this.center = center;
      this.radius = radius;
   }

   public Point2D getCenter()
   {
      return center;
   }

   public double getRadius()
   {
      return radius;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/TurnStraightTurnFootstepOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.CompositeFootstepOverheadPath;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.FootstepOverheadPath;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.InterpolatedFootstepOverheadPath;
import us.ihmc.robotics.geometry.AngleTools;

/**
 * Created by agrabertilton on 3/6/15.
 */
public class TurnStraightTurnFootstepOverheadPath extends FootstepOverheadPath
{
   CompositeFootstepOverheadPath overheadPath;
   public TurnStraightTurnFootstepOverheadPath(FramePose2D startPose, FramePose2D endPose)
   {
      this(startPose, endPose, 0.0);
   }

   public TurnStraightTurnFootstepOverheadPath(FramePose2D startPose, FramePose2D endPose, double offsetAngle)
   {

      startPose.checkReferenceFrameMatch(endPose);

      double deltaX = endPose.getX() - startPose.getX();
      double deltaY = endPose.getY() - startPose.getY();
      double straightYaw = AngleTools.trimAngleMinusPiToPi(Math.atan2(deltaY, deltaX) + offsetAngle);

      FramePose2D straightStart = new FramePose2D(startPose);
      straightStart.setYaw(straightYaw);
      FramePose2D straightEnd = new FramePose2D(endPose);
      straightEnd.setYaw(straightYaw);

      overheadPath = new CompositeFootstepOverheadPath(new TurnInPlaceFootstepOverheadPath(startPose, straightStart));
      overheadPath.addPath(new InterpolatedFootstepOverheadPath(straightStart, straightEnd));
      overheadPath.addPath(new TurnInPlaceFootstepOverheadPath(straightEnd, endPose));
   }

   @Override
   public FramePose2D getPoseAtDistance(double distanceAlongPath)
   {
      return overheadPath.getPoseAtDistance(distanceAlongPath);
   }

   @Override
   public double getTotalDistance()
   {
      return overheadPath.getTotalDistance();
   }

   @Override
   public FootstepOverheadPath changeFrameCopy(ReferenceFrame desiredFrame)
   {
      return overheadPath.changeFrameCopy(desiredFrame);
   }

   @Override
   public ReferenceFrame getReferenceFrame()
   {
      return overheadPath.getReferenceFrame();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/SimulationDispatcher.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;

import java.net.MalformedURLException;
import java.rmi.Naming;
import java.rmi.NotBoundException;
import java.rmi.RMISecurityManager;
import java.rmi.RemoteException;
import java.rmi.registry.LocateRegistry;
import java.security.Permission;
import java.util.ArrayList;

import us.ihmc.simulationConstructionSetTools.simulationDispatcher.client.gui.SimulationDispatcherGUI;
import us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces.RemoteSimulationRunnerInterface;

public class SimulationDispatcher implements Runnable
{
   private final String password = "***<PASSWORD>***";
   private final DispatchHostList dispatchHostList;

   private final ArrayList<SimulationToDispatch> simulationsToDispatch = new ArrayList<SimulationToDispatch>();
   private final ArrayList<RemoteSimulationPair> remoteSimulationPairs = new ArrayList<RemoteSimulationPair>();
   private final ArrayList<SimulationToDispatch> doneSimulations = new ArrayList<SimulationToDispatch>();

   private final ArrayList<SimulationsChangedListener> simulationsChangedListeners = new ArrayList<SimulationsChangedListener>();

   public synchronized int getNumberSimulationsToDispatch()
   {
      return simulationsToDispatch.size();
   }

   public synchronized SimulationToDispatch[] getSimulationsToDispatch()
   {
      SimulationToDispatch[] ret = new SimulationToDispatch[simulationsToDispatch.size()];

      simulationsToDispatch.toArray(ret);

      return ret;
   }

   public synchronized SimulationToDispatch[] getDoneSimulations()
   {
      SimulationToDispatch[] ret = new SimulationToDispatch[doneSimulations.size()];

      doneSimulations.toArray(ret);

      return ret;
   }

   public synchronized int getNumberSimulationsCurrentlyDispatched()
   {
      return remoteSimulationPairs.size();
   }


   public SimulationDispatcher(String[] hostNames, String myCodeBase)
   {
      RMISecurityManager laxManager = new RMISecurityManager()
      {
         @Override
         public void checkPermission(Permission p)
         {
            // System.out.println(p);
            // try
            // {super.checkPermission(p);}
            // catch(Exception e)
            {
               // System.out.println(p);
            }
         }
      };


      System.setSecurityManager(laxManager);    // new RMISecurityManager());
      dispatchHostList = new DispatchHostList(this.password);

      for (int i = 0; i < hostNames.length; i++)
      {
         String hostName = hostNames[i];
         int indexOfColon = hostName.indexOf(':');

         String justTheName = hostName.substring(0, indexOfColon);

         int numberOfThreads = Integer.valueOf(hostName.substring(indexOfColon + 1));

         for (int j = 0; j < numberOfThreads; j++)
         {
            dispatchHostList.addHost(justTheName + ":" + j);

         }
      }

      System.out.println("Creating an RMI Registry");

      try
      {
         LocateRegistry.createRegistry(1099);
      }
      catch (RemoteException e)
      {
         System.out.println("Registry already running");
      }


      String codebase = System.getProperty("java.rmi.server.codebase");
      if (codebase == "null")
         codebase = "";

      if (myCodeBase != null)
         codebase = codebase + myCodeBase;

      System.out.println("Setting the RMI codebase to " + codebase);
      System.setProperty("java.rmi.server.codebase", codebase);

      System.out.println("Registering the RemoteIPRegistryServer to my local RMI Registry...");

      try
      {
         RemoteIPRegistryServer server = new RemoteIPRegistryServer(dispatchHostList, this.password);
         Naming.rebind("RemoteIPRegistry", server);
      }
      catch (Exception e)
      {
         System.out.println("Couldn't register the DispatchHostList on my local RMI Registry. Make sure that it is started..." + e.getMessage());
         e.printStackTrace();
      }

      new SimulationDispatcherGUI(this, dispatchHostList, this.password);

      Thread anim = new Thread(this);
      anim.start();
   }

   public synchronized void addSimulationsChangedListener(SimulationsChangedListener listener)
   {
      this.simulationsChangedListeners.add(listener);
   }

   private synchronized void notifyListeners()
   {
      for (int i = 0; i < simulationsChangedListeners.size(); i++)
      {
         ((SimulationsChangedListener) simulationsChangedListeners.get(i)).simulationsChanged();
      }
   }

   private Throwable getRootCause(Throwable throwable)
   {
      while (throwable.getCause() != null)
      {
         throwable = throwable.getCause();
      }

      return throwable;
   }

   @Override
   public void run()
   {
      int doneIndex = 0;

      while (true)
      {
         synchronized (this)
         {
            // Check on the simulations that need to be dispatched and dispatch them:
            if (this.simulationsToDispatch.size() > 0)
            {
               if (dispatchHostList.isHostReady())
               {
                  DispatchHost dispatchHost = dispatchHostList.getNextAvailableDispatchHost();    // This returns null if there is none
                  SimulationToDispatch dispatchSim = simulationsToDispatch.get(0);

                  if (dispatchHost != null)
                  {
                     try
                     {
                        dispatchOneSimulation(dispatchSim, dispatchHost.getRemoteSim());

                        synchronized (this)
                        {
                           remoteSimulationPairs.add(new RemoteSimulationPair(dispatchSim, dispatchHost));
                        }

                        simulationsToDispatch.remove(dispatchSim);
                        dispatchHost.reportSimulationStarted(dispatchSim);
                        this.notifyListeners();
                     }
                     catch (Exception exception)
                     {
                        Throwable rootCause = getRootCause(exception);

                        System.out.println("Host " + dispatchHost.getHostName() + ": " + dispatchHost.getHostProcess() + " is broken!!! \n Root cause = "
                                           + rootCause);

                        dispatchHost.reportBroken();

//                      rootCause.printStackTrace();

                        dispatchHostList.checkInDispatchHost(dispatchHost);
                        simulationsToDispatch.add(dispatchSim);
                        this.notifyListeners();
                     }
                  }
               }
            }


            // Check on the simulations that are running remotely and see if they are done or dead.
            if (remoteSimulationPairs.size() > 0)
            {
               if (doneIndex >= remoteSimulationPairs.size())
                  doneIndex = 0;
               RemoteSimulationPair remotePair = (RemoteSimulationPair) remoteSimulationPairs.get(doneIndex);
               SimulationToDispatch dispatchSim = remotePair.simulationToDispatch;
               DispatchHost dispatchHost = remotePair.dispatchHost;

               try
               {
                  if (dispatchHost.getRemoteSim().isSimulationDone(this.password))
                  {
                     DispatchDoneListener listener = dispatchSim.getDispatchDoneListener();
                     double[] finalState = (double[]) dispatchHost.getRemoteSim().getSimulationState(this.password);
                     dispatchSim.setFinalState(finalState);

                     if (listener != null)
                     {
//                      System.out.println("Dispatch Done. Notifying Listener!");
                        listener.dispatchDone(dispatchSim, finalState);
                     }

                     dispatchHost.reportSimulationFinished();

                     remoteSimulationPairs.remove(remotePair);

                     // System.out.println("Remote Simulation Completed Successfully and is being removed from the list of running simulations!!!");

                     dispatchSim.setSimulationFinished();
                     doneSimulations.add(dispatchSim);

                     dispatchHostList.checkInDispatchHost(dispatchHost);
                     this.notifyListeners();
                  }

                  // else{System.out.println("Sim not done yet...");}
               }
               catch (Exception e)
               {
                  System.out.println(
                      "Oops!! Simulation Died after being put on a server successfully.  Need to put it back in the list of sims to run and return the host");
                  remoteSimulationPairs.remove(remotePair);
                  simulationsToDispatch.add(remotePair.simulationToDispatch);
                  dispatchHostList.checkInDispatchHost(dispatchHost);
                  this.notifyListeners();
               }
            }
         }

         doneIndex++;

         try
         {
            Thread.sleep(500);
         }
         catch (InterruptedException e)
         {
         }

      }

   }

   public synchronized void addSimulation(SimulationToDispatch sim)
   {
      System.out.println("Dispatching Simulation: " + sim.getDescription());
      simulationsToDispatch.add(sim);
      this.notifyListeners();
   }


   private void dispatchOneSimulation(SimulationToDispatch dispatchSim, RemoteSimulationRunnerInterface remoteSim)
           throws RemoteException, MalformedURLException, NotBoundException
   {
      DispatchedSimulationDescription description = new DispatchedSimulationDescription(dispatchSim.getConstructor(), dispatchSim.getInputStateVariableNames(),
                                                       dispatchSim.getOutputStateVariableNames());

      // System.out.println("Creating the Simulation."); System.out.flush();
      remoteSim.createSimulation(description, dispatchSim.getStructuralParameterNames(), dispatchSim.getStructuralParameterValues(), this.password);

      // System.out.println("Setting the Simulation State."); System.out.flush();
      remoteSim.setSimulationState(dispatchSim.getInputState(), this.password);

      // System.out.println("Running the Simulation."); System.out.flush();
      remoteSim.startSimulation(this.password);
   }


   public class RemoteSimulationPair
   {
      private SimulationToDispatch simulationToDispatch;
      private DispatchHost dispatchHost;

      public RemoteSimulationPair(SimulationToDispatch simulationToDispatch, DispatchHost dispatchHost)
      {
         this.simulationToDispatch = simulationToDispatch;
         this.dispatchHost = dispatchHost;
      }
   }



}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/TurnInPlaceFootstepGenerator.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import us.ihmc.euclid.referenceFrame.FrameOrientation2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath.TurningOverheadPath;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.robotSide.SideDependentList;

public class TurnInPlaceFootstepGenerator extends AbstractSimpleParametersFootstepGenerator
{
   private TurningOverheadPath footstepPath;
   private FrameOrientation2D endOrientation;

   public TurnInPlaceFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FrameOrientation2D pathYaw, PathTypeStepParameters pathType)
   {
      super(feet, soleFrames, pathType);
      this.endOrientation = pathYaw;
   }

   protected void initialize(FramePose2D startPose)
   {
      setFootstepPath(new TurningOverheadPath(startPose, endOrientation));
      footstepCounter = new FootstepCounterForSingleTurnPath();
   }

   protected void initializeFootstepCounter(double openingAngle, double closingAngle, double stepLength, boolean hipExtensionFirst,
           boolean willSetFirstStraightStepToFarSideIfNoTurns, double standardFootWidth)
   {
      ((FootstepCounterForSingleTurnPath) footstepCounter).initialize(openingAngle, closingAngle, getSignedInitialTurnDirection(), hipExtensionFirst, initialDeltaFeetYaw);
   }

   protected double getSignedInitialTurnDirection()
   {
      startPose.checkReferenceFrameMatch(endOrientation);
      FrameOrientation2D startOrientation = new FrameOrientation2D(startPose.getOrientation());
      double deltaYaw = endOrientation.difference(startOrientation);

      return deltaYaw;
   }

   public void setFootstepPath(TurningOverheadPath turningOverheadPath)
   {
      this.footstepPath = turningOverheadPath;
   }

   protected TurningOverheadPath getPath()
   {
      return footstepPath;
   }

   public double getTurningAmount()
   {
      return getSignedInitialTurnDirection();
   }
   
   public boolean hasDisplacement()
   {
      return (Math.abs(getTurningAmount()) > noTranslationTolerance);
   }
   
   public boolean turnStepsTaken() {
      return ((FootstepCounterForSingleTurnPath) footstepCounter).turnStepsTaken();
   }

   public double getOvershootStepFraction() {
      return ((FootstepCounterForSingleTurnPath) footstepCounter).getOvershootStepFraction();
   }

}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/bootstrap/MessageAndServiceInterfaceGenerator.java<|end_filename|>
package us.ihmc.utilities.ros.bootstrap;

import java.io.File;
import java.io.IOException;
import java.util.Collection;
import java.util.List;

import org.ros.internal.message.GenerateInterfaces;

import com.google.common.collect.Lists;

import ihmc_msgs.FootstepDataListRosMessage;
import ihmc_msgs.WholeBodyTrajectoryRosMessage;

/**
 * Regenerates the ROSJava messages from ROS messages ".msg".
 * <p>
 * This is the second step when updating the ROS API. The first step is DRCROSMessageGenerator.
 * </p>
 * <p>
 * The program arguments should be something like : "../ihmc-ros-tools/generated-src ROSMessagesAndServices"
 * </p>
 * <p>
 * For most messages, this will be enough, but for some other like
 * {@link WholeBodyTrajectoryRosMessage} and {@link FootstepDataListRosMessage}, there is a last
 * step which is about updating IHMCROSTranslationRuntimeTools.
 * </p>
 */
public class MessageAndServiceInterfaceGenerator
{

   public static void main(String[] args) throws IOException
   {
      List<String> arguments = Lists.newArrayList(args);
      if (arguments.size() < 2)
      {
         System.err.println("insufficient Arguments. Generating the Interfaces from Spoof requires at least two arguments:\n"
               + "First: the output directory for the interface files\n" + "Second and following: merged together to form the ros package path");
         return;
      }
      File outputDirectory = new File(arguments.remove(0));

      Collection<File> packagePath = Lists.newArrayList();
      for (int i = 0; i < arguments.size(); i++)
      {
         packagePath.add(new File(arguments.remove(0)));
      }

      log("InterfaceFromSpoof: packagePath is " + packagePath);
      GenerateInterfaces generateInterfaces = new GenerateInterfaces();
      generateInterfaces.generate(outputDirectory, arguments, packagePath);
   }

   private static void log(String aMessage)
   {
      System.out.println(aMessage);
   }

}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/contactPoints/NoOffsetFrictionConeCalculator.java<|end_filename|>
package us.ihmc.wholeBodyController.contactPoints;

import java.util.List;

import us.ihmc.euclid.tuple2D.Point2D;

public class NoOffsetFrictionConeCalculator implements FrictionConeRotationCalculator
{

   @Override
   public double computeYawOffset(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx)
   {
      return 0.0;
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/InterpolatedFootstepOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple2D.Point2D;

/**
 * Created by agrabertilton on 2/20/15.
 */
public class InterpolatedFootstepOverheadPath extends FootstepOverheadPath
{
   FramePose2D startPose;
   FramePose2D endPose;
   double distance;

   public InterpolatedFootstepOverheadPath(FramePose2D startPose, FramePose2D endPose)
   {
      startPose.checkReferenceFrameMatch(endPose);
      this.startPose = new FramePose2D(startPose);
      this.endPose = new FramePose2D(endPose);
      double deltaX = endPose.getX() - startPose.getX();
      double deltaY = endPose.getY() - startPose.getY();
      distance = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
   }

   @Override
   public FramePose2D getPoseAtDistance(double distanceAlongPath)
   {
      double interpolationValue = distanceAlongPath / distance;
      interpolationValue = Math.max(interpolationValue, 0.0);
      interpolationValue = Math.min(interpolationValue, 1.0);
      double x = (1-interpolationValue) * startPose.getX() + (interpolationValue) * endPose.getX();
      double y = (1-interpolationValue) * startPose.getY() + (interpolationValue) * endPose.getY();
      double yaw = (1-interpolationValue) * startPose.getYaw() + (interpolationValue) * endPose.getYaw();
      return new FramePose2D(startPose.getReferenceFrame(), new Point2D(x,y), yaw);
   }

   @Override
   public double getTotalDistance()
   {
      return distance;
   }

   @Override
   public FootstepOverheadPath changeFrameCopy(ReferenceFrame desiredFrame)
   {
      FramePose2D newStartPose = new FramePose2D(startPose);
      newStartPose.changeFrame(desiredFrame);
      FramePose2D newEndPose = new FramePose2D(endPose);
      newEndPose.changeFrame(desiredFrame);
      return new InterpolatedFootstepOverheadPath(newStartPose, newEndPose);
   }

   @Override
   public ReferenceFrame getReferenceFrame()
   {
      return startPose.getReferenceFrame();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedBodyHeightMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "QuadrupedBodyHeightMessage" defined in "QuadrupedBodyHeightMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from QuadrupedBodyHeightMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit QuadrupedBodyHeightMessage_.idl instead.
*
*/
public class QuadrupedBodyHeightMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.QuadrupedBodyHeightMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::QuadrupedBodyHeightMessage_";

   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.QuadrupedBodyHeightMessage 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.QuadrupedBodyHeightMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.QuadrupedBodyHeightMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.QuadrupedBodyHeightMessage data, int current_alignment)
   {
      int initial_alignment = 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.getCdrSerializedSize(data.getEuclideanTrajectory(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.QuadrupedBodyHeightMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_7(data.getIsExpressedInAbsoluteTime());

      cdr.write_type_7(data.getControlBodyHeight());

      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.write(data.getEuclideanTrajectory(), cdr);
   }

   public static void read(controller_msgs.msg.dds.QuadrupedBodyHeightMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setIsExpressedInAbsoluteTime(cdr.read_type_7());
      	
      data.setControlBodyHeight(cdr.read_type_7());
      	
      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.read(data.getEuclideanTrajectory(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.QuadrupedBodyHeightMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_7("is_expressed_in_absolute_time", data.getIsExpressedInAbsoluteTime());
      ser.write_type_7("control_body_height", data.getControlBodyHeight());
      ser.write_type_a("euclidean_trajectory", new controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType(), data.getEuclideanTrajectory());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.QuadrupedBodyHeightMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setIsExpressedInAbsoluteTime(ser.read_type_7("is_expressed_in_absolute_time"));
      data.setControlBodyHeight(ser.read_type_7("control_body_height"));
      ser.read_type_a("euclidean_trajectory", new controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType(), data.getEuclideanTrajectory());

   }

   public static void staticCopy(controller_msgs.msg.dds.QuadrupedBodyHeightMessage src, controller_msgs.msg.dds.QuadrupedBodyHeightMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.QuadrupedBodyHeightMessage createData()
   {
      return new controller_msgs.msg.dds.QuadrupedBodyHeightMessage();
   }
   @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.QuadrupedBodyHeightMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.QuadrupedBodyHeightMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.QuadrupedBodyHeightMessage src, controller_msgs.msg.dds.QuadrupedBodyHeightMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public QuadrupedBodyHeightMessagePubSubType newInstance()
   {
      return new QuadrupedBodyHeightMessagePubSubType();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PilotInterfacePacket.java<|end_filename|>
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;

public class PilotInterfacePacket extends Packet<PilotInterfacePacket> implements Settable<PilotInterfacePacket>, EpsilonComparable<PilotInterfacePacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int behaviour_state_;
   public int requested_behavior_state_;
   public int desired_step_type_;
   public int desired_step_length_type_;
   public int desired_step_stairs_type_;
   public boolean desired_step_continous_walk_;
   public int desired_steps_to_take_;
   public boolean execute_behavior_;
   public int desired_slope_step_type_;
   public int current_pilot_state_;

   public PilotInterfacePacket()
   {
   }

   public PilotInterfacePacket(PilotInterfacePacket other)
   {
      this();
      set(other);
   }

   public void set(PilotInterfacePacket other)
   {
      sequence_id_ = other.sequence_id_;

      behaviour_state_ = other.behaviour_state_;

      requested_behavior_state_ = other.requested_behavior_state_;

      desired_step_type_ = other.desired_step_type_;

      desired_step_length_type_ = other.desired_step_length_type_;

      desired_step_stairs_type_ = other.desired_step_stairs_type_;

      desired_step_continous_walk_ = other.desired_step_continous_walk_;

      desired_steps_to_take_ = other.desired_steps_to_take_;

      execute_behavior_ = other.execute_behavior_;

      desired_slope_step_type_ = other.desired_slope_step_type_;

      current_pilot_state_ = other.current_pilot_state_;

   }

   /**
            * 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 void setBehaviourState(int behaviour_state)
   {
      behaviour_state_ = behaviour_state;
   }
   public int getBehaviourState()
   {
      return behaviour_state_;
   }

   public void setRequestedBehaviorState(int requested_behavior_state)
   {
      requested_behavior_state_ = requested_behavior_state;
   }
   public int getRequestedBehaviorState()
   {
      return requested_behavior_state_;
   }

   public void setDesiredStepType(int desired_step_type)
   {
      desired_step_type_ = desired_step_type;
   }
   public int getDesiredStepType()
   {
      return desired_step_type_;
   }

   public void setDesiredStepLengthType(int desired_step_length_type)
   {
      desired_step_length_type_ = desired_step_length_type;
   }
   public int getDesiredStepLengthType()
   {
      return desired_step_length_type_;
   }

   public void setDesiredStepStairsType(int desired_step_stairs_type)
   {
      desired_step_stairs_type_ = desired_step_stairs_type;
   }
   public int getDesiredStepStairsType()
   {
      return desired_step_stairs_type_;
   }

   public void setDesiredStepContinousWalk(boolean desired_step_continous_walk)
   {
      desired_step_continous_walk_ = desired_step_continous_walk;
   }
   public boolean getDesiredStepContinousWalk()
   {
      return desired_step_continous_walk_;
   }

   public void setDesiredStepsToTake(int desired_steps_to_take)
   {
      desired_steps_to_take_ = desired_steps_to_take;
   }
   public int getDesiredStepsToTake()
   {
      return desired_steps_to_take_;
   }

   public void setExecuteBehavior(boolean execute_behavior)
   {
      execute_behavior_ = execute_behavior;
   }
   public boolean getExecuteBehavior()
   {
      return execute_behavior_;
   }

   public void setDesiredSlopeStepType(int desired_slope_step_type)
   {
      desired_slope_step_type_ = desired_slope_step_type;
   }
   public int getDesiredSlopeStepType()
   {
      return desired_slope_step_type_;
   }

   public void setCurrentPilotState(int current_pilot_state)
   {
      current_pilot_state_ = current_pilot_state;
   }
   public int getCurrentPilotState()
   {
      return current_pilot_state_;
   }


   public static Supplier<PilotInterfacePacketPubSubType> getPubSubType()
   {
      return PilotInterfacePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PilotInterfacePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PilotInterfacePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.behaviour_state_, other.behaviour_state_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.requested_behavior_state_, other.requested_behavior_state_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_step_type_, other.desired_step_type_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_step_length_type_, other.desired_step_length_type_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_step_stairs_type_, other.desired_step_stairs_type_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.desired_step_continous_walk_, other.desired_step_continous_walk_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_steps_to_take_, other.desired_steps_to_take_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.execute_behavior_, other.execute_behavior_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_slope_step_type_, other.desired_slope_step_type_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.current_pilot_state_, other.current_pilot_state_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PilotInterfacePacket)) return false;

      PilotInterfacePacket otherMyClass = (PilotInterfacePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.behaviour_state_ != otherMyClass.behaviour_state_) return false;

      if(this.requested_behavior_state_ != otherMyClass.requested_behavior_state_) return false;

      if(this.desired_step_type_ != otherMyClass.desired_step_type_) return false;

      if(this.desired_step_length_type_ != otherMyClass.desired_step_length_type_) return false;

      if(this.desired_step_stairs_type_ != otherMyClass.desired_step_stairs_type_) return false;

      if(this.desired_step_continous_walk_ != otherMyClass.desired_step_continous_walk_) return false;

      if(this.desired_steps_to_take_ != otherMyClass.desired_steps_to_take_) return false;

      if(this.execute_behavior_ != otherMyClass.execute_behavior_) return false;

      if(this.desired_slope_step_type_ != otherMyClass.desired_slope_step_type_) return false;

      if(this.current_pilot_state_ != otherMyClass.current_pilot_state_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PilotInterfacePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("behaviour_state=");
      builder.append(this.behaviour_state_);      builder.append(", ");
      builder.append("requested_behavior_state=");
      builder.append(this.requested_behavior_state_);      builder.append(", ");
      builder.append("desired_step_type=");
      builder.append(this.desired_step_type_);      builder.append(", ");
      builder.append("desired_step_length_type=");
      builder.append(this.desired_step_length_type_);      builder.append(", ");
      builder.append("desired_step_stairs_type=");
      builder.append(this.desired_step_stairs_type_);      builder.append(", ");
      builder.append("desired_step_continous_walk=");
      builder.append(this.desired_step_continous_walk_);      builder.append(", ");
      builder.append("desired_steps_to_take=");
      builder.append(this.desired_steps_to_take_);      builder.append(", ");
      builder.append("execute_behavior=");
      builder.append(this.execute_behavior_);      builder.append(", ");
      builder.append("desired_slope_step_type=");
      builder.append(this.desired_slope_step_type_);      builder.append(", ");
      builder.append("current_pilot_state=");
      builder.append(this.current_pilot_state_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/jfxvisualizer/AtlasJavaFxRobotVisualizer.java<|end_filename|>
package us.ihmc.atlas.jfxvisualizer;

import java.io.IOException;

import javafx.animation.AnimationTimer;
import javafx.application.Application;
import javafx.scene.AmbientLight;
import javafx.scene.paint.Color;
import javafx.stage.Stage;
import us.ihmc.atlas.AtlasRobotModel;
import us.ihmc.atlas.AtlasRobotModelFactory;
import us.ihmc.atlas.AtlasRobotVersion;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.graphicsDescription.structure.Graphics3DNode;
import us.ihmc.javaFXToolkit.node.JavaFXGraphics3DNode;
import us.ihmc.javaFXToolkit.scenes.View3DFactory;
import us.ihmc.simulationConstructionSetTools.util.HumanoidFloatingRootJointRobot;
import us.ihmc.simulationconstructionset.OneDegreeOfFreedomJoint;
import us.ihmc.simulationconstructionset.graphics.GraphicsRobot;

public class AtlasJavaFxRobotVisualizer extends Application
{
   private HumanoidFloatingRootJointRobot humanoidFloatingRootJointRobot;
   private GraphicsRobot graphicsRobot;
   private JavaFXGraphics3DNode rootNode;
   private double t;

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      primaryStage.setTitle("SDF Viewer");

      AtlasRobotModel robotModel = AtlasRobotModelFactory.createDefaultRobotModel();
      humanoidFloatingRootJointRobot = robotModel.createHumanoidFloatingRootJointRobot(false);

      graphicsRobot = new GraphicsRobot(humanoidFloatingRootJointRobot);
      graphicsRobot.update();

      rootNode = new JavaFXGraphics3DNode(graphicsRobot.getRootNode());
      addNodesRecursively(graphicsRobot.getRootNode(), rootNode);
      rootNode.update();

      View3DFactory view3DFactory = new View3DFactory(1024, 768);
      view3DFactory.addCameraController();
//      view3DFactory.setBackgroundColor(Color.WHITE);
      view3DFactory.addNodeToView(new AmbientLight(Color.WHITE));
      view3DFactory.addPointLight(5, 0, 2, Color.GRAY);
      view3DFactory.addPointLight(-5, 0, 2, Color.GRAY);
      view3DFactory.addNodeToView(rootNode);

      primaryStage.setScene(view3DFactory.getScene());
      primaryStage.show();

      AnimationTimer animationTimer = new AnimationTimer()
      {

         @Override
         public void handle(long arg0)
         {
            updateRobotAndGraphics();
         }
      };
      animationTimer.start();

   }

   public void addNodesRecursively(Graphics3DNode graphics3dNode, JavaFXGraphics3DNode parentNode)
   {
      JavaFXGraphics3DNode node = new JavaFXGraphics3DNode(graphics3dNode);
      parentNode.addChild(node);

      for (Graphics3DNode child : graphics3dNode.getChildrenNodes())
      {
         addNodesRecursively(child, node);
      }
   }

   public void updateRobotAndGraphics()
   {
      OneDegreeOfFreedomJoint[] oneDegreeOfFreedomJoints = humanoidFloatingRootJointRobot.getOneDegreeOfFreedomJoints();
//      for (int i = 0; i < oneDegreeOfFreedomJoints.length; i++)
//      {
//         t += 0.001;
//         double q = Math.sin(t);
//         oneDegreeOfFreedomJoints[i].setQ(q);
//      }
      humanoidFloatingRootJointRobot.update();
      graphicsRobot.update();
      rootNode.update();
   }

   public static void main(String[] args) throws IOException
   {
      launch(args);
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedTimedStepMessage.java<|end_filename|>
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;

public class QuadrupedTimedStepMessage extends Packet<QuadrupedTimedStepMessage> implements Settable<QuadrupedTimedStepMessage>, EpsilonComparable<QuadrupedTimedStepMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public controller_msgs.msg.dds.TimeIntervalMessage time_interval_;
   public controller_msgs.msg.dds.QuadrupedStepMessage quadruped_step_message_;

   public QuadrupedTimedStepMessage()
   {
      time_interval_ = new controller_msgs.msg.dds.TimeIntervalMessage();
      quadruped_step_message_ = new controller_msgs.msg.dds.QuadrupedStepMessage();
   }

   public QuadrupedTimedStepMessage(QuadrupedTimedStepMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedTimedStepMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.TimeIntervalMessagePubSubType.staticCopy(other.time_interval_, time_interval_);
      controller_msgs.msg.dds.QuadrupedStepMessagePubSubType.staticCopy(other.quadruped_step_message_, quadruped_step_message_);
   }

   /**
            * 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 controller_msgs.msg.dds.TimeIntervalMessage getTimeInterval()
   {
      return time_interval_;
   }


   public controller_msgs.msg.dds.QuadrupedStepMessage getQuadrupedStepMessage()
   {
      return quadruped_step_message_;
   }


   public static Supplier<QuadrupedTimedStepMessagePubSubType> getPubSubType()
   {
      return QuadrupedTimedStepMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedTimedStepMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedTimedStepMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.time_interval_.epsilonEquals(other.time_interval_, epsilon)) return false;
      if (!this.quadruped_step_message_.epsilonEquals(other.quadruped_step_message_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedTimedStepMessage)) return false;

      QuadrupedTimedStepMessage otherMyClass = (QuadrupedTimedStepMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.time_interval_.equals(otherMyClass.time_interval_)) return false;
      if (!this.quadruped_step_message_.equals(otherMyClass.quadruped_step_message_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedTimedStepMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("time_interval=");
      builder.append(this.time_interval_);      builder.append(", ");
      builder.append("quadruped_step_message=");
      builder.append(this.quadruped_step_message_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/YoPID3DGains.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains;

import us.ihmc.yoVariables.variable.YoDouble;

/**
 * An extension of the {@link PID3DGains} interface that provides additional access
 * to YoVariables in the implementation.
 */
public interface YoPID3DGains extends PID3DGains
{
   /**
    * Returns the maximum allowed controller output as a YoVariable.
    *
    * @return the maximum output.
    */
   public abstract YoDouble getYoMaximumFeedback();

   /**
    * Returns the maximum allowed controller output rate as a YoVariable.
    *
    * @return the maximum output rate.
    */
   public abstract YoDouble getYoMaximumFeedbackRate();

   /**
    * Returns the maximum error in the derivative input to the controller that
    * should be considered as a YoVariable.
    *
    * @return the maximum derivative error.
    */
   public abstract YoDouble getYoMaximumDerivativeError();

   /**
    * Returns the maximum error in the proportional input to the controller that
    * should be considered as a YoVariable.
    *
    * @return the maximum proportional error.
    */
   public abstract YoDouble getYoMaximumProportionalError();
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/commandrecorders/RecordedDataInputOutputStream.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication.commandrecorders;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

public class RecordedDataInputOutputStream
{
   private DataInputStream in;
   private DataOutputStream out;

   private StringBuffer buff = new StringBuffer();

   public RecordedDataInputOutputStream(DataInputStream in, DataOutputStream out)
   {
      this.in = in;
      this.out = out;
   }

   public void close() throws IOException
   {
      in.close();
   }

   public byte readByte() throws IOException
   {
      byte ret = in.readByte();

      buff.append("byte: ");
      buff.append(ret);
      buff.append("\n");

      return ret;
   }

   public int readInt() throws IOException
   {
      int ret = in.readInt();

      buff.append("int: ");
      buff.append(ret);
      buff.append("\n");

      return ret;
   }

   public String readUTF() throws IOException
   {
      String ret = in.readUTF();

      buff.append("String: ");
      buff.append(ret);
      buff.append("\n");

      return ret;
   }

   public float readFloat() throws IOException
   {
      float ret = in.readFloat();

      buff.append("float: ");
      buff.append(ret);
      buff.append("\n");

      return ret;
   }


   // Out Methods:

   public void flush() throws IOException
   {
      out.flush();
   }

   public void writeInt(int i) throws IOException
   {
      out.writeInt(i);
   }

   public void writeUTF(String s) throws IOException
   {
      out.writeUTF(s);
   }

   public void writeFloat(float f) throws IOException
   {
      out.writeFloat(f);
   }


   public void main(String[] args)
   {
      @SuppressWarnings("unused")
      RecordedDataInputOutputStream stream = new RecordedDataInputOutputStream(null, null);
   }

   public StringBuffer getBuffer()
   {
      return buff;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/converters/BoundingBoxMessageConverter.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.converters;

import java.util.Scanner;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple4D.Quaternion;

import us.ihmc.euclid.tuple3D.Vector3D;

import us.ihmc.jOctoMap.boundingBox.OcTreeBoundingBoxInterface;
import us.ihmc.jOctoMap.boundingBox.OcTreeBoundingBoxWithCenterAndYaw;
import us.ihmc.jOctoMap.boundingBox.OcTreeSimpleBoundingBox;
import us.ihmc.robotEnvironmentAwareness.communication.packets.BoundingBoxParametersMessage;
import us.ihmc.robotEnvironmentAwareness.communication.packets.BoxMessage;

public class BoundingBoxMessageConverter
{

   public static BoxMessage convertToMessage(OcTreeBoundingBoxInterface boundingBox)
   {
      if (boundingBox == null)
         return BoxMessage.emptyBox();

      if (boundingBox instanceof OcTreeBoundingBoxWithCenterAndYaw)
         return convertToMessage((OcTreeBoundingBoxWithCenterAndYaw) boundingBox);
      else if (boundingBox instanceof OcTreeSimpleBoundingBox)
         return convertToMessage((OcTreeSimpleBoundingBox) boundingBox);
      else
         throw new RuntimeException("No conversion implemented for this bounding box: " + boundingBox.getClass().getSimpleName());
   }

   public static BoxMessage convertToMessage(OcTreeSimpleBoundingBox boundingBox)
   {
      Quaternion orientation = new Quaternion(0.0, 0.0, 0.0, 1.0);
      Point3D center = new Point3D();
      Vector3D size = new Vector3D();
      boundingBox.getSize(size);
      boundingBox.getCenterCoordinate(center);

      BoxMessage boxMessage = new BoxMessage();
      boxMessage.setCenter(center);
      boxMessage.setOrientation(orientation);
      boxMessage.setSize(size);
      return boxMessage;
   }

   public static BoxMessage convertToMessage(OcTreeBoundingBoxWithCenterAndYaw boundingBox)
   {
      Point3D center = new Point3D();
      Vector3D size = new Vector3D();
      boundingBox.getLocalSize(size);
      boundingBox.getCenterCoordinate(center);
      Quaternion orientation = new Quaternion(0.0, 0.0, Math.sin(0.5 * boundingBox.getYaw()), Math.cos(0.5 * boundingBox.getYaw()));

      BoxMessage boxMessage = new BoxMessage();
      boxMessage.setCenter(center);
      boxMessage.setOrientation(orientation);
      boxMessage.setSize(size);
      return boxMessage;
   }

   public static BoundingBoxParametersMessage createBoundingBoxParametersMessage(float minX, float minY, float minZ, float maxX, float maxY, float maxZ)
   {
      BoundingBoxParametersMessage message = new BoundingBoxParametersMessage();

      message.minX = minX;
      message.minY = minY;
      message.minZ = minZ;
      message.maxX = maxX;
      message.maxY = maxY;
      message.maxZ = maxZ;

      return message;
   }

   public static BoundingBoxParametersMessage parse(String boundingBoxAsString)
   {
      boundingBoxAsString = boundingBoxAsString.replace("(", "").replace(")", " ").replace(",", "");
      Scanner scanner = new Scanner(boundingBoxAsString);
      scanner.next();
      float minX = scanner.nextFloat();
      float minY = scanner.nextFloat();
      float minZ = scanner.nextFloat();
      scanner.next();
      float maxX = scanner.nextFloat();
      float maxY = scanner.nextFloat();
      float maxZ = scanner.nextFloat();
      scanner.close();
      return createBoundingBoxParametersMessage(minX, minY, minZ, maxX, maxY, maxZ);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WrenchTrajectoryMessage.java<|end_filename|>
package controller_msgs.msg.dds;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.euclid.interfaces.EpsilonComparable;
import java.util.function.Supplier;
import us.ihmc.pubsub.TopicDataType;

/**
       * This message is part of the IHMC whole-body controller API.
       * This class is used to build a wrench (force & moment) profile over time.
       * A linear interpolation is used to interpolate between trajectory points.
       */
public class WrenchTrajectoryMessage extends Packet<WrenchTrajectoryMessage> implements Settable<WrenchTrajectoryMessage>, EpsilonComparable<WrenchTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * List of trajectory points (in taskpsace) to go through while executing the trajectory.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.WrenchTrajectoryPointMessage>  wrench_trajectory_points_;
   /**
            * Frame information for this message.
            */
   public controller_msgs.msg.dds.FrameInformation frame_information_;
   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public boolean use_custom_control_frame_;
   /**
            * Pose of custom control frame expressed in the end-effector frame.
            * This is the frame attached to the rigid body that the wrench trajectory is defined for.
            */
   public us.ihmc.euclid.geometry.Pose3D control_frame_pose_;
   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage queueing_properties_;

   public WrenchTrajectoryMessage()
   {
      wrench_trajectory_points_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.WrenchTrajectoryPointMessage> (50, new controller_msgs.msg.dds.WrenchTrajectoryPointMessagePubSubType());
      frame_information_ = new controller_msgs.msg.dds.FrameInformation();
      control_frame_pose_ = new us.ihmc.euclid.geometry.Pose3D();
      queueing_properties_ = new controller_msgs.msg.dds.QueueableMessage();

   }

   public WrenchTrajectoryMessage(WrenchTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(WrenchTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      wrench_trajectory_points_.set(other.wrench_trajectory_points_);
      controller_msgs.msg.dds.FrameInformationPubSubType.staticCopy(other.frame_information_, frame_information_);
      use_custom_control_frame_ = other.use_custom_control_frame_;

      geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.control_frame_pose_, control_frame_pose_);
      controller_msgs.msg.dds.QueueableMessagePubSubType.staticCopy(other.queueing_properties_, queueing_properties_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * List of trajectory points (in taskpsace) to go through while executing the trajectory.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.WrenchTrajectoryPointMessage>  getWrenchTrajectoryPoints()
   {
      return wrench_trajectory_points_;
   }


   /**
            * Frame information for this message.
            */
   public controller_msgs.msg.dds.FrameInformation getFrameInformation()
   {
      return frame_information_;
   }

   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public void setUseCustomControlFrame(boolean use_custom_control_frame)
   {
      use_custom_control_frame_ = use_custom_control_frame;
   }
   /**
            * Flag that tells the controller whether the use of a custom control frame is requested.
            */
   public boolean getUseCustomControlFrame()
   {
      return use_custom_control_frame_;
   }


   /**
            * Pose of custom control frame expressed in the end-effector frame.
            * This is the frame attached to the rigid body that the wrench trajectory is defined for.
            */
   public us.ihmc.euclid.geometry.Pose3D getControlFramePose()
   {
      return control_frame_pose_;
   }


   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage getQueueingProperties()
   {
      return queueing_properties_;
   }


   public static Supplier<WrenchTrajectoryMessagePubSubType> getPubSubType()
   {
      return WrenchTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WrenchTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WrenchTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.wrench_trajectory_points_.size() != other.wrench_trajectory_points_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.wrench_trajectory_points_.size(); i++)
         {  if (!this.wrench_trajectory_points_.get(i).epsilonEquals(other.wrench_trajectory_points_.get(i), epsilon)) return false; }
      }

      if (!this.frame_information_.epsilonEquals(other.frame_information_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_custom_control_frame_, other.use_custom_control_frame_, epsilon)) return false;

      if (!this.control_frame_pose_.epsilonEquals(other.control_frame_pose_, epsilon)) return false;
      if (!this.queueing_properties_.epsilonEquals(other.queueing_properties_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WrenchTrajectoryMessage)) return false;

      WrenchTrajectoryMessage otherMyClass = (WrenchTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.wrench_trajectory_points_.equals(otherMyClass.wrench_trajectory_points_)) return false;
      if (!this.frame_information_.equals(otherMyClass.frame_information_)) return false;
      if(this.use_custom_control_frame_ != otherMyClass.use_custom_control_frame_) return false;

      if (!this.control_frame_pose_.equals(otherMyClass.control_frame_pose_)) return false;
      if (!this.queueing_properties_.equals(otherMyClass.queueing_properties_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WrenchTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("wrench_trajectory_points=");
      builder.append(this.wrench_trajectory_points_);      builder.append(", ");
      builder.append("frame_information=");
      builder.append(this.frame_information_);      builder.append(", ");
      builder.append("use_custom_control_frame=");
      builder.append(this.use_custom_control_frame_);      builder.append(", ");
      builder.append("control_frame_pose=");
      builder.append(this.control_frame_pose_);      builder.append(", ");
      builder.append("queueing_properties=");
      builder.append(this.queueing_properties_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KinematicsPlanningToolboxRigidBodyMessage.java<|end_filename|>
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;

public class KinematicsPlanningToolboxRigidBodyMessage extends Packet<KinematicsPlanningToolboxRigidBodyMessage> implements Settable<KinematicsPlanningToolboxRigidBodyMessage>, EpsilonComparable<KinematicsPlanningToolboxRigidBodyMessage>
{
   /**
            * This message is part of the IHMC whole-body inverse kinematics module.
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int end_effector_hash_code_;
   /**
            * This is the list of desired times for each key frames.
            */
   public us.ihmc.idl.IDLSequence.Double  key_frame_times_;
   /**
            * This is the list of desired key frames for end effector.
            */
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.geometry.Pose3D>  key_frame_poses_;
   /**
            * The selection frames coming along with the given selection matrix are used to determine to
            * what reference frame the selected axes are referring to. For instance, if only the hand height
            * in world should be controlled on the linear z component of the selection matrix should be
            * selected and the reference frame should be world frame. When no reference frame is provided
            * with the selection matrix, it will be used as it is in the control frame, i.e. the body-fixed
            * frame if not defined otherwise.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage angular_selection_matrix_;
   /**
            * The selection matrix is used to determinate which degree of freedom of the end-effector should
            * be controlled.
            * The selection frames coming along with the given selection matrix are used to determine to
            * what reference frame the selected axes are referring to. For instance, if only the hand height
            * in world should be controlled on the linear z component of the selection matrix should be
            * selected and the reference frame should be world frame. When no reference frame is provided
            * with the selection matrix, it will be used as it is in the control frame, i.e. the body-fixed
            * frame if not defined otherwise.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage linear_selection_matrix_;
   /**
            * Weight matrix used to define the priority of controlling the rotation around each axis on the solver side.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage angular_weight_matrix_;
   /**
            * Weight matrix used to define the priority of controlling the translation around each axis on the solver side.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage linear_weight_matrix_;
   /**
            * This is the position of the control frame's origin expressed in endEffector.getBodyFixedFrame().
            * By default, the control frame is coincident to endEffector.getBodyFixedFrame().
            * The control frame is rigidly attached to the end-effector.
            */
   public us.ihmc.euclid.tuple3D.Point3D control_frame_position_in_end_effector_;
   /**
            * This is the orientation of the control frame expressed in endEffector.getBodyFixedFrame().
            * By default, the control frame is coincident to endEffector.getBodyFixedFrame().
            */
   public us.ihmc.euclid.tuple4D.Quaternion control_frame_orientation_in_end_effector_;
   /**
            * This is the allowable displacement of the position for each key frame.
            * By default, the solver will try to find a solution without modifying the position of the key frames.
            * When a positive value is provided, the solver may adjust a key frame to improve the overall solution quality.
            */
   public us.ihmc.idl.IDLSequence.Double  allowable_position_displacement_;
   /**
            * This is the allowable displacement of the orientation for each key frame.
            * By default, the solver will try to find a solution without modifying the orientation of the key frames.
            * When a positive value is provided, the solver may adjust a key frame to improve the overall solution quality.
            */
   public us.ihmc.idl.IDLSequence.Double  allowable_orientation_displacement_;

   public KinematicsPlanningToolboxRigidBodyMessage()
   {
      key_frame_times_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      key_frame_poses_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.geometry.Pose3D> (100, new geometry_msgs.msg.dds.PosePubSubType());
      angular_selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      linear_selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      angular_weight_matrix_ = new controller_msgs.msg.dds.WeightMatrix3DMessage();
      linear_weight_matrix_ = new controller_msgs.msg.dds.WeightMatrix3DMessage();
      control_frame_position_in_end_effector_ = new us.ihmc.euclid.tuple3D.Point3D();
      control_frame_orientation_in_end_effector_ = new us.ihmc.euclid.tuple4D.Quaternion();
      allowable_position_displacement_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      allowable_orientation_displacement_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");


   }

   public KinematicsPlanningToolboxRigidBodyMessage(KinematicsPlanningToolboxRigidBodyMessage other)
   {
      this();
      set(other);
   }

   public void set(KinematicsPlanningToolboxRigidBodyMessage other)
   {
      sequence_id_ = other.sequence_id_;

      end_effector_hash_code_ = other.end_effector_hash_code_;

      key_frame_times_.set(other.key_frame_times_);
      key_frame_poses_.set(other.key_frame_poses_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.angular_selection_matrix_, angular_selection_matrix_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.linear_selection_matrix_, linear_selection_matrix_);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.staticCopy(other.angular_weight_matrix_, angular_weight_matrix_);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.staticCopy(other.linear_weight_matrix_, linear_weight_matrix_);
      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.control_frame_position_in_end_effector_, control_frame_position_in_end_effector_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.control_frame_orientation_in_end_effector_, control_frame_orientation_in_end_effector_);
      allowable_position_displacement_.set(other.allowable_position_displacement_);
      allowable_orientation_displacement_.set(other.allowable_orientation_displacement_);
   }

   /**
            * This message is part of the IHMC whole-body inverse kinematics module.
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * This message is part of the IHMC whole-body inverse kinematics module.
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }

   public void setEndEffectorHashCode(int end_effector_hash_code)
   {
      end_effector_hash_code_ = end_effector_hash_code;
   }
   public int getEndEffectorHashCode()
   {
      return end_effector_hash_code_;
   }


   /**
            * This is the list of desired times for each key frames.
            */
   public us.ihmc.idl.IDLSequence.Double  getKeyFrameTimes()
   {
      return key_frame_times_;
   }


   /**
            * This is the list of desired key frames for end effector.
            */
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.geometry.Pose3D>  getKeyFramePoses()
   {
      return key_frame_poses_;
   }


   /**
            * The selection frames coming along with the given selection matrix are used to determine to
            * what reference frame the selected axes are referring to. For instance, if only the hand height
            * in world should be controlled on the linear z component of the selection matrix should be
            * selected and the reference frame should be world frame. When no reference frame is provided
            * with the selection matrix, it will be used as it is in the control frame, i.e. the body-fixed
            * frame if not defined otherwise.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage getAngularSelectionMatrix()
   {
      return angular_selection_matrix_;
   }


   /**
            * The selection matrix is used to determinate which degree of freedom of the end-effector should
            * be controlled.
            * The selection frames coming along with the given selection matrix are used to determine to
            * what reference frame the selected axes are referring to. For instance, if only the hand height
            * in world should be controlled on the linear z component of the selection matrix should be
            * selected and the reference frame should be world frame. When no reference frame is provided
            * with the selection matrix, it will be used as it is in the control frame, i.e. the body-fixed
            * frame if not defined otherwise.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage getLinearSelectionMatrix()
   {
      return linear_selection_matrix_;
   }


   /**
            * Weight matrix used to define the priority of controlling the rotation around each axis on the solver side.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage getAngularWeightMatrix()
   {
      return angular_weight_matrix_;
   }


   /**
            * Weight matrix used to define the priority of controlling the translation around each axis on the solver side.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage getLinearWeightMatrix()
   {
      return linear_weight_matrix_;
   }


   /**
            * This is the position of the control frame's origin expressed in endEffector.getBodyFixedFrame().
            * By default, the control frame is coincident to endEffector.getBodyFixedFrame().
            * The control frame is rigidly attached to the end-effector.
            */
   public us.ihmc.euclid.tuple3D.Point3D getControlFramePositionInEndEffector()
   {
      return control_frame_position_in_end_effector_;
   }


   /**
            * This is the orientation of the control frame expressed in endEffector.getBodyFixedFrame().
            * By default, the control frame is coincident to endEffector.getBodyFixedFrame().
            */
   public us.ihmc.euclid.tuple4D.Quaternion getControlFrameOrientationInEndEffector()
   {
      return control_frame_orientation_in_end_effector_;
   }


   /**
            * This is the allowable displacement of the position for each key frame.
            * By default, the solver will try to find a solution without modifying the position of the key frames.
            * When a positive value is provided, the solver may adjust a key frame to improve the overall solution quality.
            */
   public us.ihmc.idl.IDLSequence.Double  getAllowablePositionDisplacement()
   {
      return allowable_position_displacement_;
   }


   /**
            * This is the allowable displacement of the orientation for each key frame.
            * By default, the solver will try to find a solution without modifying the orientation of the key frames.
            * When a positive value is provided, the solver may adjust a key frame to improve the overall solution quality.
            */
   public us.ihmc.idl.IDLSequence.Double  getAllowableOrientationDisplacement()
   {
      return allowable_orientation_displacement_;
   }


   public static Supplier<KinematicsPlanningToolboxRigidBodyMessagePubSubType> getPubSubType()
   {
      return KinematicsPlanningToolboxRigidBodyMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return KinematicsPlanningToolboxRigidBodyMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(KinematicsPlanningToolboxRigidBodyMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.end_effector_hash_code_, other.end_effector_hash_code_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.key_frame_times_, other.key_frame_times_, epsilon)) return false;

      if (this.key_frame_poses_.size() != other.key_frame_poses_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.key_frame_poses_.size(); i++)
         {  if (!this.key_frame_poses_.get(i).epsilonEquals(other.key_frame_poses_.get(i), epsilon)) return false; }
      }

      if (!this.angular_selection_matrix_.epsilonEquals(other.angular_selection_matrix_, epsilon)) return false;
      if (!this.linear_selection_matrix_.epsilonEquals(other.linear_selection_matrix_, epsilon)) return false;
      if (!this.angular_weight_matrix_.epsilonEquals(other.angular_weight_matrix_, epsilon)) return false;
      if (!this.linear_weight_matrix_.epsilonEquals(other.linear_weight_matrix_, epsilon)) return false;
      if (!this.control_frame_position_in_end_effector_.epsilonEquals(other.control_frame_position_in_end_effector_, epsilon)) return false;
      if (!this.control_frame_orientation_in_end_effector_.epsilonEquals(other.control_frame_orientation_in_end_effector_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.allowable_position_displacement_, other.allowable_position_displacement_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.allowable_orientation_displacement_, other.allowable_orientation_displacement_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof KinematicsPlanningToolboxRigidBodyMessage)) return false;

      KinematicsPlanningToolboxRigidBodyMessage otherMyClass = (KinematicsPlanningToolboxRigidBodyMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.end_effector_hash_code_ != otherMyClass.end_effector_hash_code_) return false;

      if (!this.key_frame_times_.equals(otherMyClass.key_frame_times_)) return false;
      if (!this.key_frame_poses_.equals(otherMyClass.key_frame_poses_)) return false;
      if (!this.angular_selection_matrix_.equals(otherMyClass.angular_selection_matrix_)) return false;
      if (!this.linear_selection_matrix_.equals(otherMyClass.linear_selection_matrix_)) return false;
      if (!this.angular_weight_matrix_.equals(otherMyClass.angular_weight_matrix_)) return false;
      if (!this.linear_weight_matrix_.equals(otherMyClass.linear_weight_matrix_)) return false;
      if (!this.control_frame_position_in_end_effector_.equals(otherMyClass.control_frame_position_in_end_effector_)) return false;
      if (!this.control_frame_orientation_in_end_effector_.equals(otherMyClass.control_frame_orientation_in_end_effector_)) return false;
      if (!this.allowable_position_displacement_.equals(otherMyClass.allowable_position_displacement_)) return false;
      if (!this.allowable_orientation_displacement_.equals(otherMyClass.allowable_orientation_displacement_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("KinematicsPlanningToolboxRigidBodyMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("end_effector_hash_code=");
      builder.append(this.end_effector_hash_code_);      builder.append(", ");
      builder.append("key_frame_times=");
      builder.append(this.key_frame_times_);      builder.append(", ");
      builder.append("key_frame_poses=");
      builder.append(this.key_frame_poses_);      builder.append(", ");
      builder.append("angular_selection_matrix=");
      builder.append(this.angular_selection_matrix_);      builder.append(", ");
      builder.append("linear_selection_matrix=");
      builder.append(this.linear_selection_matrix_);      builder.append(", ");
      builder.append("angular_weight_matrix=");
      builder.append(this.angular_weight_matrix_);      builder.append(", ");
      builder.append("linear_weight_matrix=");
      builder.append(this.linear_weight_matrix_);      builder.append(", ");
      builder.append("control_frame_position_in_end_effector=");
      builder.append(this.control_frame_position_in_end_effector_);      builder.append(", ");
      builder.append("control_frame_orientation_in_end_effector=");
      builder.append(this.control_frame_orientation_in_end_effector_);      builder.append(", ");
      builder.append("allowable_position_displacement=");
      builder.append(this.allowable_position_displacement_);      builder.append(", ");
      builder.append("allowable_orientation_displacement=");
      builder.append(this.allowable_orientation_displacement_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosPoseStampedPublisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import org.ros.message.Time;

import geometry_msgs.PoseStamped;
import std_msgs.Header;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;


public class RosPoseStampedPublisher extends RosTopicPublisher<PoseStamped>
{
   private final PoseStamped initialValue;
   private int seq = 0;

   public RosPoseStampedPublisher(boolean latched)
   {
      this(latched, null);
   }

   public RosPoseStampedPublisher(boolean latched, PoseStamped initialValue)
   {
      super(PoseStamped._TYPE, latched);
      this.initialValue = initialValue;
   }
   
   @Override
   public void connected()
   {
      if(initialValue != null)
      {
         publish(initialValue);
      }
   }

   public void publish(String frameID, Vector3D pos, Quaternion rot, Time t)
   {
      PoseStamped message = getMessage();

      Header header = message.getHeader();
      header.setStamp(t);
      header.setFrameId(frameID);
      header.setSeq(seq++);
      message.setHeader(header);

      if (pos != null) {
          message.getPose().getPosition().setX(pos.getX());
          message.getPose().getPosition().setY(pos.getY());
          message.getPose().getPosition().setZ(pos.getZ());
      } else {
          message.getPose().getPosition().setX(0.0);
          message.getPose().getPosition().setY(0.0);
          message.getPose().getPosition().setZ(0.0);
      }

      if (rot != null) {
          message.getPose().getOrientation().setX(rot.getX());
          message.getPose().getOrientation().setY(rot.getY());
          message.getPose().getOrientation().setZ(rot.getZ());
          message.getPose().getOrientation().setW(rot.getS());
      } else {
          message.getPose().getOrientation().setX(0.0);
          message.getPose().getOrientation().setY(0.0);
          message.getPose().getOrientation().setZ(0.0);
          message.getPose().getOrientation().setW(1.0);
      }

      publish(message);
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tree/ParameterTreeItem.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tree;

import javafx.scene.control.TreeItem;

public class ParameterTreeItem extends TreeItem<ParameterTreeValue>
{
   private static final int minParametersToExpand = 4;

   public ParameterTreeItem(ParameterTreeValue parameterTreeValue)
   {
      super(parameterTreeValue);
      expandedProperty().addListener((observable, oldValue, newValue) -> expandChildrenForSmallRegistries());
   }

   public void expandChildrenForSmallRegistries()
   {
      getChildren().stream().filter(child -> !hasParameters(child, minParametersToExpand)).forEach(child -> child.setExpanded(true));
   }

   private static boolean hasParameters(TreeItem<ParameterTreeValue> item, int minSize)
   {
      return item.getChildren().filtered(child -> !child.getValue().isRegistry()).size() > minSize;
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/gui/YoVariableSimpleStatusDisplay.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.gui;



import java.awt.Color;

import javax.swing.JLabel;

import us.ihmc.yoVariables.variable.YoBoolean;
import us.ihmc.simulationconstructionset.NewDataListener;

@SuppressWarnings("serial")
public class YoVariableSimpleStatusDisplay extends JLabel implements NewDataListener
{
   private final String trueString = "ON";
   private final String falseString = "OFF";
   private final Color trueColor = Color.GREEN;
   private final Color falseColor = Color.RED;

   private YoBoolean currentStateVariable;

   public YoVariableSimpleStatusDisplay(YoBoolean currentStateVariable)
   {
      this.currentStateVariable = currentStateVariable;

      handleStateChange();
   }

   private void handleStateChange()
   {
      if (currentStateVariable.getBooleanValue())
      {
         setTrue();
      }
      else
      {
         setFalse();
      }
   }

   private void setTrue()
   {
      this.setForeground(trueColor);
      this.setText(trueString);
   }

   private void setFalse()
   {
      this.setForeground(falseColor);
      this.setText(falseString);
   }

   public NewDataListener getDataListener()
   {
      return this;
   }

   @Override
   public void newDataHasBeenSent()
   {
   }

   @Override
   public void newDataHasBeenReceived()
   {
      handleStateChange();
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/EnumTuner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import gnu.trove.map.TObjectIntMap;
import javafx.application.Platform;
import javafx.collections.ObservableList;
import javafx.geometry.Pos;
import javafx.scene.Node;
import javafx.scene.control.ChoiceBox;
import javafx.scene.control.TextField;
import javafx.scene.layout.HBox;
import javafx.scene.text.Text;
import us.ihmc.commons.PrintTools;
import us.ihmc.parameterTuner.ParameterTuningTools;
import us.ihmc.parameterTuner.guiElements.GuiParameter;

public class EnumTuner extends HBox implements InputNode
{
   public EnumTuner(GuiParameter parameter)
   {
      setupNode();

      TObjectIntMap<String> valueOptions = parameter.getValueOptions();
      if (valueOptions == null)
      {
         createTextField(parameter);
      }
      else
      {
         createChoiceBox(parameter, valueOptions);
      }
   }

   private void setupNode()
   {
      setSpacing(10.0);
      setAlignment(Pos.CENTER_LEFT);
      setMaxHeight(Double.NEGATIVE_INFINITY);
      setMaxWidth(Double.NEGATIVE_INFINITY);
      setPrefWidth(800.0);
      getChildren().add(new Text("Value"));
   }

   private TextField enumString;
   private void createTextField(GuiParameter parameter)
   {
      enumString = new TextField();

      enumString.setText(parameter.getCurrentValue());
      ParameterTuningTools.addThreadSafeListeners(enumString, () -> parameter.setValue(enumString.getText()));

      parameter.addChangedListener(p -> {
         // This listener will be triggered by an external change and is called from the animation timer.
         enumString.setText(parameter.getCurrentValue());
      });

      getChildren().add(enumString);
   }

   private ChoiceBox<String> choiceBox;
   private void createChoiceBox(GuiParameter parameter, TObjectIntMap<String> valueOptions)
   {
      choiceBox = new ChoiceBox<>();

      ObservableList<String> items = choiceBox.getItems();
      valueOptions.forEachKey(option -> {
         items.add(option);
         return true;
      });
      setChoiceBoxValue(parameter, choiceBox);

      choiceBox.getSelectionModel().selectedItemProperty().addListener((observable, oldValue, newValue) -> {
         Platform.runLater(() -> parameter.setValue(choiceBox.getSelectionModel().getSelectedItem()));
      });

      parameter.addChangedListener(p -> {
         // This listener will be triggered by an external change and is called from the animation timer.
         setChoiceBoxValue(parameter, choiceBox);
      });

      getChildren().add(choiceBox);
   }

   private void setChoiceBoxValue(GuiParameter parameter, ChoiceBox<String> choiceBox)
   {
      ObservableList<String> items = choiceBox.getItems();
      String newValue = parameter.getCurrentValue();
      if (!items.contains(newValue))
      {
         PrintTools.info("Enum parameter " + parameter.getName() + " did not provide the current value as option.");
         items.add(newValue);
      }
      choiceBox.getSelectionModel().select(newValue);
   }

   @Override
   public Node getSimpleInputNode(double width, double height)
   {
      if (choiceBox != null)
      {
         ChoiceBox<String> duplicate = new ChoiceBox<>(choiceBox.getItems());
         duplicate.setPrefHeight(height);
         duplicate.setPrefWidth(width);
         duplicate.setSelectionModel(choiceBox.getSelectionModel());
         return duplicate;
      }
      else
      {
         TextField duplicate = new TextField(enumString.getText());
         duplicate.setPrefHeight(height);
         duplicate.setPrefWidth(width);
         enumString.textProperty().addListener((observable, oldValue, newValue) -> duplicate.setText(newValue));
         duplicate.textProperty().addListener((observable, oldValue, newValue) -> enumString.setText(newValue));
         return duplicate;
      }
   }

   @Override
   public Node getFullInputNode()
   {
      return this;
   }

   @Override
   public void setValueFromPercent(double percent)
   {
   }

   @Override
   public double getValuePercent()
   {
      return 0.0;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/RosTfPublisher.java<|end_filename|>
package us.ihmc.avatar.ros;

import us.ihmc.commons.PrintTools;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.publisher.RosTf1Publisher;
import us.ihmc.utilities.ros.publisher.RosTf2Publisher;
import us.ihmc.utilities.ros.publisher.RosTfPublisherInterface;

public class RosTfPublisher implements RosTfPublisherInterface
{
   private final RosTfPublisherInterface tfPublisher;
   private final String tfPrefix;

   public RosTfPublisher(final RosMainNode rosMainNode, String tfPrefix)
   {
      if(tfPrefix == null)
      {
         this.tfPrefix = "";
      }
      else if (tfPrefix.equals("NONE"))
      {
         this.tfPrefix = "";
         PrintTools.info("tfPrefix option set to NONE - using no prefix");
      }
      else
      {
         if(tfPrefix.length() > 1 && !tfPrefix.endsWith("/"))
         {
            tfPrefix = tfPrefix + "/";
         }
         this.tfPrefix = tfPrefix;
         PrintTools.info("tfPrefix: " + this.tfPrefix);
      }
      
      if (rosMainNode.isUseTf2())
      {
         tfPublisher = new RosTf2Publisher(false);
         rosMainNode.attachPublisher("/tf", (RosTf2Publisher) tfPublisher);
      }
      else
      {
         tfPublisher = new RosTf1Publisher(false);
         rosMainNode.attachPublisher("/tf", (RosTf1Publisher) tfPublisher);
      }
   }

   @Override
   public void publish(RigidBodyTransform transform3d, long timeStamp,
         String parentFrame, String childFrame)
   {
      if(tfPrefix.length() > 0)
      {
         tfPublisher.publish(transform3d, timeStamp, tfPrefix + parentFrame, tfPrefix + childFrame);
      }
      else
      {
         tfPublisher.publish(transform3d, timeStamp, parentFrame, childFrame);
      }
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/JoystickCompatibilityFilter.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick;

import net.java.games.input.Component.Identifier;
import us.ihmc.tools.inputDevices.joystick.mapping.JoystickMapping;

public class JoystickCompatibilityFilter implements JoystickComponentFilter
{
   private final JoystickMapping mapping;
   
   private final boolean invert;
   private final double restToZeroCorrection;
   private final double scaleCorrection;
   
   public JoystickCompatibilityFilter(JoystickMapping channel, boolean invert, double restToZeroCorrection, double scaleCorrection)
   {
      this.mapping = channel;
      this.invert = invert;
      this.restToZeroCorrection = restToZeroCorrection;
      this.scaleCorrection = scaleCorrection;
   }

   @Override
   public double apply(double value)
   {
      double corrected = value + restToZeroCorrection;
      double scaled = corrected * scaleCorrection;
      return invert ? -scaled : scaled;
   }

   @Override
   public Identifier getIdentifier()
   {
      return mapping.getIdentifier();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HeadHybridJointspaceTaskspaceTrajectoryMessage.java<|end_filename|>
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 commands the controller to move the head in both taskspace and jointspace
       * to the desired orientation and joint angles while going through the specified trajectory points.
       */
public class HeadHybridJointspaceTaskspaceTrajectoryMessage extends Packet<HeadHybridJointspaceTaskspaceTrajectoryMessage> implements Settable<HeadHybridJointspaceTaskspaceTrajectoryMessage>, EpsilonComparable<HeadHybridJointspaceTaskspaceTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The taskspace trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage taskspace_trajectory_message_;
   /**
            * The jointspace trajectory information.
            * The indexing for the joints goes increasingly from the joint the closest to the chest to the joint the closest to the head.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage jointspace_trajectory_message_;

   public HeadHybridJointspaceTaskspaceTrajectoryMessage()
   {
      taskspace_trajectory_message_ = new controller_msgs.msg.dds.SO3TrajectoryMessage();
      jointspace_trajectory_message_ = new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
   }

   public HeadHybridJointspaceTaskspaceTrajectoryMessage(HeadHybridJointspaceTaskspaceTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(HeadHybridJointspaceTaskspaceTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.SO3TrajectoryMessagePubSubType.staticCopy(other.taskspace_trajectory_message_, taskspace_trajectory_message_);
      controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.staticCopy(other.jointspace_trajectory_message_, jointspace_trajectory_message_);
   }

   /**
            * 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_;
   }


   /**
            * The taskspace trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage getTaskspaceTrajectoryMessage()
   {
      return taskspace_trajectory_message_;
   }


   /**
            * The jointspace trajectory information.
            * The indexing for the joints goes increasingly from the joint the closest to the chest to the joint the closest to the head.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage getJointspaceTrajectoryMessage()
   {
      return jointspace_trajectory_message_;
   }


   public static Supplier<HeadHybridJointspaceTaskspaceTrajectoryMessagePubSubType> getPubSubType()
   {
      return HeadHybridJointspaceTaskspaceTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HeadHybridJointspaceTaskspaceTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HeadHybridJointspaceTaskspaceTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.taskspace_trajectory_message_.epsilonEquals(other.taskspace_trajectory_message_, epsilon)) return false;
      if (!this.jointspace_trajectory_message_.epsilonEquals(other.jointspace_trajectory_message_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HeadHybridJointspaceTaskspaceTrajectoryMessage)) return false;

      HeadHybridJointspaceTaskspaceTrajectoryMessage otherMyClass = (HeadHybridJointspaceTaskspaceTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.taskspace_trajectory_message_.equals(otherMyClass.taskspace_trajectory_message_)) return false;
      if (!this.jointspace_trajectory_message_.equals(otherMyClass.jointspace_trajectory_message_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HeadHybridJointspaceTaskspaceTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("taskspace_trajectory_message=");
      builder.append(this.taskspace_trajectory_message_);      builder.append(", ");
      builder.append("jointspace_trajectory_message=");
      builder.append(this.jointspace_trajectory_message_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/frames/CommonHumanoidReferenceFrames.java<|end_filename|>
package us.ihmc.sensorProcessing.frames;

import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public interface CommonHumanoidReferenceFrames extends CommonLeggedReferenceFrames<RobotSide>
{
   SideDependentList<MovingReferenceFrame> getAnkleZUpReferenceFrames();

   SideDependentList<MovingReferenceFrame> getFootReferenceFrames();

   SideDependentList<MovingReferenceFrame> getSoleFrames();

   SideDependentList<MovingReferenceFrame> getSoleZUpFrames();

   MovingReferenceFrame getPelvisFrame();

   MovingReferenceFrame getPelvisZUpFrame();

   MovingReferenceFrame getChestFrame();

   MovingReferenceFrame getMidFeetZUpFrame();

   MovingReferenceFrame getMidFeetUnderPelvisFrame();

   MovingReferenceFrame getABodyAttachedZUpFrame();

   MovingReferenceFrame getMidFootZUpGroundFrame();
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tree/ParameterTreeParameter.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tree;

import org.apache.commons.lang3.StringUtils;

import javafx.application.Platform;
import javafx.collections.ObservableList;
import javafx.geometry.Pos;
import javafx.scene.Node;
import javafx.scene.control.ContextMenu;
import javafx.scene.control.MenuItem;
import javafx.scene.control.SeparatorMenuItem;
import javafx.scene.control.Tooltip;
import javafx.scene.control.TreeItem;
import javafx.scene.input.Clipboard;
import javafx.scene.input.ClipboardContent;
import javafx.scene.layout.HBox;
import javafx.scene.text.Text;
import us.ihmc.parameterTuner.guiElements.GuiElement;
import us.ihmc.parameterTuner.guiElements.GuiParameter;
import us.ihmc.parameterTuner.guiElements.GuiParameterStatus;

public class ParameterTreeParameter implements ParameterTreeValue
{
   private final GuiParameter parameter;
   private final Node treeTuner;
   private final ObservableList<TreeItem<ParameterTreeValue>> selectedItems;

   private ParameterNode node;

   public ParameterTreeParameter(GuiParameter parameter, Node treeTuner, ObservableList<TreeItem<ParameterTreeValue>> selectedItems)
   {
      this.parameter = parameter;
      this.treeTuner = treeTuner;
      this.selectedItems = selectedItems;
   }

   @Override
   public boolean isRegistry()
   {
      return false;
   }

   @Override
   public String getName()
   {
      return parameter.getName();
   }

   @Override
   public GuiParameter getParameter()
   {
      return parameter;
   }

   @Override
   public Node getOrCreateNode()
   {
      if (node == null)
      {
         node = new ParameterNode(parameter, treeTuner);
      }
      return node;
   }

   private class ParameterNode extends HBox
   {
      private final Text name = new Text();

      private final ContextMenu contextMenu = new ContextMenu();
      private final MenuItem discard = new MenuItem("Discard");
      private final MenuItem markModified = new MenuItem("Mark as Modified");
      private final MenuItem copyName = new MenuItem("Copy Name");
      private final MenuItem copyNamespace = new MenuItem("Copy Namespace");
      private final Clipboard clipboard = Clipboard.getSystemClipboard();
      private final ClipboardContent content = new ClipboardContent();

      public ParameterNode(GuiParameter parameter, Node treeTuner)
      {
         // Creating this using fxml causes crazy slow down since it is done a lot.
         setSpacing(10.0);
         setAlignment(Pos.CENTER_LEFT);
         getChildren().add(treeTuner);
         getChildren().add(name);
         setOnContextMenuRequested((event) -> contextMenu.show(name, event.getScreenX(), event.getScreenY()));

         // Setup context menu for copying name to system clipboard.
         contextMenu.getItems().add(copyName);
         copyName.setOnAction((event) -> {
            content.putString(parameter.getName());
            clipboard.setContent(content);
         });
         contextMenu.getItems().add(copyNamespace);
         copyNamespace.setOnAction((event) -> {
            String[] splitNamespace = StringUtils.split(parameter.getUniqueName(), GuiElement.SEPERATOR);
            content.putString(splitNamespace[splitNamespace.length - 2]);
            clipboard.setContent(content);
         });
         contextMenu.getItems().add(new SeparatorMenuItem());

         // Setup context menu for discarding changes.
         contextMenu.getItems().add(discard);
         discard.setOnAction(event -> {
            selectedItems.filtered(item -> !item.getValue().isRegistry()).forEach(item -> item.getValue().getParameter().reset());
         });

         // Setup context menu for marking a parameter as modified.
         contextMenu.getItems().add(markModified);
         markModified.setOnAction(event -> {
            selectedItems.filtered(item -> !item.getValue().isRegistry()).forEach(item -> item.getValue().getParameter().markAsModified());
         });

         // Set up the css styles for the parameter status.
         updateStyle(parameter);

         parameter.addChangedListener(p -> Platform.runLater(() -> {
            updateStyle(parameter);
         }));

         name.setText(parameter.getName());

         Tooltip tooltip = new Tooltip(StringUtils.replaceAll(parameter.getUniqueName(), GuiElement.SEPERATOR, "\n"));
         Tooltip.install(this, tooltip);
      }

      private void updateStyle(GuiParameter parameter)
      {
         if (parameter.getStatus() == GuiParameterStatus.DEFAULT)
         {
            name.setId("default-parameter-name-in-tree-view");
         }
         else if (parameter.getStatus() == GuiParameterStatus.MODIFIED)
         {
            name.setId("modified-parameter-name-in-tree-view");
         }
         else
         {
            name.setId("parameter-name-in-tree-view");
         }
      }
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/NormalOcTreeNodeMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import org.apache.commons.math3.util.Precision;

import us.ihmc.communication.packets.Packet;

public class NormalOcTreeNodeMessage extends Packet<NormalOcTreeNodeMessage>
{
   public float size = Float.NaN;
   public int depth = 0;
   public int key0 = -1;
   public int key1 = -1;
   public int key2 = -1;
   public float normalX = Float.NaN;
   public float normalY = Float.NaN;
   public float normalZ = Float.NaN;
   public float normalAverageDeviation = Float.NaN;
   public int normalConsensusSize = 0;
   public float hitLocationX = Float.NaN;
   public float hitLocationY = Float.NaN;
   public float hitLocationZ = Float.NaN;
   public long numberOfHits = 0;

   public NormalOcTreeNodeMessage[] children = null;

   public NormalOcTreeNodeMessage()
   {
   }

   @Override
   public void set(NormalOcTreeNodeMessage other)
   {
      size = other.size;
      depth = other.depth;
      key0 = other.key0;
      key1 = other.key1;
      key2 = other.key2;
      normalX = other.normalX;
      normalY = other.normalY;
      normalZ = other.normalZ;
      normalAverageDeviation = other.normalAverageDeviation;
      normalConsensusSize = other.normalConsensusSize;
      hitLocationX = other.hitLocationX;
      hitLocationY = other.hitLocationY;
      hitLocationZ = other.hitLocationZ;
      numberOfHits = other.numberOfHits;

      if (other.children != null)
      {
         children = new NormalOcTreeNodeMessage[other.children.length];

         for (int i = 0; i < children.length; i++)
         {
            if (other.children[i] != null)
            {
               children[i] = new NormalOcTreeNodeMessage();
               children[i].set(other.children[i]);
            }
         }
      }
      setPacketInformation(other);
   }

   public int getNumberOfChildren()
   {
      if (children == null)
         return 0;

      int numberOfChildren = 0;

      for (NormalOcTreeNodeMessage child : children)
         numberOfChildren += child == null ? 0 : 1;

      return numberOfChildren;
   }

   @Override
   public boolean epsilonEquals(NormalOcTreeNodeMessage other, double epsilon)
   {
      if (!Precision.equals(size, other.size, epsilon))
         return false;
      if (depth != other.depth)
         return false;
      if (key0 != other.key0)
         return false;
      if (key1 != other.key1)
         return false;
      if (key2 != other.key2)
         return false;
      if (!Precision.equals(normalX, other.normalX, epsilon))
         return false;
      if (!Precision.equals(normalY, other.normalY, epsilon))
         return false;
      if (!Precision.equals(normalZ, other.normalZ, epsilon))
         return false;
      if (!Precision.equals(hitLocationX, other.hitLocationX, epsilon))
         return false;
      if (!Precision.equals(hitLocationY, other.hitLocationY, epsilon))
         return false;
      if (!Precision.equals(hitLocationZ, other.hitLocationZ, epsilon))
         return false;
      if (numberOfHits != other.numberOfHits)
         return false;

      if (getNumberOfChildren() != other.getNumberOfChildren())
         return false;

      for (int childIndex = 0; childIndex < 8; childIndex++)
      {
         if (!children[childIndex].epsilonEquals(other.children[childIndex], epsilon))
            return false;
      }

      return true;
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/mapping/MadCatzV1StickMapping.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick.mapping;

import java.util.ArrayList;
import java.util.List;

import org.apache.commons.lang3.SystemUtils;

import com.google.common.collect.BiMap;
import com.google.common.collect.HashBiMap;

import net.java.games.input.Component;
import net.java.games.input.Component.Identifier;
import us.ihmc.tools.inputDevices.joystick.JoystickCompatibilityFilter;
import net.java.games.input.Event;

public enum MadCatzV1StickMapping implements JoystickMapping
{
   TRIGGER,
   THROTTLE,
   HAT,
   STICK_ROLL,
   STICK_PITCH,
   STICK_YAW,
   BUTTON_2,
   BUTTON_3,
   BUTTON_4,
   BUTTON_5,
   BUTTON_6,
   PINKY_TRIGGER;
   
   public static final MadCatzV1StickMapping[] values = values();

   private static final BiMap<Identifier, MadCatzV1StickMapping> windowsBiMap = HashBiMap.create(values.length);
   private static final BiMap<Identifier, MadCatzV1StickMapping> macBiMap = HashBiMap.create(values.length);
   private static final BiMap<Identifier, MadCatzV1StickMapping> linuxBiMap = HashBiMap.create(values.length);

   static
   {
      mapValues(TRIGGER, Identifier.Button._0, Identifier.Button._0, Identifier.Button.TRIGGER);
      mapValues(THROTTLE, Identifier.Axis.Z, Identifier.Axis.Z, Identifier.Axis.Z);
      mapValues(HAT, Identifier.Axis.POV, Identifier.Axis.POV, Identifier.Axis.POV);
      mapValues(STICK_ROLL, Identifier.Axis.X, Identifier.Axis.X, Identifier.Axis.X);
      mapValues(STICK_PITCH, Identifier.Axis.Y, Identifier.Axis.Y, Identifier.Axis.Y);
      mapValues(STICK_YAW, Identifier.Axis.RZ, Identifier.Axis.RZ, Identifier.Axis.RZ);
      mapValues(BUTTON_2, Identifier.Button._1, Identifier.Button._1, Identifier.Button.THUMB);
      mapValues(BUTTON_3, Identifier.Button._2, Identifier.Button._2, Identifier.Button.THUMB2);
      mapValues(BUTTON_4, Identifier.Button._3, Identifier.Button._3, Identifier.Button.TOP);
      mapValues(BUTTON_5, Identifier.Button._4, Identifier.Button._4, Identifier.Button.TOP2);
      mapValues(BUTTON_6, Identifier.Button._5, Identifier.Button._5, Identifier.Button.PINKIE);
      mapValues(PINKY_TRIGGER, Identifier.Button._6, Identifier.Button._6, Identifier.Button.BASE);
   }

   private static void mapValues(MadCatzV1StickMapping mapping, Identifier windowsIdentifier, Identifier macIdentifier, Identifier linuxIdentifier)
   {
      windowsBiMap.put(windowsIdentifier, mapping);
      macBiMap.put(macIdentifier, mapping);
      linuxBiMap.put(linuxIdentifier, mapping);
   }

   public static List<JoystickCompatibilityFilter> getCompatibilityFilters()
   {
      ArrayList<JoystickCompatibilityFilter> compatibilityFilters = new ArrayList<>();
      
      return compatibilityFilters;
   }

   @Override
   public Identifier getIdentifier()
   {
      if (SystemUtils.IS_OS_WINDOWS)
      {
         return windowsBiMap.inverse().get(this);
      }
      else if (SystemUtils.IS_OS_MAC)
      {
         return macBiMap.inverse().get(this);
      }
      else if (SystemUtils.IS_OS_LINUX)
      {
         return linuxBiMap.inverse().get(this);
      }
      else
      {
         return null;
      }
   }

   public static MadCatzV1StickMapping getMapping(Identifier identifier)
   {
      if (SystemUtils.IS_OS_WINDOWS)
      {
         return windowsBiMap.get(identifier);
      }
      else if (SystemUtils.IS_OS_MAC)
      {
         return macBiMap.get(identifier);
      }
      else if (SystemUtils.IS_OS_LINUX)
      {
         return linuxBiMap.get(identifier);
      }
      else
      {
         return null;
      }
   }
   
   private static MadCatzV1StickMapping getMapping(Component component)
   {
      return getMapping(component.getIdentifier());
   }

   public static MadCatzV1StickMapping getMapping(Event event)
   {
      return getMapping(event.getComponent());
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/tcpServer/ReconnectingTCPServer.java<|end_filename|>
package us.ihmc.communication.net.tcpServer;

import java.io.DataInputStream;
import java.io.IOException;
import java.net.ServerSocket;
import java.net.Socket;

import us.ihmc.commons.thread.ThreadTools;

public class ReconnectingTCPServer extends ReconnectingTCPConnection
{
   private final ServerSocket singleConnectionServer;
   private Socket socket;

   public ReconnectingTCPServer(int port) throws IOException
   {
      super(2097152);
      singleConnectionServer = new ServerSocket(port);

   }

   public void disconnect()
   {
      synchronized (connectionStatusSync)
      {
         if (getStatus() == Status.CONNECTED)
         {
            try
            {
               inputStream.close();
               outputStream.close();
               socket.close();
            }
            catch (IOException e)
            {
               e.printStackTrace();
            }

            setStatus(Status.DISCONNECTED);
            notifyDisconnectedListeners();
         }
      }
   }

   protected void connect()
   {

      if (getStatus() == Status.CLOSED)
      {
         throw new RuntimeException("Connection is closed");
      }
      while (getStatus() == Status.DISCONNECTED)
      {
         try
         {
            socket = singleConnectionServer.accept();
            socket.setTcpNoDelay(true);
            inputStream = new DataInputStream(socket.getInputStream());
            outputStream = socket.getOutputStream();
            reset();
            setStatus(Status.CONNECTED);
            notifyConnectedListeners();
         }
         catch (IOException e)
         {
            System.out.println("Connecting timed out: " + e.getMessage());
            ThreadTools.sleep(1000);
         }
      }
   }

   @Override
   public void close()
   {
      synchronized (connectionStatusSync)
      {
         disconnect();
         setStatus(Status.CLOSED);
      }

   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/producers/CompressedVideoHandler.java<|end_filename|>
package us.ihmc.communication.producers;

import us.ihmc.communication.interfaces.Connectable;
import us.ihmc.communication.net.ConnectionStateListener;
import us.ihmc.communication.video.CompressedVideoCallback;

public interface CompressedVideoHandler extends Connectable, CompressedVideoCallback
{
   public void addNetStateListener(ConnectionStateListener compressedVideoDataServer);
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/DetectedFacesPacket.java<|end_filename|>
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;

public class DetectedFacesPacket extends Packet<DetectedFacesPacket> implements Settable<DetectedFacesPacket>, EpsilonComparable<DetectedFacesPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.StringBuilderHolder  ids_;
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  positions_;

   public DetectedFacesPacket()
   {
      ids_ = new us.ihmc.idl.IDLSequence.StringBuilderHolder (100, "type_d");
      positions_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D> (100, new geometry_msgs.msg.dds.PointPubSubType());

   }

   public DetectedFacesPacket(DetectedFacesPacket other)
   {
      this();
      set(other);
   }

   public void set(DetectedFacesPacket other)
   {
      sequence_id_ = other.sequence_id_;

      ids_.set(other.ids_);
      positions_.set(other.positions_);
   }

   /**
            * 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 us.ihmc.idl.IDLSequence.StringBuilderHolder  getIds()
   {
      return ids_;
   }


   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  getPositions()
   {
      return positions_;
   }


   public static Supplier<DetectedFacesPacketPubSubType> getPubSubType()
   {
      return DetectedFacesPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return DetectedFacesPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(DetectedFacesPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilderSequence(this.ids_, other.ids_, epsilon)) return false;

      if (this.positions_.size() != other.positions_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.positions_.size(); i++)
         {  if (!this.positions_.get(i).epsilonEquals(other.positions_.get(i), epsilon)) return false; }
      }


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof DetectedFacesPacket)) return false;

      DetectedFacesPacket otherMyClass = (DetectedFacesPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.ids_.equals(otherMyClass.ids_)) return false;
      if (!this.positions_.equals(otherMyClass.positions_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("DetectedFacesPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("ids=");
      builder.append(this.ids_);      builder.append(", ");
      builder.append("positions=");
      builder.append(this.positions_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/beetle/parameters/RhinoBeetleJointNameMapAndContactDefinition.java<|end_filename|>
package us.ihmc.exampleSimulations.beetle.parameters;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import org.apache.commons.lang3.tuple.ImmutablePair;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotics.partNames.ArmJointName;
import us.ihmc.robotics.partNames.ContactPointDefinitionHolder;
import us.ihmc.robotics.partNames.JointNameMap;
import us.ihmc.robotics.partNames.JointRole;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.partNames.NeckJointName;
import us.ihmc.robotics.partNames.SpineJointName;
import us.ihmc.robotics.robotSide.RobotSextant;
import us.ihmc.robotics.robotSide.SegmentDependentList;

public class RhinoBeetleJointNameMapAndContactDefinition implements JointNameMap, ContactPointDefinitionHolder
{
   private final String modelName = RhinoBeetleSDFParameters.SDF_MODEL_NAME;
   private final String rootJoint = "BODY";
   
   private final List<ImmutablePair<String, Vector3D>> jointNameGroundContactPointMap = new ArrayList<ImmutablePair<String, Vector3D>>();
   private final SegmentDependentList<RobotSextant, HashMap<LegJointName, RhinoBeetleJointName>> jointIdsBySextant = new SegmentDependentList<>(RobotSextant.class);
   private final HashSet<String> lastSimulatedJoints = new HashSet<>();
   private final String[] jointNamesBeforeFeet = new String[6];

   public RhinoBeetleJointNameMapAndContactDefinition()
   {
      for(RhinoBeetleJointName jointId : RhinoBeetleJointName.values)
      {
         RobotSextant sextant = jointId.getSextant();
         LegJointName legJointName = jointId.getLegJointName();
         
         //map legs
         if(sextant != null)
         {
            //add to maps for each leg
            HashMap<LegJointName, RhinoBeetleJointName> legJointIds = jointIdsBySextant.get(sextant);
            if(legJointIds == null)
            {
               legJointIds = new HashMap<>();
               jointIdsBySextant.set(sextant, legJointIds);
            }
            legJointIds.put(legJointName, jointId);
            
            if(legJointName == LegJointName.KNEE_PITCH)
            {
               //lastSimulated Joints
               lastSimulatedJoints.add(jointId.toString());
               
               //contact points
               Vector3D contactPoint = RhinoBeetlePhysicalProperties.getOffsetFromJointBeforeFootToSoleAlignedWithWorld(sextant);
               jointNameGroundContactPointMap.add(new ImmutablePair<String, Vector3D>(jointId.toString(), contactPoint));
            }
         }
         else
         {
            //gaster_pitch
         }
      }
      lastSimulatedJoints.toArray(jointNamesBeforeFeet);
   }
   
   @Override
   public String getModelName()
   {
      return modelName ;
   }

   @Override
   public JointRole getJointRole(String jointName)
   {
      return JointRole.LEG;
   }

   @Override
   public String getUnsanitizedRootJointInSdf()
   {
      return rootJoint;
   }

   @Override
   public List<ImmutablePair<String, Vector3D>> getJointNameGroundContactPointMap()
   {
      return jointNameGroundContactPointMap;
   }

   @Override
   public boolean isTorqueVelocityLimitsEnabled()
   {
      return false;
   }
   
   public String getJointNameBeforeFoot(RobotSextant robotSextant)
   {
      return jointIdsBySextant.get(robotSextant).get(LegJointName.KNEE_PITCH).getName();
   }
   
   @Override
   public Set<String> getLastSimulatedJoints()
   {
      return lastSimulatedJoints;
   }

   @Override
   public String[] getJointNamesBeforeFeet()
   {
      return jointNamesBeforeFeet;
   }

   @Override
   public Enum<?>[] getRobotSegments()
   {
      return RobotSextant.values;
   }

   @Override
   public LegJointName[] getLegJointNames()
   {
      return null;
   }

   @Override
   public ArmJointName[] getArmJointNames()
   {
      return null;
   }

   @Override
   public SpineJointName[] getSpineJointNames()
   {
      return null;
   }

   @Override
   public NeckJointName[] getNeckJointNames()
   {
      return null;
   }

   @Override
   @Deprecated
   public NeckJointName getNeckJointName(String jointName)
   {
      return null;
   }

   @Override
   @Deprecated
   public SpineJointName getSpineJointName(String jointName)
   {
      return null;
   }

   @Override
   public String getRootBodyName()
   {
      return rootJoint;
   }

   @Override
   @Deprecated
   public String getHeadName()
   {
      return null;
   }

   @Override
   public Enum<?> getEndEffectorsRobotSegment(String joineNameBeforeEndEffector)
   {
      return RhinoBeetleJointName.valueOf(joineNameBeforeEndEffector).getSextant();
   }

   public String getLegJointName(RobotSextant robotSextant, LegJointName legJointName)
   {
      return jointIdsBySextant.get(robotSextant).get(legJointName).toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PelvisPoseErrorPacket.java<|end_filename|>
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;

/**
       * Message part of the localization module
       */
public class PelvisPoseErrorPacket extends Packet<PelvisPoseErrorPacket> implements Settable<PelvisPoseErrorPacket>, EpsilonComparable<PelvisPoseErrorPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public float residual_error_;
   public float total_error_;
   public boolean has_map_been_reset_;

   public PelvisPoseErrorPacket()
   {
   }

   public PelvisPoseErrorPacket(PelvisPoseErrorPacket other)
   {
      this();
      set(other);
   }

   public void set(PelvisPoseErrorPacket other)
   {
      sequence_id_ = other.sequence_id_;

      residual_error_ = other.residual_error_;

      total_error_ = other.total_error_;

      has_map_been_reset_ = other.has_map_been_reset_;

   }

   /**
            * 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 void setResidualError(float residual_error)
   {
      residual_error_ = residual_error;
   }
   public float getResidualError()
   {
      return residual_error_;
   }

   public void setTotalError(float total_error)
   {
      total_error_ = total_error;
   }
   public float getTotalError()
   {
      return total_error_;
   }

   public void setHasMapBeenReset(boolean has_map_been_reset)
   {
      has_map_been_reset_ = has_map_been_reset;
   }
   public boolean getHasMapBeenReset()
   {
      return has_map_been_reset_;
   }


   public static Supplier<PelvisPoseErrorPacketPubSubType> getPubSubType()
   {
      return PelvisPoseErrorPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PelvisPoseErrorPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PelvisPoseErrorPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.residual_error_, other.residual_error_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.total_error_, other.total_error_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.has_map_been_reset_, other.has_map_been_reset_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PelvisPoseErrorPacket)) return false;

      PelvisPoseErrorPacket otherMyClass = (PelvisPoseErrorPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.residual_error_ != otherMyClass.residual_error_) return false;

      if(this.total_error_ != otherMyClass.total_error_) return false;

      if(this.has_map_been_reset_ != otherMyClass.has_map_been_reset_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PelvisPoseErrorPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("residual_error=");
      builder.append(this.residual_error_);      builder.append(", ");
      builder.append("total_error=");
      builder.append(this.total_error_);      builder.append(", ");
      builder.append("has_map_been_reset=");
      builder.append(this.has_map_been_reset_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/properties/PlanarRegionSegmentationParametersProperty.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.properties;

import javafx.beans.property.Property;
import us.ihmc.robotEnvironmentAwareness.planarRegion.PlanarRegionSegmentationParameters;

public class PlanarRegionSegmentationParametersProperty extends ParametersProperty<PlanarRegionSegmentationParameters>
{
   private final DoubleField searchRadius = new DoubleField(PlanarRegionSegmentationParameters::getSearchRadius, (p, v) -> p.setSearchRadius(v));
   private final DoubleField maxDistanceFromPlane = new DoubleField(PlanarRegionSegmentationParameters::getMaxDistanceFromPlane, (p, v) -> p.setMaxDistanceFromPlane(v));
   private final DoubleField maxAngleFromPlane = new DoubleField(PlanarRegionSegmentationParameters::getMaxAngleFromPlane, (p, v) -> p.setMaxAngleFromPlane(v));
   private final DoubleField minNormalQuality = new DoubleField(PlanarRegionSegmentationParameters::getMinNormalQuality, (p, v) -> p.setMinNormalQuality(v));
   private final IntegerField minRegionSize = new IntegerField(PlanarRegionSegmentationParameters::getMinRegionSize, (p, v) -> p.setMinRegionSize(v));
   private final DoubleField maxStandardDeviation = new DoubleField(PlanarRegionSegmentationParameters::getMaxStandardDeviation, (p, v) -> p.setMaxStandardDeviation(v));
   private final DoubleField minVolumicDensity = new DoubleField(PlanarRegionSegmentationParameters::getMinVolumicDensity, (p, v) -> p.setMinVolumicDensity(v));

   public PlanarRegionSegmentationParametersProperty(Object bean, String name)
   {
      super(bean, name, new PlanarRegionSegmentationParameters());
   }

   public void bindBidirectionalSearchRadius(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, searchRadius);
   }

   public void bindBidirectionalMaxDistanceFromPlane(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxDistanceFromPlane);
   }

   public void bindBidirectionalMaxAngleFromPlane(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxAngleFromPlane);
   }

   public void bindBidirectionalMinNormalQuality(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minNormalQuality);
   }

   public void bindBidirectionalMinRegionSize(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minRegionSize);
   }

   public void bindBidirectionalMaxStandardDeviation(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxStandardDeviation);
   }

   public void bindBidirectionalMinVolumicDensity(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minVolumicDensity);
   }

   @Override
   protected PlanarRegionSegmentationParameters getValueCopy(PlanarRegionSegmentationParameters valueToCopy)
   {
      return new PlanarRegionSegmentationParameters(valueToCopy);
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/updaters/RegionFeaturesProvider.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.updaters;

import java.util.List;

import us.ihmc.euclid.geometry.LineSegment3D;
import us.ihmc.robotEnvironmentAwareness.planarRegion.PlanarRegionSegmentationNodeData;
import us.ihmc.robotics.geometry.PlanarRegionsList;

public interface RegionFeaturesProvider
{
   List<PlanarRegionSegmentationNodeData> getSegmentationNodeData();
   
   PlanarRegionsList getPlanarRegionsList();

   int getNumberOfPlaneIntersections();

   LineSegment3D getIntersection(int index);

}

<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/LineSegment3DMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.tuple3D.Point3D32;

public class LineSegment3DMessage extends Packet<LineSegment3DMessage>
{
   public Point3D32 start, end;

   public LineSegment3DMessage()
   {
   }

   @Override
   public void set(LineSegment3DMessage other)
   {
      start = new Point3D32(other.start);
      end = new Point3D32(other.end);
      setPacketInformation(other);
   }

   public Point3D32 getStart()
   {
      return start;
   }

   public Point3D32 getEnd()
   {
      return end;
   }

   public void setStart(Point3D32 start)
   {
      this.start = start;
   }

   public void setEnd(Point3D32 end)
   {
      this.end = end;
   }

   @Override
   public boolean epsilonEquals(LineSegment3DMessage other, double epsilon)
   {
      return start.epsilonEquals(other.start, (float) epsilon) && end.epsilonEquals(other.end, (float) epsilon);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/Capsule.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.transform.interfaces.Transform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;

public class Capsule
{
   final public Point3D p1 = new Point3D();
   final public Point3D p2 = new Point3D();
   public double radius;

   public Capsule(Point3DReadOnly p1_, Point3DReadOnly p2_, double rad)
   {
      p1.set(p1_);
      p2.set(p2_);
      radius = rad;
   }

   public Capsule(Capsule other)
   {
      p1.set(other.p1);
      p2.set(other.p2);
      radius = other.radius;
   }

   public void set(Capsule other)
   {
      p1.set(other.p1);
      p2.set(other.p2);
      radius = other.radius;
   }

   public void transform(Transform trans)
   {
      trans.transform(p1);
      trans.transform(p2);
   }

   static private double dot(Vector3DReadOnly u, Vector3DReadOnly v)
   {
      return (u.getX() * v.getX() + u.getY() * v.getY() + u.getZ() * v.getZ());
   }

   static private Vector3D diff(Point3DReadOnly u, Point3DReadOnly v)
   {
      return new Vector3D(u.getX() - v.getX(), u.getY() - v.getY(), u.getZ() - v.getZ());
   }

   static private void sum(Point3DReadOnly u, Vector3DReadOnly v, Point3DBasics res)
   {
      res.set(u.getX() + v.getX(), u.getY() + v.getY(), u.getZ() + v.getZ());
   }

   static private double dist(Point3DReadOnly u, Point3DReadOnly v)
   {
      double dx = u.getX() - v.getX();
      double dy = u.getY() - v.getY();
      double dz = u.getZ() - v.getZ();
      return Math.sqrt(dx * dx + dy * dy + dz * dz);
   }

   // dist3D_Segment_to_Segment(): get the 3D minimum distance between 2 segments
   //    Input:  two 3D line segments S1 and S2
   //    Return: the shortest distance between S1 and S2
   static public double distanceQuery(Capsule C1, Capsule C2, Point3DBasics dp1, Point3DBasics dp2)
   {
      double SMALL_NUM = 0.00000001;

      Vector3D u = diff(C1.p2, C1.p1);
      Vector3D v = diff(C2.p2, C2.p1);
      Vector3D w = diff(C1.p1, C2.p1);
      double a = dot(u, u); // always >= 0
      double b = dot(u, v);
      double c = dot(v, v); // always >= 0
      double d = dot(u, w);
      double e = dot(v, w);
      double D = a * c - b * b; // always >= 0
      double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
      double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0

      // compute the line parameters of the two closest points
      if (D < SMALL_NUM)
      { // the lines are almost parallel
         sN = 0.0; // force using point P0 on segment S1
         sD = 1.0; // to prevent possible division by 0.0 later
         tN = e;
         tD = c;
      }
      else
      { // get the closest points on the infinite lines
         sN = (b * e - c * d);
         tN = (a * e - b * d);
         if (sN < 0.0)
         { // sc < 0 => the s=0 edge is visible
            sN = 0.0;
            tN = e;
            tD = c;
         }
         else if (sN > sD)
         { // sc > 1  => the s=1 edge is visible
            sN = sD;
            tN = e + b;
            tD = c;
         }
      }

      if (tN < 0.0)
      { // tc < 0 => the t=0 edge is visible
         tN = 0.0;
         // recompute sc for this edge
         if (-d < 0.0)
            sN = 0.0;
         else if (-d > a)
            sN = sD;
         else
         {
            sN = -d;
            sD = a;
         }
      }
      else if (tN > tD)
      { // tc > 1  => the t=1 edge is visible
         tN = tD;
         // recompute sc for this edge
         if ((-d + b) < 0.0)
            sN = 0;
         else if ((-d + b) > a)
            sN = sD;
         else
         {
            sN = (-d + b);
            sD = a;
         }
      }
      // finally do the division to get sc and tc
      sc = (Math.abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
      tc = (Math.abs(tN) < SMALL_NUM ? 0.0 : tN / tD);

      // get the difference of the two closest points 
      u.scale(sc);
      sum(C1.p1, u, dp1);

      v.scale(tc);
      sum(C2.p1, v, dp2);

      Vector3D dP = diff(dp2, dp1);
      dP.normalize();
      double distance_radius_zero = dist(dp1, dp2);

      if (Math.abs(C1.radius) > SMALL_NUM)
      {
         dP.scale(C1.radius);
         dp1.add(dP);

         dP.scale(-C2.radius / C1.radius);
         dp2.add(dP);
      }
      else
      {
         dP.scale(-C2.radius);
         dp2.add(dP);
      }
      double distA = distance_radius_zero - C1.radius - C2.radius;
      double distB = diff(dp1, dp2).length();
      double distC = dist(dp1, dp2);

      return (distance_radius_zero - C1.radius - C2.radius); // return the closest distance
   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/ObjectCommunicator.java<|end_filename|>
package us.ihmc.communication.net;

import java.io.IOException;

import us.ihmc.communication.interfaces.Connectable;

public interface ObjectCommunicator extends GlobalObjectConsumer, Connectable
{
   public abstract void attachStateListener(ConnectionStateListener stateListener);

   public abstract <T> void attachListener(Class<T> clazz, ObjectConsumer<T> listener);

   public abstract <T> void detachListener(Class<T> clazz, ObjectConsumer<T> listener);

   public abstract void attachGlobalListener(GlobalObjectConsumer listener);

   public abstract void detachGlobalListener(GlobalObjectConsumer listener);
   
   public void disconnect() throws IOException;
   
   public void connect() throws IOException;
}

<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/visibilityGraphs/interfaces/VisibilityMapHolder.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.interfaces;

import us.ihmc.pathPlanning.visibilityGraphs.dataStructure.Connection;
import us.ihmc.pathPlanning.visibilityGraphs.dataStructure.VisibilityMap;

public interface VisibilityMapHolder
{
   public int getMapId();

   public VisibilityMap getVisibilityMapInLocal();

   public VisibilityMap getVisibilityMapInWorld();
   
   default double getConnectionWeight(Connection connection)
   {
      return connection.length();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/HandDesiredConfigurationMessageSubscriber.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import java.util.concurrent.ConcurrentLinkedQueue;

import controller_msgs.msg.dds.HandDesiredConfigurationMessage;
import us.ihmc.pubsub.subscriber.Subscriber;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.ros2.NewMessageListener;

public class HandDesiredConfigurationMessageSubscriber implements NewMessageListener<HandDesiredConfigurationMessage>
{
   private final ConcurrentLinkedQueue<HandDesiredConfigurationMessage> messageQueue = new ConcurrentLinkedQueue<HandDesiredConfigurationMessage>();
   private RobotSide robotSide;

   public HandDesiredConfigurationMessageSubscriber(RobotSide robotSide)
   {
      this.robotSide = robotSide;
   }

   @Override
   public void onNewDataMessage(Subscriber<HandDesiredConfigurationMessage> subscriber)
   {
      receivedPacket(subscriber.takeNextData());
   }

   public void receivedPacket(HandDesiredConfigurationMessage ihmcMessage)
   {
      if (this.robotSide == null)
         messageQueue.add(ihmcMessage);
      else if (ihmcMessage.getRobotSide() == this.robotSide.toByte())
         messageQueue.add(ihmcMessage);
   }

   public HandDesiredConfigurationMessage pollMessage()
   {
      return messageQueue.poll();
   }

   public boolean isNewDesiredConfigurationAvailable()
   {
      return !messageQueue.isEmpty();
   }

   public RobotSide getSide()
   {
      return robotSide;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/trajectories/ListOfPointsTrajectory.java<|end_filename|>
package us.ihmc.robotics.trajectories;

import java.util.ArrayList;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple2D.Point2D;

public class ListOfPointsTrajectory
{
   private ArrayList<FramePoint3D> listOfPoints;
   private double lengthOfPath;
   private LinearInterpolater linearInterpolater;
   private double[] alpha;

   public ListOfPointsTrajectory(final ArrayList<FramePoint3D> listOfPointsForSegment)
   {
      if ((listOfPointsForSegment == null) || (listOfPointsForSegment.size() < 2))
      {
         throw new RuntimeException("listOfPoints must have at least 2 elements.");
      }

      this.listOfPoints = new ArrayList<FramePoint3D>();

      // add the start and end point
      this.listOfPoints.add(new FramePoint3D(listOfPointsForSegment.get(0)));

      for (int i = 1; i < listOfPointsForSegment.size(); i++)
      {
         double distanceToPreviousPoint = listOfPointsForSegment.get(i).distance(listOfPointsForSegment.get(i - 1));
         if (distanceToPreviousPoint > 0.0)
            this.listOfPoints.add(new FramePoint3D(listOfPointsForSegment.get(i)));
      }

      // set all the Z's to zero and add to list
//    for()
//    {
//       framePoint.setZ(0.0);
//       this.listOfPoints.add(new FramePoint(framePoint));
//    }

      setupAlphaArrayAndLinearInterpolator();
   }

   public static ListOfPointsTrajectory createListOfPointsTrajectory(ArrayList<Point2D> points, ReferenceFrame referenceFrame)
   {
      ArrayList<FramePoint3D> framePoints = new ArrayList<FramePoint3D>(points.size());

      for (Point2D point : points)
      {
         FramePoint3D framePoint = new FramePoint3D(referenceFrame, point.getX(), point.getY(), 0.0);
         framePoints.add(framePoint);
      }

      return new ListOfPointsTrajectory(framePoints);
   }

   private void setupAlphaArrayAndLinearInterpolator()
   {
      int numberOfPoints = listOfPoints.size();

      alpha = new double[numberOfPoints];
      double[] index = new double[numberOfPoints];
      double[] distanceOnPath = calculateLengthOfPath(listOfPoints);

      this.lengthOfPath = distanceOnPath[distanceOnPath.length - 1];

      if ((numberOfPoints > 1) && (this.lengthOfPath == 0.0))
         throw new RuntimeException("If more than one point, the length of the path must be greater than zero");

      alpha[0] = 0.0;
      index[0] = 0;

      for (int i = 1; i < numberOfPoints; i++)
      {
         alpha[i] = distanceOnPath[i] / lengthOfPath;
         index[i] = i;
      }

      try
      {
         this.linearInterpolater = new LinearInterpolater(alpha, index);
      }
      catch (Exception ex)
      {
         this.linearInterpolater = null;

//       System.out.println(ex);
//       throw new RuntimeException("ListOfPointsTrajectory: error in generating alpha or index");
      }
   }

   public ListOfPointsTrajectory(ListOfPointsTrajectory listOfPointsTrajectory)
   {
      this(listOfPointsTrajectory.getOriginalList());
   }

   public double getPathLength()
   {
      return lengthOfPath;
   }


   public double getAlphaGivenIndexInOriginalList(int index)
   {
      if (index > (alpha.length - 1))
         index = alpha.length - 1;

      return alpha[index];
   }

   public int getSizeOfInternalList()
   {
      return listOfPoints.size();
   }

   public ArrayList<FramePoint3D> getOriginalList()
   {
      return new ArrayList<FramePoint3D>(listOfPoints);
   }


   public ArrayList<FramePointAndAlpha> getOriginalListAndAlphas()
   {
      ArrayList<FramePointAndAlpha> ret = new ArrayList<FramePointAndAlpha>();

      for (int i = 0; i < listOfPoints.size(); i++)
      {
         ret.add(new FramePointAndAlpha(listOfPoints.get(i), alpha[i]));
      }

      return ret;
   }

   public class FramePointAndAlpha
   {
      private final FramePoint3D framePoint;
      private final double alpha;

      FramePointAndAlpha(FramePoint3D framePoint, double alpha)
      {
         this.framePoint = framePoint;
         this.alpha = alpha;
      }

      public FramePoint3D getFramePoint()
      {
         return framePoint;
      }

      public double getAlpha()
      {
         return alpha;
      }
   }


   private double[] calculateLengthOfPath(ArrayList<FramePoint3D> pointsOnPath)
   {
      double[] distanceOnPath = new double[pointsOnPath.size()];
      distanceOnPath[0] = 0.0;

      for (int i = 1; i < pointsOnPath.size(); i++)
      {
         distanceOnPath[i] = distanceOnPath[i - 1] + pointsOnPath.get(i).distance(pointsOnPath.get(i - 1));
      }

      return distanceOnPath;
   }


   public double getAlpha(FramePoint3D pointToCheck)
   {
      double minimumDistance = Double.POSITIVE_INFINITY;
      double alphaToReturn = 1.0;

      double deltaAlpha = 0.01;

      // +++JEP 3/4/2007: Needed to add deltaAlpha/2.0 or it might never get to 1.0
      for (double alpha = 0.0; alpha <= 1.0 + deltaAlpha / 2.0; alpha = alpha + deltaAlpha)
      {
         if (alpha > 1.0)
            alpha = 1.0;

         FramePoint3D testPoint = this.getPointOnPath(alpha);
         double distance = testPoint.distance(pointToCheck);
         if (distance < minimumDistance)
         {
            minimumDistance = distance;
            alphaToReturn = alpha;
         }
      }

      return alphaToReturn;
   }

   public int getIndexOfClosestPoint(FramePoint3D pointToCheck)
   {
      double alpha = getAlpha(pointToCheck);

      return (int) Math.round(getIndexOfClosestAlpha(alpha));
   }


   public double getIndexOfClosestAlpha(double alpha)
   {
      double index;
      if (linearInterpolater != null)
         index = linearInterpolater.getPoint(alpha);
      else
         index = 0.0;

      if (index > listOfPoints.size() - 1)
         index = listOfPoints.size() - 1;
      if (index < 0.0)
         index = 0.0;

      return index;

   }

   public FramePoint3D getPointOnPath(double alpha)
   {
      return getPointOnPath(alpha, false);
   }

   public FramePoint3D getPointOnPathAndAddToInternalList(double alpha)
   {
      return getPointOnPath(alpha, true);
   }


   private FramePoint3D getPointOnPath(double alpha, boolean addToInternalRepresentation)
   {
      if (alpha < 0.0)
         alpha = 0.0;
      if (alpha > 1.0)
         alpha = 1.0;

      double indexDouble;
      if (linearInterpolater != null)
         indexDouble = this.linearInterpolater.getPoint(alpha);
      else
         indexDouble = 0.0;

      int indexBefore = (int) Math.floor(indexDouble);
      int indexAfter = (int) Math.ceil(indexDouble);

      double fractionBetweenPoints = indexDouble - indexBefore;

      FramePoint3D beforePoint = new FramePoint3D(listOfPoints.get(indexBefore));
      FramePoint3D afterPoint = new FramePoint3D(listOfPoints.get(indexAfter));

      FrameVector3D vectorBetweenPoints = new FrameVector3D(beforePoint.getReferenceFrame());
      vectorBetweenPoints.sub(afterPoint, beforePoint);

      if (fractionBetweenPoints == 0.0)
      {
         vectorBetweenPoints.set(0.0, 0.0, 0.0);
      }
      else
      {
         vectorBetweenPoints.scale(fractionBetweenPoints);
      }

      FramePoint3D returnPoint = new FramePoint3D(beforePoint);
      returnPoint.add(vectorBetweenPoints);

      if ((addToInternalRepresentation) && (beforePoint.distance(returnPoint) > 1e-7) && (afterPoint.distance(returnPoint) > 1e-7))
      {
         ArrayList<FramePoint3D> newListOfPoints = new ArrayList<FramePoint3D>();
         for (int i = 0; i <= indexBefore; i++)
         {
            newListOfPoints.add(listOfPoints.get(i));
         }

         newListOfPoints.add(returnPoint);

         for (int i = indexAfter; i < listOfPoints.size(); i++)
         {
            newListOfPoints.add(listOfPoints.get(i));
         }

         listOfPoints = newListOfPoints;
         setupAlphaArrayAndLinearInterpolator();
      }

      return returnPoint;
   }

   public FramePoint3D getPointOnPathDistanceFromStart(double distanceFromStart)
   {
      double pathLength = this.getPathLength();
      if (pathLength < 1e-12)
         return listOfPoints.get(0);

      double alpha = distanceFromStart / pathLength;

      return getPointOnPath(alpha);
   }


   public FramePoint3D getPointOnPathDistanceFromStartAndAddToInternalList(double distanceFromStart)
   {
      double pathLength = this.getPathLength();
      if (pathLength < 1e-12)
         return listOfPoints.get(0);

      double alpha = distanceFromStart / pathLength;

      return getPointOnPathAndAddToInternalList(alpha);
   }




   public static ArrayList<FramePoint3D> expandList(ArrayList<FramePoint3D> listOfPoints, int numberOfPoints)
   {
//    if (numberOfPoints <= listOfPoints.size())
//       return new ArrayList<FramePoint>(listOfPoints);

      ListOfPointsTrajectory listOfPointsTrajectory = new ListOfPointsTrajectory(listOfPoints);
      double percentStepSize = 1.0 / (numberOfPoints - 1.0);

      ArrayList<FramePoint3D> ret = new ArrayList<FramePoint3D>();
      for (double alpha = 0.0; alpha <= 1.0; alpha = alpha + percentStepSize)
      {
         ret.add(listOfPointsTrajectory.getPointOnPath(alpha));
      }

      return ret;
   }


// public void visualizeListOfPointsTrajectory(BagOfBalls bagOfBalls)
// {
//    bagOfBalls.reset();
//
//    for (double alpha = 0.0; alpha < 1.0; alpha = alpha + 0.01)
//    {
//       bagOfBalls.setBall(getPointOnPath(alpha));
//    }
// }

   public static void main(String[] args)
   {
      FramePoint3D framePoint;
      ArrayList<FramePoint3D> list = new ArrayList<FramePoint3D>();

      ReferenceFrame referenceFrame = ReferenceFrame.getWorldFrame();

      framePoint = new FramePoint3D(referenceFrame, 0.0, 0.0, 0.0);
      list.add(framePoint);

//    framePoint = new FramePoint(LittleDogFrames.getWorldFrame(), 1.0, 0.0, 0.0);
//    list.add(framePoint);
//
//    framePoint = new FramePoint(LittleDogFrames.getWorldFrame(), 1.0, 0.0, 0.0);
//    list.add(framePoint);

      ListOfPointsTrajectory listOfPointsTrajectory = null;
      try
      {
         listOfPointsTrajectory = new ListOfPointsTrajectory(list);
      }
      catch (Exception ex)
      {
      }

      for (double alpha = 0.0; alpha <= 1.0; alpha = alpha + 0.1)
      {
         FramePoint3D testPoint = listOfPointsTrajectory.getPointOnPath(alpha);
         System.out.println("testPoint=" + testPoint);
      }

   }


}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/newtonsCradle/PileOfRandomObjectsRobot.java<|end_filename|>
package us.ihmc.exampleSimulations.newtonsCradle;

import java.util.ArrayList;
import java.util.Random;

import us.ihmc.commons.RandomNumbers;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.robotics.robotDescription.CollisionMeshDescription;
import us.ihmc.robotics.robotDescription.FloatingJointDescription;
import us.ihmc.robotics.robotDescription.LinkDescription;
import us.ihmc.robotics.robotDescription.LinkGraphicsDescription;
import us.ihmc.robotics.robotDescription.RobotDescription;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.RobotFromDescription;

public class PileOfRandomObjectsRobot
{
   private final ArrayList<Robot> robots = new ArrayList<Robot>();
   private int collisionGroup = 0xffffffff;
   private int collisionMask = 0xffffffff;

   private final int estimatedContactPointsPerShape = 8;

   private final Random random = new Random(1886L);

   private double xExtents = 1.5;
   private double yExtents = 1.5;

   private double zMin = 0.2;
   private double zMax = 6.0;

   private double sizeScale = 1.0;
   private double massScale = 1.0;

   private int numberOfObjects = 200;

   public PileOfRandomObjectsRobot()
   {
   }

   public void setSizeScale(double sizeScale)
   {
      this.sizeScale = sizeScale;
   }

   public void setMassScale(double massScale)
   {
      this.massScale = massScale;
   }

   public void setGroupAndCollisionMask(int collisionGroup, int collisionMask)
   {
      this.collisionGroup = collisionGroup;
      this.collisionMask = collisionMask;
   }

   public void setNumberOfObjects(int numberOfObjects)
   {
      this.numberOfObjects = numberOfObjects;
   }

   public void setXYExtents(double xExtents, double yExtents)
   {
      this.xExtents = xExtents;
      this.yExtents = yExtents;
   }

   public void setZMinAndMax(double zMin, double zMax)
   {
      this.zMin = zMin;
      this.zMax = zMax;
   }

   private void createFallingObjects()
   {
      for (int i = 0; i < numberOfObjects; i++)
      {
         RobotDescription robotDescription = new RobotDescription("RandomRobot" + i);
         //         Robot robot = new Robot("RandomRobot" + i);

         Vector3D offset = new Vector3D(0.0, 0.0, 0.0);
         FloatingJointDescription floatingJointDescription = new FloatingJointDescription("object" + i, "object" + i);
         floatingJointDescription.setOffsetFromParentJoint(offset);

         LinkDescription link;

         int shape = random.nextInt(4);
         if (shape == 0)
         {
            link = createRandomBox(random, i);
         }
         else if (shape == 1)
         {
            link = createRandomSphere(random, i);
         }
         else if (shape == 2)
         {
            link = createRandomCapsule(random, i);
         }
         else
         {
            link = createRandomCylinder(random, i);
         }

         floatingJointDescription.setLink(link);
         robotDescription.addRootJoint(floatingJointDescription);

         double x = RandomNumbers.nextDouble(random, -xExtents, xExtents);
         double y = RandomNumbers.nextDouble(random, -yExtents, yExtents);

         double z = RandomNumbers.nextDouble(random, zMin, zMax);

         double angleExtents = Math.PI / 2.0;
         double yaw = RandomNumbers.nextDouble(random, -angleExtents, angleExtents);
         double pitch = RandomNumbers.nextDouble(random, -angleExtents, angleExtents);
         double roll = RandomNumbers.nextDouble(random, -angleExtents, angleExtents);

         Robot robot = new RobotFromDescription(robotDescription);

         FloatingJoint floatingJoint = (FloatingJoint) robot.getRootJoints().get(0);

         floatingJoint.setPosition(x, y, z);
         floatingJoint.setYawPitchRoll(yaw, pitch, roll);

         this.robots.add(robot);
      }
   }

   //   private void createBoardFrame(Random random)
   //   {
   //      double boardZ = 0.14;
   //
   //      FloatingJoint board0 = createContainerBoard("board0", random);
   //      board0.setPosition(0.51, 0.0, boardZ);
   //      board0.setYawPitchRoll(Math.PI / 2.0, 0.0, 0.0);
   //
   //      FloatingJoint board1 = createContainerBoard("board1", random);
   //      board1.setPosition(0.0, 0.35, boardZ);
   //      board1.setYawPitchRoll(0.0, 0.0, 0.0);
   //
   //      FloatingJoint board2 = createContainerBoard("board2", random);
   //      board2.setPosition(0.0, -0.35, boardZ);
   //      board2.setYawPitchRoll(0.0, 0.0, 0.0);
   //
   //      FloatingJoint board3 = createContainerBoard("board3", random);
   //      board3.setPosition(-0.51, 0.0, boardZ);
   //      board3.setYawPitchRoll(Math.PI / 2.0, 0.0, 0.0);
   //   }

   //   private FloatingJoint createContainerBoard(String name, Random random)
   //   {
   //      Robot robot = new Robot(name);
   //
   //      Vector3d offset = new Vector3d(0.0, 0.0, 0.0);
   //      FloatingJoint floatingJoint = new FloatingJoint(name, offset, robot);
   //
   //      Link link = createContainerBoardLink(name, random, robot);
   //      floatingJoint.setLink(link);
   //      robot.addRootJoint(floatingJoint);
   //      this.robots.add(robot);
   //
   //      return floatingJoint;
   //   }

   //   private Link createContainerBoardLink(String name, Random random, Robot robot)
   //   {
   //      double objectWidth = 0.2;
   //      double objectLength = 0.8;
   //      double objectHeight = 0.2;
   //      double objectMass = 10.0;
   //
   //      Link link = new Link(name);
   //      link.setMassAndRadiiOfGyration(objectMass, objectLength / 2.0, objectWidth / 2.0, objectHeight / 2.0);
   //      link.setComOffset(0.0, 0.0, 0.0);
   //
   //      Graphics3DObject linkGraphics = new Graphics3DObject();
   //      linkGraphics.translate(0.0, 0.0, -objectHeight / 2.0);
   //      AppearanceDefinition randomColor = YoAppearance.Aquamarine();
   //
   //      linkGraphics.addCube(objectLength, objectWidth, objectHeight, randomColor);
   //      link.setLinkGraphics(linkGraphics);
   //
   //      CollisionMeshDescription collisionMeshDescription = new CollisionMeshDescription();
   //      collisionMeshDescription.addCubeReferencedAtCenter(objectLength, objectWidth, objectHeight);
   //      collisionMeshDescription.setCollisionGroup(0xff);
   //      collisionMeshDescription.setCollisionMask(0xff);
   //      link.addCollisionMesh(collisionMeshDescription);
   //
   //      return link;
   //   }

   private LinkDescription createRandomBox(Random random, int i)
   {
      double objectLength = sizeScale * RandomNumbers.nextDouble(random, 0.04, 0.1);
      double objectWidth = sizeScale * RandomNumbers.nextDouble(random, 0.04, 0.2);
      double objectHeight = sizeScale * RandomNumbers.nextDouble(random, 0.04, 0.1);
      double objectMass = massScale * RandomNumbers.nextDouble(random, 0.2, 1.0);

      LinkDescription link = new LinkDescription("object" + i);
      link.setMassAndRadiiOfGyration(objectMass, objectLength / 2.0, objectWidth / 2.0, objectHeight / 2.0);
      link.setCenterOfMassOffset(0.0, 0.0, 0.0);

      LinkGraphicsDescription linkGraphics = new LinkGraphicsDescription();
      linkGraphics.translate(0.0, 0.0, -objectHeight / 2.0);
      AppearanceDefinition randomColor = YoAppearance.randomColor(random);
      linkGraphics.addCube(objectLength, objectWidth, objectHeight, randomColor);
      link.setLinkGraphics(linkGraphics);

      CollisionMeshDescription collisionMesh = new CollisionMeshDescription();
      collisionMesh.addCubeReferencedAtCenter(objectLength, objectWidth, objectHeight);
      collisionMesh.setCollisionGroup(collisionGroup);
      collisionMesh.setCollisionMask(collisionMask);
      collisionMesh.setEstimatedNumberOfContactPoints(estimatedContactPointsPerShape);
      link.addCollisionMesh(collisionMesh);

      return link;
   }

   private LinkDescription createRandomSphere(Random random, int i)
   {
      double objectRadius = sizeScale * RandomNumbers.nextDouble(random, 0.01, 0.05);
      double objectMass = massScale * RandomNumbers.nextDouble(random, 0.2, 1.0);

      LinkDescription link = new LinkDescription("object" + i);
      link.setMassAndRadiiOfGyration(objectMass, objectRadius / 2.0, objectRadius / 2.0, objectRadius / 2.0);
      link.setCenterOfMassOffset(0.0, 0.0, 0.0);

      LinkGraphicsDescription linkGraphics = new LinkGraphicsDescription();
      AppearanceDefinition randomColor = YoAppearance.randomColor(random);
      linkGraphics.addSphere(objectRadius, randomColor);
      link.setLinkGraphics(linkGraphics);

      CollisionMeshDescription collisionMesh = new CollisionMeshDescription();
      collisionMesh.addSphere(objectRadius);
      collisionMesh.setCollisionGroup(collisionGroup);
      collisionMesh.setCollisionMask(collisionMask);
      collisionMesh.setEstimatedNumberOfContactPoints(16);
      link.addCollisionMesh(collisionMesh);

      return link;
   }

   private LinkDescription createRandomCapsule(Random random, int i)
   {
      double objectRadius = sizeScale * RandomNumbers.nextDouble(random, 0.01, 0.05);
      double objectHeight = sizeScale * 2.0 * objectRadius + RandomNumbers.nextDouble(random, 0.02, 0.05);
      double objectMass = massScale * RandomNumbers.nextDouble(random, 0.2, 1.0);

      LinkDescription link = new LinkDescription("object" + i);
      link.setMassAndRadiiOfGyration(objectMass, objectRadius / 2.0, objectRadius / 2.0, objectHeight / 2.0);
      link.setCenterOfMassOffset(0.0, 0.0, 0.0);

      LinkGraphicsDescription linkGraphics = new LinkGraphicsDescription();
      AppearanceDefinition randomColor = YoAppearance.randomColor(random);
      linkGraphics.addCapsule(objectRadius, objectHeight, randomColor);
      link.setLinkGraphics(linkGraphics);

      CollisionMeshDescription collisionMesh = new CollisionMeshDescription();
      collisionMesh.addCapsule(objectRadius, objectHeight);
      collisionMesh.setCollisionGroup(collisionGroup);
      collisionMesh.setCollisionMask(collisionMask);
      collisionMesh.setEstimatedNumberOfContactPoints(16);
      link.addCollisionMesh(collisionMesh);

      return link;
   }

   private LinkDescription createRandomCylinder(Random random, int i)
   {
      double objectHeight = sizeScale * RandomNumbers.nextDouble(random, 0.05, 0.15);
      double objectRadius = sizeScale * RandomNumbers.nextDouble(random, 0.01, 0.10);
      double objectMass = massScale * RandomNumbers.nextDouble(random, 0.2, 1.0);

      LinkDescription link = new LinkDescription("object" + i);
      link.setMassAndRadiiOfGyration(objectMass, objectRadius / 2.0, objectRadius / 2.0, objectHeight / 2.0);
      link.setCenterOfMassOffset(0.0, 0.0, 0.0);

      LinkGraphicsDescription linkGraphics = new LinkGraphicsDescription();
      linkGraphics.translate(0.0, 0.0, -objectHeight / 2.0);

      AppearanceDefinition randomColor = YoAppearance.randomColor(random);
      linkGraphics.addCylinder(objectHeight, objectRadius, randomColor);
      link.setLinkGraphics(linkGraphics);

      CollisionMeshDescription collisionMesh = new CollisionMeshDescription();
      collisionMesh.addCylinderReferencedAtCenter(objectRadius, objectHeight);
      collisionMesh.setCollisionGroup(collisionGroup);
      collisionMesh.setCollisionMask(collisionMask);
      collisionMesh.setEstimatedNumberOfContactPoints(16);
      link.addCollisionMesh(collisionMesh);

      return link;
   }

   public ArrayList<Robot> createAndGetRobots()
   {
      this.createFallingObjects();
      return robots;
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandLoadBearingMessage.java<|end_filename|>
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.
       * Request the controller to use a hand to help supporting the robot weight.
       */
public class HandLoadBearingMessage extends Packet<HandLoadBearingMessage> implements Settable<HandLoadBearingMessage>, EpsilonComparable<HandLoadBearingMessage>
{
   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_;
   /**
            * The robot side of the hand that will be load bearing.
            */
   public byte robot_side_ = (byte) 255;
   /**
            * Determines whether hybrid load bearing and jointspace control will be used.
            */
   public boolean use_jointspace_command_;
   /**
            * The arm desired jointspace trajectory that will be used for hybrid control if use_jointspace_command is true.
            * The indexing for the joints goes increasingly from the first shoulder joint to the last arm joint.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage jointspace_trajectory_;
   /**
            * The time to delay this message on the controller side before being executed.
            */
   public double execution_delay_time_;
   /**
            * Information specific to the load bearing properties.
            */
   public controller_msgs.msg.dds.LoadBearingMessage load_bearing_message_;

   public HandLoadBearingMessage()
   {
      jointspace_trajectory_ = new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
      load_bearing_message_ = new controller_msgs.msg.dds.LoadBearingMessage();
   }

   public HandLoadBearingMessage(HandLoadBearingMessage other)
   {
      this();
      set(other);
   }

   public void set(HandLoadBearingMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      use_jointspace_command_ = other.use_jointspace_command_;

      controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.staticCopy(other.jointspace_trajectory_, jointspace_trajectory_);
      execution_delay_time_ = other.execution_delay_time_;

      controller_msgs.msg.dds.LoadBearingMessagePubSubType.staticCopy(other.load_bearing_message_, load_bearing_message_);
   }

   /**
            * 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_;
   }

   /**
            * The robot side of the hand that will be load bearing.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * The robot side of the hand that will be load bearing.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }

   /**
            * Determines whether hybrid load bearing and jointspace control will be used.
            */
   public void setUseJointspaceCommand(boolean use_jointspace_command)
   {
      use_jointspace_command_ = use_jointspace_command;
   }
   /**
            * Determines whether hybrid load bearing and jointspace control will be used.
            */
   public boolean getUseJointspaceCommand()
   {
      return use_jointspace_command_;
   }


   /**
            * The arm desired jointspace trajectory that will be used for hybrid control if use_jointspace_command is true.
            * The indexing for the joints goes increasingly from the first shoulder joint to the last arm joint.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage getJointspaceTrajectory()
   {
      return jointspace_trajectory_;
   }

   /**
            * The time to delay this message on the controller side before being executed.
            */
   public void setExecutionDelayTime(double execution_delay_time)
   {
      execution_delay_time_ = execution_delay_time;
   }
   /**
            * The time to delay this message on the controller side before being executed.
            */
   public double getExecutionDelayTime()
   {
      return execution_delay_time_;
   }


   /**
            * Information specific to the load bearing properties.
            */
   public controller_msgs.msg.dds.LoadBearingMessage getLoadBearingMessage()
   {
      return load_bearing_message_;
   }


   public static Supplier<HandLoadBearingMessagePubSubType> getPubSubType()
   {
      return HandLoadBearingMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HandLoadBearingMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HandLoadBearingMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.use_jointspace_command_, other.use_jointspace_command_, epsilon)) return false;

      if (!this.jointspace_trajectory_.epsilonEquals(other.jointspace_trajectory_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_delay_time_, other.execution_delay_time_, epsilon)) return false;

      if (!this.load_bearing_message_.epsilonEquals(other.load_bearing_message_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HandLoadBearingMessage)) return false;

      HandLoadBearingMessage otherMyClass = (HandLoadBearingMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if(this.use_jointspace_command_ != otherMyClass.use_jointspace_command_) return false;

      if (!this.jointspace_trajectory_.equals(otherMyClass.jointspace_trajectory_)) return false;
      if(this.execution_delay_time_ != otherMyClass.execution_delay_time_) return false;

      if (!this.load_bearing_message_.equals(otherMyClass.load_bearing_message_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HandLoadBearingMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("use_jointspace_command=");
      builder.append(this.use_jointspace_command_);      builder.append(", ");
      builder.append("jointspace_trajectory=");
      builder.append(this.jointspace_trajectory_);      builder.append(", ");
      builder.append("execution_delay_time=");
      builder.append(this.execution_delay_time_);      builder.append(", ");
      builder.append("load_bearing_message=");
      builder.append(this.load_bearing_message_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/scripts/engine/ScriptTransformer.java<|end_filename|>
package us.ihmc.humanoidBehaviors.behaviors.scripts.engine;

import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.StandardCopyOption;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import com.thoughtworks.xstream.converters.ConversionException;
import com.thoughtworks.xstream.io.StreamException;
import com.thoughtworks.xstream.mapper.CannotResolveClassException;

import us.ihmc.commons.PrintTools;
import us.ihmc.euclid.transform.RigidBodyTransform;

public abstract class ScriptTransformer
{
   private final List<File> filesToTransform = new ArrayList<>();
   private final HashSet<String> listOfDirectoriesContainingAtLeastOneScript = new HashSet<>();
   private final List<String> listOfDirectoriesToCreate = new ArrayList<String>();

   private final RigidBodyTransform identity = new RigidBodyTransform();

   public ScriptTransformer(String scriptDirectoryPath) throws IOException, InterruptedException
   {
      findScripts(scriptDirectoryPath, filesToTransform, listOfDirectoriesContainingAtLeastOneScript);

      for (File scriptFileToTransform : filesToTransform)
      {
    	  if (scriptFileToTransform.isDirectory())
    	  {
    		  System.err.println("File should not be a director!?");
    		  continue;
    	  }

    	  File parentDirectory = scriptFileToTransform.getParentFile();
    	  if (parentDirectory.isDirectory())
    	  {
    	     File subDirectoryToMoveOriginals = new File(parentDirectory, "OriginalBackups");
    	     if (!subDirectoryToMoveOriginals.exists()) subDirectoryToMoveOriginals.mkdir();

    	     File scriptFileToTransformBackup = new File(subDirectoryToMoveOriginals, scriptFileToTransform.getName());
    	     Files.copy(scriptFileToTransform.toPath(), scriptFileToTransformBackup.toPath(), StandardCopyOption.REPLACE_EXISTING);
    	  }

         ArrayList<ScriptObject> scriptObjects = new ArrayList<>();
         ArrayList<Object> newScriptObjects = new ArrayList<>();
         ScriptFileLoader scriptFileLoader = null;

         String scriptName = scriptFileToTransform.getName();

         try
         {
            scriptFileLoader = new ScriptFileLoader(scriptFileToTransform);
         }
         catch (StreamException e)
         {
            PrintTools.error("Empty script: " + scriptName);
            continue;
         }

         while (true)
         {
            try
            {
               scriptObjects.add(scriptFileLoader.getScriptObject(identity));
            }
            catch (ConversionException | CannotResolveClassException e)
            {
               if (e.getCause() != null)
                  PrintTools.error("In script: " + scriptName + ". Problem loading packet, cause: " + e.getCause().getMessage());
               else
                  PrintTools.error("In script: " + scriptName + ". Problem loading packet, exception: " + e.getClass().getSimpleName() + ", message: " + e.getMessage());
               continue;
            }
            catch (IOException | StreamException e)
            {
               break;
            }
         }
         scriptFileLoader.close();

         for (ScriptObject scriptObject : scriptObjects)
         {
            Object object = scriptObject.getScriptObject();
            Object newScriptObject = transformScriptObject(object);
            newScriptObjects.add(newScriptObject);
         }

         File newScript = new File(scriptFileToTransform.getParentFile(), scriptFileToTransform.getName());
         ScriptFileSaver scriptFileSaver = null;
         try
         {
            scriptFileSaver = new ScriptFileSaver(newScript, true);
         }
         catch (IOException e)
         {
            PrintTools.error("During writing: " + e.getClass().getSimpleName() + " for the file:" + newScript);
            if (e.getCause() != null)
               PrintTools.error(e.getCause().getMessage());
            else
               PrintTools.error(e.getMessage());
            continue;
         }

         for (Object newObject : newScriptObjects)
         {
            ScriptObject newScriptObject = new ScriptObject(System.currentTimeMillis(), newObject);
            scriptFileSaver.recordObject(newScriptObject.getTimeStamp(), newScriptObject.getScriptObject());
         }

         scriptFileSaver.close();
         System.out.println("Done transforming script: " + newScript);
      }
   }

   public abstract Object transformScriptObject(Object object);

   private static void findScripts(final String scriptDirectoryPath, final List<File> foundScriptFilesToPack,
		   final Set<String> listOfDirectoriesContainingAtLeastOneScript) throws IOException
   {
	   final List<String> potentialSubDirectories = new ArrayList<String>();

	   File directory = new File(scriptDirectoryPath);
	   if (!directory.isDirectory())
	   {
	      if (directory.getName().endsWith(".xml"))
         {
            System.out.println("Found script file: " + directory.getName());
            foundScriptFilesToPack.add(directory);
         }
		   return;
	   }

	   System.out.println(directory.getAbsolutePath() + " is a directory. Exploring contents");

	   File[] filesInDirectory = directory.listFiles();

	   for (File file : filesInDirectory)
	   {
		   String filename = file.getName();
		   if (filename.endsWith(".xml"))
		   {
			   System.out.println("Found script file: " + filename);
			   foundScriptFilesToPack.add(file);
		   }
	   }

	   for (int i = 0; i < potentialSubDirectories.size(); i++)
	   {
		   findScripts(potentialSubDirectories.get(i), foundScriptFilesToPack, listOfDirectoriesContainingAtLeastOneScript);
	   }
   }

   public Iterable<File> getFilesToTransform()
   {
      return filesToTransform;
   }

   public Iterable<String> getListOfOriginalDirectories()
   {
      return listOfDirectoriesContainingAtLeastOneScript;
   }

   public Iterable<String> getListOfNewDirectories()
   {
      return listOfDirectoriesToCreate;
   }

}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/HumanoidRobotPunchMessenger.java<|end_filename|>
package us.ihmc.avatar.joystickBasedJavaFXController;

import us.ihmc.robotics.robotSide.RobotSide;

public interface HumanoidRobotPunchMessenger
{
   void sendArmHomeConfiguration(double trajectoryDuration, RobotSide... robotSides);

   void sendArmStraightConfiguration(double trajectoryDuration, RobotSide robotSide);
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/controller/REABasicUIController.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.controller;

import java.io.File;

import javafx.scene.control.ComboBox;
import javafx.scene.control.Slider;
import javafx.scene.control.Spinner;
import javafx.scene.control.ToggleButton;
import us.ihmc.messager.MessagerAPIFactory.Topic;
import us.ihmc.robotEnvironmentAwareness.communication.REAUIMessager;
import us.ihmc.robotEnvironmentAwareness.io.FilePropertyHelper;

public abstract class REABasicUIController
{
   protected REAUIMessager uiMessager;
   private FilePropertyHelper filePropertyHelper;

   public abstract void bindControls();

   public REABasicUIController()
   {
   }

   public void attachREAMessager(REAUIMessager uiMessager)
   {
      this.uiMessager = uiMessager;
   }

   public void setConfigurationFile(File configurationFile)
   {
      filePropertyHelper = new FilePropertyHelper(configurationFile);
   }

   protected void saveUIControlProperty(Topic<Boolean> topic, ToggleButton toggleButton)
   {
      filePropertyHelper.saveProperty(topic.getName(), toggleButton.isSelected());
   }

   protected <T> void saveUIControlProperty(Topic<T> topic, ComboBox<T> comboBox)
   {
      filePropertyHelper.saveProperty(topic.getName(), comboBox.getValue().toString());
   }

   protected void saveUIControlProperty(Topic<? extends Number> topic, Slider slider)
   {
      filePropertyHelper.saveProperty(topic.getName(), slider.getValue());
   }

   protected void loadUIControlProperty(Topic<Boolean> topic, ToggleButton toggleButton)
   {
      Boolean loadedProperty = filePropertyHelper.loadBooleanProperty(topic.getName());
      if (loadedProperty != null)
         toggleButton.setSelected(loadedProperty);
   }

   protected void loadUIControlProperty(Topic<? extends Number> topic, Slider slider)
   {
      Double loadedProperty = filePropertyHelper.loadDoubleProperty(topic.getName());
      if (loadedProperty != null)
         slider.setValue(loadedProperty);
   }

   protected void loadUIControlProperty(Topic<Double> topic, Spinner<Double> spinner)
   {
      Double loadedProperty = filePropertyHelper.loadDoubleProperty(topic.getName());
      if (loadedProperty != null)
         spinner.getValueFactory().setValue(loadedProperty);
   }

   @SuppressWarnings("unchecked")
   protected <T extends Enum<T>> void loadUIControlProperty(Topic<T> topic, ComboBox<T> comboBox)
   {
      String loadedProperty = filePropertyHelper.loadProperty(topic.getName());
      if (loadedProperty != null)
      {
         T valueOf = (T) Enum.valueOf(comboBox.getItems().get(0).getClass(), loadedProperty);
         comboBox.setValue(valueOf);
      }
   }
}

<|start_filename|>ihmc-interfaces/src/rosdocker/windows/Dockerfile<|end_filename|>
FROM microsoft/windowsservercore:1803 as ros2_build_base_win

# I guess this makes the image smaller
ENV ChocolateyUseWindowsCompression false

# Install chocolatey
RUN @"%SystemRoot%\System32\WindowsPowerShell\v1.0\powershell.exe" -NoProfile -InputFormat None -ExecutionPolicy Bypass \
    -Command "iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'))" && \
    SET "PATH=%PATH%;%ALLUSERSPROFILE%\chocolatey\bin"

# From https://github.com/ros2/ros2/wiki/Windows-Install-Binary

# Install command line utilities
RUN choco install -y python --version 3.6.5
RUN choco install -y cmake curl 7zip patch git which
RUN setx PATH "%PATH%;C:\Python37\Scripts"

# Install tee (useful to see status while logging to file)
WORKDIR C:/Users/ContainerUser/Downloads
RUN curl -skL https://downloads.sourceforge.net/project/unxutils/unxutils/current/UnxUtils.zip -o UnxUtils.zip
RUN 7z x UnxUtils.zip -y -oUnxUtils
RUN md C:\tee
RUN copy UnxUtils\usr\local\wbin\tee.exe C:\tee\tee.exe
RUN setx PATH "%PATH%;C:\tee"

# Install OpenSSL
WORKDIR C:/Users/ContainerUser/Downloads
RUN curl -sk https://slproweb.com/download/Win64OpenSSL-1_0_2o.exe -o Win64OpenSSL.exe
RUN Win64OpenSSL.exe /VERYSILENT
RUN setx -m OPENSSL_CONF C:\OpenSSL-Win64\bin\openssl.cfg
RUN setx PATH "%PATH%;C:\OpenSSL-Win64\bin"

# Install Visual Studio
RUN choco install -y visualstudio2017community visualstudio2017-workload-nativedesktop

# Install OpenCV
WORKDIR C:/Users/ContainerUser/Downloads
RUN curl -skL https://github.com/ros2/ros2/releases/download/opencv-archives/opencv-3.4.1-vc15.VS2017.zip -o opencv.zip
RUN dir
RUN 7z x opencv.zip -y -oc:
RUN setx -m OpenCV_DIR C:\opencv
RUN setx PATH "%PATH%;C:\opencv\x64\vc15\bin"

# Install ROS choco dependencies
WORKDIR C:/Users/ContainerUser/Downloads
RUN curl -skL https://github.com/ros2/choco-packages/releases/download/2018-06-12-1/asio.1.12.1.nupkg -o asio.1.12.1.nupkg
RUN curl -skL https://github.com/ros2/choco-packages/releases/download/2018-06-12-1/eigen.3.3.4.nupkg -o eigen.3.3.4.nupkg
RUN curl -skL https://github.com/ros2/choco-packages/releases/download/2018-06-12-1/tinyxml-usestl.2.6.2.nupkg -o tinyxml-usestl.2.6.2.nupkg
RUN curl -skL https://github.com/ros2/choco-packages/releases/download/2018-06-12-1/tinyxml2.6.0.0.nupkg -o tinyxml2.6.0.0.nupkg
RUN choco install -y -s C:/Users/ContainerUser/Downloads asio eigen tinyxml-usestl tinyxml2

# Install python dependencies
RUN python -m pip install --upgrade setuptools pip
RUN pip install --upgrade catkin_pkg empy pyparsing pyyaml

# From https://github.com/ros2/ros2/wiki/Windows-Development-Setup

# Additional Prerequisites
RUN setx PATH "%PATH%;C:\Program Files\Git\cmd;C:\Program Files\CMake\bin"

# Installing Developer Tools
RUN pip install vcstool
RUN pip install --upgrade colcon-common-extensions

# Install dependencies
RUN pip install pytest coverage mock
RUN pip install flake8 flake8-blind-except flake8-builtins flake8-class-newline flake8-comprehensions
RUN pip install flake8-deprecated flake8-docstrings flake8-import-order flake8-quotes pep8 pydocstyle
RUN choco install -y cppcheck
RUN setx PATH "%PATH%;C:\Program Files\Cppcheck"

# Skip Install Qt5

# Getting the Source Code
ARG ROS_DISTRO
ENV ROS_DISTRO ${ROS_DISTRO}
WORKDIR C:/
RUN md dev\ros2\src
WORKDIR C:/dev/ros2
RUN curl -sk https://raw.githubusercontent.com/ros2/ros2/release-%ROS_DISTRO%/ros2.repos -o ros2.repos
RUN vcs import src < ros2.repos

# Building the ROS 2 Code
SHELL ["C:\\Windows\\System32\\cmd.exe", "/c", "\"C:\\Program Files (x86)\\Microsoft Visual Studio\\2017\\Community\\VC\\Auxiliary\\Build\\vcvars64.bat\" && "]
RUN colcon build --merge-install --packages-skip rviz

WORKDIR C:/dev/ros2
ENTRYPOINT ["C:\\Windows\\System32\\cmd.exe", "/k", "C:\\Program Files (x86)\\Microsoft Visual Studio\\2017\\Community\\VC\\Auxiliary\\Build\\vcvars64.bat"]
#ENTRYPOINT ["C:\\Windows\\System32\\cmd.exe"]


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/RigidBodyExplorationConfigurationMessage.java<|end_filename|>
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;

/**
       * Message used to configure the exploration for a RRT-based planner.
       * Main usage is the IHMC WholeBodyTrajectoryToolbox.
       */
public class RigidBodyExplorationConfigurationMessage extends Packet<RigidBodyExplorationConfigurationMessage> implements Settable<RigidBodyExplorationConfigurationMessage>, EpsilonComparable<RigidBodyExplorationConfigurationMessage>
{
   public static final byte CONFIGURATION_SPACE_NAME_X = (byte) 0;
   public static final byte CONFIGURATION_SPACE_NAME_Y = (byte) 1;
   public static final byte CONFIGURATION_SPACE_NAME_Z = (byte) 2;
   public static final byte CONFIGURATION_SPACE_NAME_ROLL = (byte) 3;
   public static final byte CONFIGURATION_SPACE_NAME_PITCH = (byte) 4;
   public static final byte CONFIGURATION_SPACE_NAME_YAW = (byte) 5;
   public static final byte CONFIGURATION_SPACE_NAME_SO3 = (byte) 6;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int rigid_body_hash_code_;
   public us.ihmc.idl.IDLSequence.Byte  configuration_space_names_to_explore_;
   public us.ihmc.idl.IDLSequence.Double  exploration_range_upper_limits_;
   public us.ihmc.idl.IDLSequence.Double  exploration_range_lower_limits_;

   public RigidBodyExplorationConfigurationMessage()
   {
      configuration_space_names_to_explore_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9");

      exploration_range_upper_limits_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      exploration_range_lower_limits_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

   }

   public RigidBodyExplorationConfigurationMessage(RigidBodyExplorationConfigurationMessage other)
   {
      this();
      set(other);
   }

   public void set(RigidBodyExplorationConfigurationMessage other)
   {
      sequence_id_ = other.sequence_id_;

      rigid_body_hash_code_ = other.rigid_body_hash_code_;

      configuration_space_names_to_explore_.set(other.configuration_space_names_to_explore_);
      exploration_range_upper_limits_.set(other.exploration_range_upper_limits_);
      exploration_range_lower_limits_.set(other.exploration_range_lower_limits_);
   }

   /**
            * 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 void setRigidBodyHashCode(int rigid_body_hash_code)
   {
      rigid_body_hash_code_ = rigid_body_hash_code;
   }
   public int getRigidBodyHashCode()
   {
      return rigid_body_hash_code_;
   }


   public us.ihmc.idl.IDLSequence.Byte  getConfigurationSpaceNamesToExplore()
   {
      return configuration_space_names_to_explore_;
   }


   public us.ihmc.idl.IDLSequence.Double  getExplorationRangeUpperLimits()
   {
      return exploration_range_upper_limits_;
   }


   public us.ihmc.idl.IDLSequence.Double  getExplorationRangeLowerLimits()
   {
      return exploration_range_lower_limits_;
   }


   public static Supplier<RigidBodyExplorationConfigurationMessagePubSubType> getPubSubType()
   {
      return RigidBodyExplorationConfigurationMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return RigidBodyExplorationConfigurationMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(RigidBodyExplorationConfigurationMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.rigid_body_hash_code_, other.rigid_body_hash_code_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.configuration_space_names_to_explore_, other.configuration_space_names_to_explore_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.exploration_range_upper_limits_, other.exploration_range_upper_limits_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.exploration_range_lower_limits_, other.exploration_range_lower_limits_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof RigidBodyExplorationConfigurationMessage)) return false;

      RigidBodyExplorationConfigurationMessage otherMyClass = (RigidBodyExplorationConfigurationMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.rigid_body_hash_code_ != otherMyClass.rigid_body_hash_code_) return false;

      if (!this.configuration_space_names_to_explore_.equals(otherMyClass.configuration_space_names_to_explore_)) return false;
      if (!this.exploration_range_upper_limits_.equals(otherMyClass.exploration_range_upper_limits_)) return false;
      if (!this.exploration_range_lower_limits_.equals(otherMyClass.exploration_range_lower_limits_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("RigidBodyExplorationConfigurationMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("rigid_body_hash_code=");
      builder.append(this.rigid_body_hash_code_);      builder.append(", ");
      builder.append("configuration_space_names_to_explore=");
      builder.append(this.configuration_space_names_to_explore_);      builder.append(", ");
      builder.append("exploration_range_upper_limits=");
      builder.append(this.exploration_range_upper_limits_);      builder.append(", ");
      builder.append("exploration_range_lower_limits=");
      builder.append(this.exploration_range_lower_limits_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullLeggedRobotModel.java<|end_filename|>
package us.ihmc.robotModels;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.partNames.LimbName;
import us.ihmc.robotics.robotSide.RobotSegment;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SegmentDependentList;

public interface FullLeggedRobotModel<E extends Enum<E> & RobotSegment<E>> extends FullRobotModel
{
   /**
    *  Returns the {@link ReferenceFrame} attached right after the specified leg joint.
    *  Its origin is at the joint location.
    *  When all the joints are at 0, the {@link ReferenceFrame} is oriented: x = forward, y = left, and z = up.
    *
    * @param robotSegment Refers to which leg the frame's joint belongs to (assuming there is only one left leg and one right leg).
    * @param legJointName Refers to which joint the frame is attached to.
    */
   MovingReferenceFrame getFrameAfterLegJoint(E robotSegment, LegJointName legJointName);

   /**
    * Return the {@link OneDoFJointBasics} describing the the corresponding leg joint.
    * @param robotSegment Refers to which leg the joint belongs to (assuming there is only one left leg and one right leg).
    * @param legJointName Refers to the joint's name.
    */
   OneDoFJointBasics getLegJoint(E robotSegment, LegJointName legJointName);

   /**
    * Returns the {@link RigidBodyBasics} describing the left or right foot of this robot.
    * A foot is considered as the end-effector (thus the last {@RigidBody}) of the leg to which it belongs to.
    *
    * @param robotSegment Refers to which side the foot belongs to (assuming there is only one left foot and one right foot).
    */
   RigidBodyBasics getFoot(E robotSegment);

   /**
    * This method is equivalent to:
    * <p>{@link FullLeggedRobotModel#getFoot(E)} when {@code limbName == LimbName.LEG},</p>
    * <p>{@link FullHumanoidRobotModel#getHand(RobotSide)} when {@code limbName == LimbName.ARM}.</p>
    * @param robotSegment refers to the end-effector side.
    * @param limbName refers to the limb the end-effector is attached to (either a leg or an arm).
    */
   RigidBodyBasics getEndEffector(E robotSegment, LimbName limbName);

   /**
    * This methods returns the frame located right after the parent joint of this end-effector (see {@link FullLeggedRobotModel#getEndEffector(E, LimbName)}).
    */
   MovingReferenceFrame getEndEffectorFrame(E robotSegment, LimbName limbName);

   /**
    * Returns the left or right sole reference frame.
    * A sole frame is attached to a foot and is generally used to put the foot contact points.
    * Its origin is right in the middle of the bottom of the foot.
    * @param robotSegment
    * @return
    */
   MovingReferenceFrame getSoleFrame(E robotSegment);

   SegmentDependentList<E, MovingReferenceFrame> getSoleFrames();

   E[] getRobotSegments();
}

<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/FeedbackControllerInterface.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController;

import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.InverseDynamicsCommand;
import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseKinematics.InverseKinematicsCommand;
import us.ihmc.commonWalkingControlModules.controllerCore.command.virtualModelControl.VirtualModelControlCommand;

public interface FeedbackControllerInterface
{
   void initialize();

   void setEnabled(boolean isEnabled);

   void computeInverseDynamics();

   void computeInverseKinematics();

   void computeVirtualModelControl();

   void computeAchievedAcceleration();

   boolean isEnabled();

   InverseDynamicsCommand<?> getInverseDynamicsOutput();

   InverseKinematicsCommand<?> getInverseKinematicsOutput();

   VirtualModelControlCommand<?> getVirtualModelControlOutput();
}

<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/DoorEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationConstructionSetTools.util.environments.environmentRobots.ContactableDoorRobot;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationConstructionSetTools.robotController.ContactController;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class DoorEnvironment implements CommonAvatarEnvironmentInterface
{
   private final List<ContactableDoorRobot> doorRobots = new ArrayList<ContactableDoorRobot>();
   private final CombinedTerrainObject3D combinedTerrainObject;
      
   private final ArrayList<ExternalForcePoint> contactPoints = new ArrayList<ExternalForcePoint>();

   public static final Point3D DEFAULT_DOOR_LOCATION = new Point3D(3.0, 0.0, 0.0);
   
   public DoorEnvironment()
   {
      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject.addTerrainObject(setUpGround("Ground"));
      
      ContactableDoorRobot door = new ContactableDoorRobot("doorRobot", DEFAULT_DOOR_LOCATION);
      doorRobots.add(door);
      door.createAvailableContactPoints(0, 15, 15, 0.02, true);
   }
   
   private CombinedTerrainObject3D setUpGround(String name)
   {
      CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);

      combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.DarkGray());
      combinedTerrainObject.addBox(2.0, -0.05, 3.0, 0.05, 2.0, YoAppearance.Beige());
      combinedTerrainObject.addBox(3.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.getX(), -0.05, 4.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.getX(), 0.05, 2.0, YoAppearance.Beige());
      
      return combinedTerrainObject;
   }
   
   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return doorRobots;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
      ContactController contactController = new ContactController();
      contactController.setContactParameters(100000.0, 100.0, 0.5, 0.3);

      contactController.addContactPoints(contactPoints);
      contactController.addContactables(doorRobots);
      doorRobots.get(0).setController(contactController);    
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
      this.contactPoints.addAll(externalForcePoints);      
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/PelvisOffsetWhileWalkingParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

public class PelvisOffsetWhileWalkingParameters
{
   /**
    * Whether or not to utilize pelvis orientation angle modifications when walking to create natural pelvis motion
    */
   public boolean addPelvisOrientationOffsetsFromWalkingMotion()
   {
      return false;
   }

   /**
    * Multiplier of the step angle to determine the desired pelvis yaw magnitude when walking
    */
   public double getPelvisYawRatioOfStepAngle()
   {
      return 0.25;
   }

   /**
    * Step length in meters to add the pelvis yawing motion
    */
   public double getStepLengthToAddYawingMotion()
   {
      return 0.03;
   }

   /**
    * Multiplier of the leg angle to determine the desired pelvis pitch magnitude when walking
    */
   public double getPelvisPitchRatioOfLegAngle()
   {
      return 0.3;
   }

   /**
    * Fraction of the end part of the swing phase in which to interpolate the pelvis tilt between the stance leg
    * and the upcoming stance leg. Effectively forces the pelvis to "turn over" sooner.
    * Must be between 0.0 and 1.0.
    */
   public double getFractionOfSwingPitchingFromUpcomingLeg()
   {
      return 0.15;
   }

   /**
    * Fraction of the first part of the swing phase in which to interpolate the pelvis tilt between the stance leg
    * and the swing leg. Effectively delays the pelvis from "turning over".
    * Must be between 0.0 and 1.0.
    */
   public double getFractionOfSwingPitchingFromSwingLeg()
   {
      return 0.2;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/BoundingBoxParametersMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.tuple3D.Point3D;

public class BoundingBoxParametersMessage extends Packet<BoundingBoxParametersMessage>
{
   public float minX, minY, minZ;
   public float maxX, maxY, maxZ;

   public BoundingBoxParametersMessage()
   {
   }

   public BoundingBoxParametersMessage(BoundingBoxParametersMessage other)
   {
      set(other);
   }

   @Override
   public void set(BoundingBoxParametersMessage other)
   {
      setPacketInformation(other);
      minX = other.minX;
      minY = other.minY;
      minZ = other.minZ;
      maxX = other.maxX;
      maxY = other.maxY;
      maxZ = other.maxZ;
   }

   public float getMinX()
   {
      return minX;
   }

   public float getMinY()
   {
      return minY;
   }

   public float getMinZ()
   {
      return minZ;
   }

   public float getMaxX()
   {
      return maxX;
   }

   public float getMaxY()
   {
      return maxY;
   }

   public float getMaxZ()
   {
      return maxZ;
   }

   public void setMinX(float minX)
   {
      this.minX = minX;
   }

   public void setMinY(float minY)
   {
      this.minY = minY;
   }

   public void setMinZ(float minZ)
   {
      this.minZ = minZ;
   }

   public void setMaxX(float maxX)
   {
      this.maxX = maxX;
   }

   public void setMaxY(float maxY)
   {
      this.maxY = maxY;
   }

   public void setMaxZ(float maxZ)
   {
      this.maxZ = maxZ;
   }

   public Point3D getMin()
   {
      return new Point3D(minX, minY, minZ);
   }

   public Point3D getMax()
   {
      return new Point3D(maxX, maxY, maxZ);
   }

   @Override
   public boolean epsilonEquals(BoundingBoxParametersMessage other, double epsilon)
   {
      if (Float.compare(minX, other.minX) != 0)
         return false;
      if (Float.compare(minY, other.minY) != 0)
         return false;
      if (Float.compare(minZ, other.minZ) != 0)
         return false;
      if (Float.compare(maxX, other.maxX) != 0)
         return false;
      if (Float.compare(maxY, other.maxY) != 0)
         return false;
      if (Float.compare(maxZ, other.maxZ) != 0)
         return false;
      return true;
   }

   @Override
   public String toString()
   {
      return "min: (" + minX + ", " + minY + ", " + minZ + "), max: (" + maxX + ", " + maxY + ", " + maxZ + ")";
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosTfPublisherInterface.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import us.ihmc.euclid.transform.RigidBodyTransform;

public interface RosTfPublisherInterface
{
   public void publish(RigidBodyTransform transform3d, long timeStamp, String parentFrame, String childFrame);
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/scripts/engine/ScriptFileLoader.java<|end_filename|>
package us.ihmc.humanoidBehaviors.behaviors.scripts.engine;

import java.io.BufferedInputStream;
import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;
import java.io.InputStream;
import java.io.ObjectInputStream;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.ArrayList;

import com.thoughtworks.xstream.XStream;
import com.thoughtworks.xstream.io.StreamException;
import com.thoughtworks.xstream.mapper.MapperWrapper;

import us.ihmc.commons.PrintTools;
import us.ihmc.euclid.transform.RigidBodyTransform;

public class ScriptFileLoader
{
   private final BufferedReader reader;
   private ObjectInputStream inputStream;
   
   public ScriptFileLoader(Path scriptFilePath) throws IOException, StreamException
   {
      PrintTools.info("Working directory: " + System.getProperty("user.dir"));
      if (!Files.exists(scriptFilePath))
      {
         throw new IOException("!Files.exists(path): " + scriptFilePath.toString());
      }
      XStream xStream = new XStream()
      {
         @Override
         protected MapperWrapper wrapMapper(MapperWrapper next)
         {
            return new MapperWrapper(next)
            {
               @Override
               public boolean shouldSerializeMember(@SuppressWarnings("rawtypes") Class definedIn, String fieldName)
               {
                  return definedIn != Object.class ? super.shouldSerializeMember(definedIn, fieldName) : false;
               }
            };
         }
      };

      reader = Files.newBufferedReader(scriptFilePath);
      inputStream = xStream.createObjectInputStream(reader);
   }
   
   
   public ScriptFileLoader(File file) throws IOException
   {
	   this(new BufferedInputStream(new FileInputStream(file)));
   }
   
   public ScriptFileLoader(InputStream scriptInputStream) throws IOException
   {
	   XStream xStream = new XStream()
	      {
	         @Override
            protected MapperWrapper wrapMapper(MapperWrapper next)
	         {
	            return new MapperWrapper(next)
	            {

	               @Override
                  public boolean shouldSerializeMember(@SuppressWarnings("rawtypes") Class definedIn, String fieldName)
	               {
	                  return definedIn != Object.class ? super.shouldSerializeMember(definedIn, fieldName) : false;
	               }
	            };
	         }
	      };
	      
	      reader = null;
	      inputStream = xStream.createObjectInputStream(scriptInputStream);
   }

   public void close()
   {
      try
      {
    	  if(reader != null)
    	  {
    		  reader.close();    		  
    	  }
         inputStream.close();
      }
      catch (IOException e)
      {
         System.err.println(e);
      }
   }

   public Object readObject() throws IOException
   {
      try
      {
         return inputStream.readObject();
      }
      catch (ClassNotFoundException e)
      {
         throw new RuntimeException(e);
      }
   }

   public long getTimestamp() throws IOException
   {
      return inputStream.readLong();
   }

   @SuppressWarnings("rawtypes")
   public Object getObject(RigidBodyTransform transform) throws IOException
   {
      Object object;
      try
      {
         object = inputStream.readObject();
         MessageTransformer.transform(object, transform);
         return object;
      }
      catch (ClassNotFoundException e)
      {
         throw new RuntimeException(e);
      }
   }

   public ScriptObject getScriptObject(RigidBodyTransform transform) throws IOException
   {
      long timestamp = getTimestamp();
      Object object = getObject(transform);
      return new ScriptObject(timestamp, object);
   }

   public ArrayList<ScriptObject> readIntoList()
   {
      return readIntoList(new RigidBodyTransform());
   }

   public ArrayList<ScriptObject> readIntoList(RigidBodyTransform transform)
   {
      ArrayList<ScriptObject> list = new ArrayList<ScriptObject>();
      while (true)
      {
         try
         {
            list.add(getScriptObject(transform));
         }
         catch (IOException e)
         {
            break;
         }
      }
      return list;
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/eventBased/Observer.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.eventBased;

/**
 * Interface for classes that listen to events emitted by an Observable.
 *
 * @author <NAME>
 *
 */
public interface Observer
{
   /**
    * handles an event generated by an Observable
    * @param source event source, i.e. the observable that generated the event
    * @param event event generated by the source
    */
   public abstract void handleEvent(Observable source, Event event);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/hyperCubeTree/SphericalLinearResolutionProvider.java<|end_filename|>
package us.ihmc.robotics.hyperCubeTree;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;

public class SphericalLinearResolutionProvider implements ResolutionProvider
{
   private static final boolean DEBUG = false;
   private final FramePoint3D center;
   private final FramePoint3D centerInWorldTemp;
   private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
   private final double[] tempCenter = new double[3];
   private final OneDimensionalBounds linearRegion;
   private final double centerResolution;
   private final double exteriorResolution;
   private final double outerRadiusSquared;
   private final double innerRadiusSquared;
   private final LowPassTimingReporter timer = new LowPassTimingReporter(5);

   public SphericalLinearResolutionProvider(FramePoint3D center, double radius1, double innerResolution, double radius2, double outerResolution)
   {
      this(center, new OneDimensionalBounds(Math.abs(radius1), Math.abs(radius2)), innerResolution, outerResolution);
   }

   public SphericalLinearResolutionProvider(FramePoint3D center, OneDimensionalBounds bounds, double innerResolution, double exteriorResolution)
   {
      this.center = center;
      centerInWorldTemp = new FramePoint3D(center);
      centerInWorldTemp.changeFrame(worldFrame);
      this.linearRegion = bounds;
      this.centerResolution = innerResolution;
      this.exteriorResolution = exteriorResolution;
      outerRadiusSquared = linearRegion.max() * linearRegion.max();
      innerRadiusSquared = linearRegion.min() * linearRegion.min();
      if (DEBUG)
         timer.setupRecording("SphericalLinearResolutionProvider", "getResolution", 5000, 5000);
   }

   public double getResolution(double[] location)
   {
      timer.startTime();
      double distanceSquared = 0;


      centerInWorldTemp.setIncludingFrame(center);
      centerInWorldTemp.changeFrame(worldFrame);
      centerInWorldTemp.get(tempCenter);

      for (int i = 0; i < location.length; i++)
      {
         double difference = location[i] - tempCenter[i];
         distanceSquared += difference * difference;
      }

      if (outerRadiusSquared <= (distanceSquared))
         return exteriorResolution;
      if (innerRadiusSquared >= distanceSquared)
         return centerResolution;
      double distance = Math.sqrt(distanceSquared);
      double resolution = centerResolution + (exteriorResolution - centerResolution) * linearRegion.unScale(distance);
      timer.endTime();

      return resolution;
   }

   public double getMinResolution()
   {
      return centerResolution;
   }


}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/StateTransitionAction.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

@Deprecated
public interface StateTransitionAction
{
   public abstract void doTransitionAction();
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/clouds/detect/alg/LocalFitShapeNN.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.clouds.detect.alg;

import java.util.ArrayList;
import java.util.List;

import org.ddogleg.fitting.modelset.DistanceFromModel;
import org.ddogleg.fitting.modelset.ModelCodec;
import org.ddogleg.fitting.modelset.ModelFitter;

import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.CheckShapeParameters;

/**
 * Finds a locally optimal best fit set of points and model parameters to a shape in a point cloud with nearest-neighbor
 * information.  Initially the algorithm is given a set of points and parameters which belong to
 * and describe the shape, respectively.  The points then are used to refine the model parameters.  A new set
 * of points which match the new parameters is found.  This process repeats until the model parameters converge or
 * it runs out of iterations.
 *
 * @author <NAME>
 */
public class LocalFitShapeNN<Model> {

	// used to estimate the shape's parameters
	private ModelFitter<Model, PointVectorNN> fitter;
	// converts the model parameters to and from double[].  Used to test for convergence
	private ModelCodec<Model> codec;

	// finds points in the local neighbor hood which match the model
	private FindMatchSetPointVectorNN<Model> findMatchSet;

	// distance which defines an inlier
	private double distanceThreshold;

	// maximum number of iterations
	private int maxIterations;
	// minimum amount of change between model parameters before convergence is declared
	private double minimumChangeThreshold;

	// storage for shape parameters converted into double[]
	private double paramPrev[] = new double[1];
	private double paramCurr[] = new double[1];

	// stores points which fit the model
	private List<PointVectorNN> listTempA = new ArrayList<PointVectorNN>();
	private List<PointVectorNN> listTempB = new ArrayList<PointVectorNN>();

	// used to see if the parameters are valid
	private CheckShapeParameters<Model> checkParam;

	/**
	 * Configures the search and fit algorithm
	 *
	 * @param maxIterations          Maximum number of inlier select and model estimate iterations it will perform.
	 * @param minimumChangeThreshold When the average change in model parameters is less than this value
	 *                               iteration will stop.
	 * @param findMatchSet           The code which searches for neighbors that match the provided model
	 */
	public LocalFitShapeNN(int maxIterations,
						   double minimumChangeThreshold,
						   FindMatchSetPointVectorNN<Model> findMatchSet) {
		this.maxIterations = maxIterations;
		this.minimumChangeThreshold = minimumChangeThreshold;
		this.findMatchSet = findMatchSet;
	}

	/**
	 * Specifies algorithms for computing he distance between the shape and a point, refining the shape parameters,
	 * and converting the model parameters into an array.
	 *
	 * @param fitter    Uses a set of points and an initial guess to estimate the shape's parameters
	 * @param distance  Computes the distance between a shape and a point
	 * @param codec     Converts the shape parameter to and from double array
	 * @param threshold Threshold which defined an inlier.  inlier <= threshold
	 */
	public void configure(ModelFitter<Model, PointVectorNN> fitter,
						  DistanceFromModel<Model, PointVectorNN> distance,
						  CheckShapeParameters<Model> checkParam,
						  ModelCodec<Model> codec,
						  double threshold) {
		this.fitter = fitter;
		this.checkParam = checkParam;
		this.codec = codec;
		this.distanceThreshold = threshold;

		findMatchSet.setModelDistance(distance);

		if (paramPrev.length < codec.getParamLength()) {
			paramPrev = new double[codec.getParamLength()];
			paramCurr = new double[codec.getParamLength()];
		}
	}

	/**
	 * Refines the set of points which belong to the shape and the model parameters which define the shape
	 * <p/>
	 * NOTE: The initialParam is used inside this function to store intermediate results<br>
	 *
	 * @param matches            (input) Initial set of points which belong to the shape.  (output) Set of points which match the new shape.
	 * @param model              (input) Initial description of the shape. (output) The newly estimated shape description.
	 * @param initialFitToPoints (input) If true it will fit the model parameters to the initial points.  If false
	 *                           it will start by selecting points which match the initial model.
	 */
	public boolean refine(List<PointVectorNN> matches, Model model, boolean initialFitToPoints) {
		codec.encode(model, paramPrev);
		if (initialFitToPoints)
			fitter.fitModel(matches, model, model);

		listTempA.clear();
		listTempA.addAll(matches);

		int iter = 0;
		while (true) {
			// find list of points which match the model
			listTempB.clear();
			// find the points which match the model
			findMatchSet.selectMatchSet(listTempA, model, distanceThreshold, true, listTempB);
			// use the points which match the model to estimate the parameters.
			fitter.fitModel(listTempB, model, model);

			// if the model has drifted into the invalid range, stop processing
			if (!checkParam.valid(model))
				return false;

			// Compute the change in parameters
			codec.encode(model, paramCurr);

			double change = 0;
			for (int i = 0; i < codec.getParamLength(); i++) {
				double d = paramCurr[i] - paramPrev[i];
				change += Math.abs(d);
			}
			change /= codec.getParamLength();

			// check to see if it has converged
			iter++;
			if (change <= minimumChangeThreshold || iter >= maxIterations) {
				break;
			} else {
				// swap current and previous
				List<PointVectorNN> tempL = listTempB;
				listTempB = listTempA;
				listTempA = tempL;

				double tempD[] = paramCurr;
				paramCurr = paramPrev;
				paramPrev = tempD;
			}
		}

		matches.clear();
		matches.addAll(listTempB);

		return true;
	}

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/environmentRobots/ContactableStaticRobot.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.environmentRobots;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.simulationconstructionset.GroundContactPoint;
import us.ihmc.simulationconstructionset.Joint;
import us.ihmc.simulationconstructionset.RigidJoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.Contactable;

public abstract class ContactableStaticRobot extends Robot implements Contactable
{
   private final InternalSingleJointArticulatedContactable articulatedContactable;

   public ContactableStaticRobot(String name)
   {
      super(name);
      articulatedContactable = new InternalSingleJointArticulatedContactable(name, this); 
   }
   
   public abstract RigidJoint getNullJoint();

   private static class InternalSingleJointArticulatedContactable extends SingleJointArticulatedContactable
   {
      private final ContactableStaticRobot contactableRobot;

      public InternalSingleJointArticulatedContactable(String name, ContactableStaticRobot robot)
      {
         super(name, robot);
         this.contactableRobot = robot;
      }

      @Override
      public boolean isClose(Point3D pointInWorldToCheck)
      {
         return contactableRobot.isClose(pointInWorldToCheck);
      }

      @Override
      public boolean isPointOnOrInside(Point3D pointInWorldToCheck)
      {
         return contactableRobot.isPointOnOrInside(pointInWorldToCheck);
      }

      @Override
      public void closestIntersectionAndNormalAt(Point3D intersectionToPack, Vector3D normalToPack, Point3D pointInWorldToCheck)
      {
         contactableRobot.closestIntersectionAndNormalAt(intersectionToPack, normalToPack, pointInWorldToCheck);
      }

      @Override
      public Joint getJoint()
      {
         return contactableRobot.getNullJoint();
      }
   }
   
   public void createAvailableContactPoints(int groupIdentifier, int totalContactPointsAvailable, double forceVectorScale, boolean addYoGraphicForceVectorsForceVectors)
   {
      articulatedContactable.createAvailableContactPoints(groupIdentifier, totalContactPointsAvailable, forceVectorScale, addYoGraphicForceVectorsForceVectors);
   }

   @Override
   public int getAndLockAvailableContactPoint()
   {
      return articulatedContactable.getAndLockAvailableContactPoint();
   }

   @Override
   public void unlockContactPoint(GroundContactPoint groundContactPoint)
   {
      articulatedContactable.unlockContactPoint(groundContactPoint);
   }

   @Override
   public GroundContactPoint getLockedContactPoint(int contactPointIndex)
   {
      return articulatedContactable.getLockedContactPoint(contactPointIndex);
   }

   @Override
   public void updateContactPoints()
   {
      articulatedContactable.updateContactPoints();
   }

   public void getBodyTransformToWorld(RigidBodyTransform transformToWorld)
   {
      getNullJoint().getTransformToWorld(transformToWorld);
   }

}



<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/EnumDependentSliderBoardMapping.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public interface EnumDependentSliderBoardMapping<T extends Enum<T>> extends SliderBoardConfiguration, EnumDependentInputMapping<T>
{

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/SymmetricPID3DGains.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains.implementations;

import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly;
import us.ihmc.robotics.controllers.pidGains.PIDGainsReadOnly;

public class SymmetricPID3DGains implements PID3DGainsReadOnly
{
   private PIDGainsReadOnly gains;

   private final double[] tempProportionalGains = new double[3];
   private final double[] tempDerivativeGains = new double[3];
   private final double[] tempIntegralGains = new double[3];

   public void setGains(PIDGainsReadOnly gains)
   {
      this.gains = gains;
   }

   @Override
   public double[] getProportionalGains()
   {
      return fill(tempProportionalGains, gains.getKp());
   }

   @Override
   public double[] getDerivativeGains()
   {
      return fill(tempDerivativeGains, gains.getKd());
   }

   @Override
   public double[] getIntegralGains()
   {
      return fill(tempIntegralGains, gains.getKi());
   }

   @Override
   public double getMaximumIntegralError()
   {
      return gains.getMaxIntegralError();
   }

   @Override
   public double getMaximumDerivativeError()
   {
      return Double.POSITIVE_INFINITY;
   }

   @Override
   public double getMaximumProportionalError()
   {
      return Double.POSITIVE_INFINITY;
   }

   @Override
   public double getMaximumFeedback()
   {
      return gains.getMaximumFeedback();
   }

   @Override
   public double getMaximumFeedbackRate()
   {
      return gains.getMaximumFeedbackRate();
   }

   private static double[] fill(double[] arrayToFill, double value)
   {
      arrayToFill[0] = value;
      arrayToFill[1] = value;
      arrayToFill[2] = value;
      return arrayToFill;
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LidarScanParametersMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "LidarScanParametersMessage" defined in "LidarScanParametersMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from LidarScanParametersMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit LidarScanParametersMessage_.idl instead.
*
*/
public class LidarScanParametersMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.LidarScanParametersMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::LidarScanParametersMessage_";

   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.LidarScanParametersMessage 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.LidarScanParametersMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      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.LidarScanParametersMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.LidarScanParametersMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.LidarScanParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_11(data.getTimestamp());

      cdr.write_type_5(data.getSweepYawMax());

      cdr.write_type_5(data.getSweepYawMin());

      cdr.write_type_5(data.getHeightPitchMax());

      cdr.write_type_5(data.getHeightPitchMin());

      cdr.write_type_5(data.getTimeIncrement());

      cdr.write_type_5(data.getScanTime());

      cdr.write_type_5(data.getMinRange());

      cdr.write_type_5(data.getMaxRange());

      cdr.write_type_2(data.getPointsPerSweep());

      cdr.write_type_2(data.getScanHeight());

   }

   public static void read(controller_msgs.msg.dds.LidarScanParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setTimestamp(cdr.read_type_11());
      	
      data.setSweepYawMax(cdr.read_type_5());
      	
      data.setSweepYawMin(cdr.read_type_5());
      	
      data.setHeightPitchMax(cdr.read_type_5());
      	
      data.setHeightPitchMin(cdr.read_type_5());
      	
      data.setTimeIncrement(cdr.read_type_5());
      	
      data.setScanTime(cdr.read_type_5());
      	
      data.setMinRange(cdr.read_type_5());
      	
      data.setMaxRange(cdr.read_type_5());
      	
      data.setPointsPerSweep(cdr.read_type_2());
      	
      data.setScanHeight(cdr.read_type_2());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.LidarScanParametersMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_11("timestamp", data.getTimestamp());
      ser.write_type_5("sweep_yaw_max", data.getSweepYawMax());
      ser.write_type_5("sweep_yaw_min", data.getSweepYawMin());
      ser.write_type_5("height_pitch_max", data.getHeightPitchMax());
      ser.write_type_5("height_pitch_min", data.getHeightPitchMin());
      ser.write_type_5("time_increment", data.getTimeIncrement());
      ser.write_type_5("scan_time", data.getScanTime());
      ser.write_type_5("min_range", data.getMinRange());
      ser.write_type_5("max_range", data.getMaxRange());
      ser.write_type_2("points_per_sweep", data.getPointsPerSweep());
      ser.write_type_2("scan_height", data.getScanHeight());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.LidarScanParametersMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setTimestamp(ser.read_type_11("timestamp"));
      data.setSweepYawMax(ser.read_type_5("sweep_yaw_max"));
      data.setSweepYawMin(ser.read_type_5("sweep_yaw_min"));
      data.setHeightPitchMax(ser.read_type_5("height_pitch_max"));
      data.setHeightPitchMin(ser.read_type_5("height_pitch_min"));
      data.setTimeIncrement(ser.read_type_5("time_increment"));
      data.setScanTime(ser.read_type_5("scan_time"));
      data.setMinRange(ser.read_type_5("min_range"));
      data.setMaxRange(ser.read_type_5("max_range"));
      data.setPointsPerSweep(ser.read_type_2("points_per_sweep"));
      data.setScanHeight(ser.read_type_2("scan_height"));
   }

   public static void staticCopy(controller_msgs.msg.dds.LidarScanParametersMessage src, controller_msgs.msg.dds.LidarScanParametersMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.LidarScanParametersMessage createData()
   {
      return new controller_msgs.msg.dds.LidarScanParametersMessage();
   }
   @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.LidarScanParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.LidarScanParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.LidarScanParametersMessage src, controller_msgs.msg.dds.LidarScanParametersMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public LidarScanParametersMessagePubSubType newInstance()
   {
      return new LidarScanParametersMessagePubSubType();
   }
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/converters/LogFormatUpdater.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger.converters;

import java.io.File;
import java.io.IOException;

import us.ihmc.robotDataLogger.LogProperties;

public class LogFormatUpdater
{
   public static void updateLogs(File directory, LogProperties properties)
   {
      try
      {
//         if(properties.getModelLoaderClass() == null)
//         {
//            LogModels model = ModelAttacher.chooseModel(directory);
//            ModelAttacher.addModel(directory, properties, model);
//         }
         
         if (properties.getVariables().getCompressed() && !properties.getVariables().getTimestamped())
         {
            LogTimeStampedIndexGenerator.convert(directory, properties);
         }
      }
      catch (IOException e)
      {
         throw new RuntimeException("Cannot convert log file", e);
      }
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/bambooTools/videoWall/VideoPlayer.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.bambooTools.videoWall;

import java.awt.image.BufferedImage;
import java.io.IOException;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.TimeUnit;

import javax.swing.ImageIcon;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.SwingUtilities;

import us.ihmc.codecs.demuxer.MP4VideoDemuxer;
import us.ihmc.codecs.generated.FilterModeEnum;
import us.ihmc.codecs.generated.YUVPicture;
import us.ihmc.codecs.yuv.YUVPictureConverter;
import us.ihmc.commons.PrintTools;
import us.ihmc.commons.thread.ThreadTools;

public class VideoPlayer
{
   private static final double FRAME_RATE = 15.0; //TODO: This should be 30.0 according to the file information. Something is inconsistent...
   private static final boolean LOOP_FOREVER = true;
   private static int videoPlayerThreadIndex = 0;
   private static int demuxerThreadIndex = 0;
   
   private final JLabel jLabel;
   private final JPanel jPanel;
   private YUVPictureConverter converter = new YUVPictureConverter();
   private BufferedImage bufferedImage;
   private boolean readyForUpdate;
   private Path currentPathToVideo;
   private MP4VideoDemuxer demuxer;
   private ImageIcon imageIcon;
   private boolean timedOut;

   public VideoPlayer()
   {
      jLabel = new JLabel();
      jPanel = new JPanel();
      
      jPanel.add(jLabel);
      readyForUpdate = true;
      
      startPlaying();
   }
   
   public void startPlaying()
   {
      PrintTools.info(garbageCollectAndPrintUsedMemoryInMB(false) + "startPlaying() ");
      
      ExecutorService executor = Executors.newSingleThreadExecutor(ThreadTools.getNamedThreadFactory(getClass().getSimpleName() + videoPlayerThreadIndex++));
      executor.execute(new Runnable()
      {
         @Override
         public void run()
         {
            while (true)
            {
               if (currentPathToVideo == null)
               {
                  ThreadTools.sleep(10);
               }
               else
               {
                  boolean demuxerCreatedSuccessfully = tryToMakeDemuxer(currentPathToVideo);
                  if (demuxerCreatedSuccessfully)
                  {
                     playVideo(demuxer);
                  }
                  else
                  {
                     currentPathToVideo = null;
                  }
                  
                  if (demuxer != null)
                  {
                     demuxer.delete();
                  }
               }
               
               readyForUpdate = true;
            }
         }
      });
      executor.shutdown();
   }
   
   private boolean tryToMakeDemuxer(final Path path)
   {
      PrintTools.debug(this, garbageCollectAndPrintUsedMemoryInMB(false) + " tryToMakeDemuxer " + path.getFileName().toString());
      
      timedOut = true;
      ThreadTools.executeWithTimeout(MP4VideoDemuxer.class.getSimpleName() + demuxerThreadIndex++, new Runnable()
      {
         @Override
         public void run()
         {
            try
            {
               demuxer = new MP4VideoDemuxer(path.toFile());
               timedOut = false;
            }
            catch (IOException e)
            {
               e.printStackTrace();
            }
         }
      }, 1000, TimeUnit.MILLISECONDS);
      if (timedOut)
         PrintTools.error("Demuxer.init() timed out!");
      return timedOut;
   }

   public void playVideo(MP4VideoDemuxer demuxer)
   {
      try
      {
         long sleepTimeMillisPerFrame = (long) (1000.0 / FRAME_RATE);
         long previousTimeMillis = System.currentTimeMillis();

         demuxer.seekToFrame(0);
         YUVPicture picture;
         while (true)
         {
            picture = demuxer.getNextFrame();
            
            if (picture == null)
            {
               if (LOOP_FOREVER)
               {
                  demuxer.seekToFrame(0);
                  return;
               }
               else
               {
                  break;
               }
            }
            
            long currentTimeMillis = System.currentTimeMillis();

            doTheNextFrame(picture);

            long timeDelta = currentTimeMillis - previousTimeMillis;
            long timeToSleep = sleepTimeMillisPerFrame - timeDelta;
            if (timeToSleep < 1)
               timeToSleep = 1;
            if (timeToSleep > sleepTimeMillisPerFrame)
               timeToSleep = sleepTimeMillisPerFrame;

            ThreadTools.sleep(timeToSleep);
            previousTimeMillis = currentTimeMillis;
         }
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }
   }

   private void doTheNextFrame(YUVPicture picture)
   {
      int width = jPanel.getWidth();
      int height = jPanel.getHeight();
      
      if (width < 1 || height < 1)
         return;
      
      picture.scale(width, height, FilterModeEnum.kFilterBilinear);
      
      bufferedImage = converter.toBufferedImage(picture, bufferedImage);
      
      SwingUtilities.invokeLater(new Runnable()
      {
         @Override
         public void run()
         {
            imageIcon = new ImageIcon(bufferedImage);
            jLabel.setIcon(imageIcon);
         }
      });
      picture.delete();
   }
   
   public void updateVideoPath(Path pathToVideo)
   {
      readyForUpdate = false;
      currentPathToVideo = pathToVideo;
   }
   
   public boolean readyToUpdateVideoPath()
   {
      return readyForUpdate;
   }

   public JPanel getjPanel()
   {
      return jPanel;
   }
   
   private String garbageCollectAndPrintUsedMemoryInMB(boolean doGarbageCollect)
   {
      Runtime runtime = Runtime.getRuntime();

      if (doGarbageCollect)
      {
         System.gc();
         try
         {
            Thread.sleep(100);
         }
         catch (InterruptedException e)
         {
            e.printStackTrace();
         }
         System.gc();
      }

      long freeMemory = runtime.freeMemory();
      long totalMemory = runtime.totalMemory();
      long usedMemory = totalMemory - freeMemory;

      int usedMemoryMB = (int) (usedMemory / 1000000);

      return "USED MEMORY: " + usedMemoryMB + " MB: ";
   }
   
   static int i = 0;
   
   public static void main(String[] args)
   {
      final ArrayList<MP4VideoDemuxer> demuxers = new ArrayList<>();

      for (i = 0; i < 1000; i++)
      {
         ThreadTools.executeWithTimeout("demuxer" + i, new Runnable()
         {
            @Override
            public void run()
            {
               MP4VideoDemuxer demuxer;
               try
               {
                  System.out.println("i = " + i);
                  demuxer = new MP4VideoDemuxer(Paths.get("20150920_0009_Atlas_DRCPushRecoveryMultiStepTest.testMultiStepForwardAndContinueWalking.mp4").toFile());
                  
                  for (int j = 0; j < 5; j++)
                  {
                  demuxer.getNextFrame();
                  }
                  demuxer.delete();
                  demuxers.add(demuxer);
               }
               catch (IOException e)
               {
                  e.printStackTrace();
               }
            }
         }, 500, TimeUnit.MILLISECONDS);
      }
      
      System.gc();

      ThreadTools.sleepSeconds(10.0);
   }
}

<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/sensorProcessors/RootJointPerfectSensorOutputMapReadOnly.java<|end_filename|>
package us.ihmc.sensorProcessing.sensorProcessors;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;

/**
 * Interface to provide perfect sensor information of the root joint   
 * @author jesper
 *
 */
public interface RootJointPerfectSensorOutputMapReadOnly
{
   public void packRootJointTransform(RigidBodyTransform transformToPack);
   
   public void packRootJointLinearVelocityInBody(Vector3D linearVelocityToPack);
   public void packRootJointAngularVelocityInBody(Vector3D angularVelocityToPack);
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/subscriber/RosArmJointTrajectorySubscriber.java<|end_filename|>
package us.ihmc.avatar.ros.subscriber;

import java.util.ArrayList;

import controller_msgs.msg.dds.ArmTrajectoryMessage;
import controller_msgs.msg.dds.OneDoFJointTrajectoryMessage;
import trajectory_msgs.JointTrajectory;
import us.ihmc.commons.Conversions;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.PacketDestination;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotics.partNames.ArmJointName;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.utilities.ros.subscriber.AbstractRosTopicSubscriber;

public class RosArmJointTrajectorySubscriber extends AbstractRosTopicSubscriber<trajectory_msgs.JointTrajectory>
{
   private final PacketCommunicator packetCommunicator;
   private final ArrayList<String> leftArmNames = new ArrayList<String>();
   private final ArrayList<String> rightArmNames = new ArrayList<String>();

   public RosArmJointTrajectorySubscriber(PacketCommunicator controllerCommunicationBridge, FullHumanoidRobotModel fullRobotModel)
   {
      super(trajectory_msgs.JointTrajectory._TYPE);
      this.packetCommunicator = controllerCommunicationBridge;
      
      ArmJointName[] armJointNames = fullRobotModel.getRobotSpecificJointNames().getArmJointNames();
      
      for (int i = 0; i < armJointNames.length; i++)
      {
         ArmJointName jointName = armJointNames[i];
         leftArmNames.add(fullRobotModel.getArmJoint(RobotSide.LEFT, jointName).getName());
         rightArmNames.add(fullRobotModel.getArmJoint(RobotSide.RIGHT, jointName).getName());
      }
   }

   @Override
   public void onNewMessage(JointTrajectory rosMessage)
   {
      RobotSide robotSide;
      if (leftArmNames.equals(rosMessage.getJointNames()))
      {
         robotSide = RobotSide.LEFT;
      }
      else if (rightArmNames.equals(rosMessage.getJointNames()))
      {
         robotSide = RobotSide.RIGHT;
      }
      else
      {
         System.out.println(getHelp());
         System.err.println(getHelp());
         return;
      }
      
      int numberOfJoints = rosMessage.getJointNames().size();
      int numberOfWaypoints = rosMessage.getPoints().size();
      
      ArmTrajectoryMessage ihmcMessage = HumanoidMessageTools.createArmTrajectoryMessage(robotSide);
      RecyclingArrayList<OneDoFJointTrajectoryMessage> jointTrajectoryMessages = ihmcMessage.getJointspaceTrajectory().getJointTrajectoryMessages();
      for (int i = 0; i < numberOfJoints; i++)
         jointTrajectoryMessages.add();

      for (int waypointIndex = 0; waypointIndex < numberOfWaypoints; waypointIndex++)
      {
         double[] positions = rosMessage.getPoints().get(waypointIndex).getPositions();
         double[] velocities = rosMessage.getPoints().get(waypointIndex).getVelocities();
         
         if (positions.length != numberOfJoints || positions.length != velocities.length)
         {
            String msg = "Number of joints positions or velocities in JointTrajectoryPoint inconsistent with expected number of joints " + numberOfJoints;
            System.out.println(msg);
            System.err.println(msg);
         }
         
         long nsecs = rosMessage.getPoints().get(waypointIndex).getTimeFromStart().totalNsecs();
         double time = Conversions.nanosecondsToSeconds(nsecs);
         
         for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++)
         {
            jointTrajectoryMessages.get(jointIndex).getTrajectoryPoints().add().set(HumanoidMessageTools.createTrajectoryPoint1DMessage(time, positions[jointIndex], velocities[jointIndex]));
         }
      }
      
      ihmcMessage.setDestination(PacketDestination.CONTROLLER.ordinal());
      packetCommunicator.send(ihmcMessage);
   }
   
   private String getHelp()
   {
      String helpText = "Expected arm joint names for the left side are:\n";
      for (int i = 0; i < leftArmNames.size(); i++)
      {
         helpText += leftArmNames.get(i) + " ";
      }
      helpText += "\nFor a right side trajectory please provide:\n";
      for (int i = 0; i < rightArmNames.size(); i++)
      {
         helpText += rightArmNames.get(i) + " ";
      }
      helpText += "\nIt is required to provide the full list of joint angles in the correct order.";
      
      return helpText;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/FootPoseProvider.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator;

import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly;
import us.ihmc.robotics.robotSide.RobotSide;

/**
 * This interface is part of the API of {@link ContinuousStepGenerator}.
 * <p>
 * It is used to provide a protocol for obtaining the current pose of the robot feet.
 * </p>
 * 
 * @author <NAME>
 */
public interface FootPoseProvider
{
   /**
    * Gets the current pose of a robot sole frame.
    * 
    * @param robotSide the side of interest.
    * @return the current pose of the foot sole.
    */
   FramePose3DReadOnly getCurrentFootPose(RobotSide robotSide);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/stiction/StictionModel.java<|end_filename|>
package us.ihmc.robotics.controllers.stiction;

public interface StictionModel
{
   double getStictionMagnitude();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/GoHomeMessage.java<|end_filename|>
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.
       * The message commands the controller to bring the given part of the body back to a default configuration called 'home'.
       * It is useful to get back to a safe configuration before walking.
       */
public class GoHomeMessage extends Packet<GoHomeMessage> implements Settable<GoHomeMessage>, EpsilonComparable<GoHomeMessage>
{
   public static final byte HUMANOID_BODY_PART_ARM = (byte) 0;
   public static final byte HUMANOID_BODY_PART_CHEST = (byte) 1;
   public static final byte HUMANOID_BODY_PART_PELVIS = (byte) 2;
   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 part of the body the use wants to move back to its home configuration.
            */
   public byte humanoid_body_part_ = (byte) 255;
   /**
            * Needed to identify a side dependent end-effector.
            */
   public byte robot_side_ = (byte) 255;
   /**
            * How long the trajectory will spline from the current desired to the home configuration.
            */
   public double trajectory_time_;
   /**
            * The time to delay this command on the controller side before being executed.
            */
   public double execution_delay_time_;

   public GoHomeMessage()
   {
   }

   public GoHomeMessage(GoHomeMessage other)
   {
      this();
      set(other);
   }

   public void set(GoHomeMessage other)
   {
      sequence_id_ = other.sequence_id_;

      humanoid_body_part_ = other.humanoid_body_part_;

      robot_side_ = other.robot_side_;

      trajectory_time_ = other.trajectory_time_;

      execution_delay_time_ = other.execution_delay_time_;

   }

   /**
            * 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_;
   }

   /**
            * Specifies the part of the body the use wants to move back to its home configuration.
            */
   public void setHumanoidBodyPart(byte humanoid_body_part)
   {
      humanoid_body_part_ = humanoid_body_part;
   }
   /**
            * Specifies the part of the body the use wants to move back to its home configuration.
            */
   public byte getHumanoidBodyPart()
   {
      return humanoid_body_part_;
   }

   /**
            * Needed to identify a side dependent end-effector.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * Needed to identify a side dependent end-effector.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }

   /**
            * How long the trajectory will spline from the current desired to the home configuration.
            */
   public void setTrajectoryTime(double trajectory_time)
   {
      trajectory_time_ = trajectory_time;
   }
   /**
            * How long the trajectory will spline from the current desired to the home configuration.
            */
   public double getTrajectoryTime()
   {
      return trajectory_time_;
   }

   /**
            * The time to delay this command on the controller side before being executed.
            */
   public void setExecutionDelayTime(double execution_delay_time)
   {
      execution_delay_time_ = execution_delay_time;
   }
   /**
            * The time to delay this command on the controller side before being executed.
            */
   public double getExecutionDelayTime()
   {
      return execution_delay_time_;
   }


   public static Supplier<GoHomeMessagePubSubType> getPubSubType()
   {
      return GoHomeMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return GoHomeMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(GoHomeMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.humanoid_body_part_, other.humanoid_body_part_, 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.trajectory_time_, other.trajectory_time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_delay_time_, other.execution_delay_time_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof GoHomeMessage)) return false;

      GoHomeMessage otherMyClass = (GoHomeMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.humanoid_body_part_ != otherMyClass.humanoid_body_part_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if(this.trajectory_time_ != otherMyClass.trajectory_time_) return false;

      if(this.execution_delay_time_ != otherMyClass.execution_delay_time_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("GoHomeMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("humanoid_body_part=");
      builder.append(this.humanoid_body_part_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("trajectory_time=");
      builder.append(this.trajectory_time_);      builder.append(", ");
      builder.append("execution_delay_time=");
      builder.append(this.execution_delay_time_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/globalParameters/GlobalParameterChangedListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.globalParameters;

public interface GlobalParameterChangedListener
{
   public abstract void booleanValueChanged(GlobalParameter globalParameter, String comment, boolean previousValue, boolean newValue);

   public abstract void doubleValueChanged(GlobalParameter globalParameter, String comment, double previousValue, double newValue);

   public abstract void integerValueChanged(GlobalParameter globalParameter, String comment, int previousValue, int newValue);

   public abstract void enumValueChanged(GlobalParameter globalParameter, String comment, Enum<?> previousValue, Enum<?> newValue);

   public abstract void globalParameterCreated(GlobalParameter globalParameter);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/extra/StateMachinesJPanel.java<|end_filename|>
package us.ihmc.robotics.stateMachine.extra;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Font;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Line2D;
import java.awt.geom.Rectangle2D;
import java.util.EnumMap;
import java.util.EnumSet;
import java.util.Map;
import java.util.Map.Entry;
import java.util.Set;

import javax.swing.JPanel;

import org.jgraph.JGraph;
import org.jgraph.graph.DefaultCellViewFactory;
import org.jgraph.graph.DefaultEdge;
import org.jgraph.graph.DefaultGraphCell;
import org.jgraph.graph.DefaultGraphModel;
import org.jgraph.graph.GraphConstants;
import org.jgraph.graph.GraphLayoutCache;

import com.jgraph.layout.JGraphFacade;
import com.jgraph.layout.tree.JGraphTreeLayout;

import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.robotics.stateMachine.core.State;
import us.ihmc.robotics.stateMachine.core.StateChangedListener;
import us.ihmc.robotics.stateMachine.core.StateMachine;
import us.ihmc.robotics.stateMachine.core.StateTransition;

/**
 * This is an old piece code that has been upgraded to work with the new {@link StateMachine}.
 * <p>
 * It will some major clean up.
 * </p>
 * <p>
 * {@link StateMachinesJPanel} can be use to create a new {@code JPanel} displaying a graphical
 * representation of a state machine.
 * </p>
 *
 * @author Sylvain
 *
 * @param <K> Type of {@link Enum} that lists the potential states and that is used by the state
 *           machine this listener is to be added to.
 */
public class StateMachinesJPanel<K extends Enum<K>> extends JPanel implements StateChangedListener<K>
{
   private static final long serialVersionUID = 2453853798153829891L;
   private final StateMachine<K, ? extends State> stateMachine;
   private boolean oldStateDiagram;
   private final Map<K, DefaultGraphCell> stateCells;
   private final Set<K> stateKeys;
   private final Map<K, State> states;
   private final Map<K, StateTransition<K>> stateTransitions;

   private final Class<K> keyType;
   private final Map<K, Set<K>> arrows;

   private JGraph graph;
   private final int numberOfStates;
   private K currentStateKey = null;

   /**
    * Creates a new graphic representation of the given state machine.
    *
    * @param stateMachine the state machine to be graphically displayed.
    */
   public StateMachinesJPanel(StateMachine<K, ? extends State> stateMachine)
   {
      this(stateMachine, false);
   }

   /**
    * Creates a new graphic representation of the given state machine.
    *
    * @param stateMachine the state machine to be graphically displayed.
    * @param oldStateDiagram when {@code true} each state is displayed as a circle and the states are
    *           organized on circular pattern, {@code false} each state is displayed as a box and the
    *           states are organized with a linear pattern.
    */
   public StateMachinesJPanel(StateMachine<K, ? extends State> stateMachine, boolean oldStateDiagram)
   {
      this.stateMachine = stateMachine;
      currentStateKey = stateMachine.getCurrentStateKey();
      keyType = stateMachine.getStateKeyType();
      states = extractActiveStates(stateMachine);
      stateTransitions = extractActiveStateTransitions(stateMachine);
      stateKeys = states.keySet();
      this.numberOfStates = states.size();
      this.oldStateDiagram = oldStateDiagram;
      stateCells = new EnumMap<>(keyType);
      arrows = new EnumMap<>(keyType);

      if (!oldStateDiagram)
      {
         graph = new JGraph();
         initializeJPanel();
      }

      Thread repaintWhenStateChange = new Thread(new Runnable()
      {
         @Override
         public void run()
         {
            while (true)
            {
               // System.out.println("painting");
               try
               {
                  Thread.sleep(100);
               }
               catch (InterruptedException e)
               {
                  e.printStackTrace();
               }

               if (currentStateKey != stateMachine.getCurrentStateKey())
               {
                  currentStateKey = stateMachine.getCurrentStateKey();
                  updateStateMachine();
               }
            }
         }
      }, "IHMC-StateMachinesPanelUpdater");
      repaintWhenStateChange.start();

   }

   private static <K extends Enum<K>> Map<K, StateTransition<K>> extractActiveStateTransitions(StateMachine<K, ? extends State> stateMachine)
   {
      Class<K> keyType = stateMachine.getStateKeyType();
      K[] allKeys = keyType.getEnumConstants();
      Map<K, StateTransition<K>> states = new EnumMap<>(keyType);

      for (K key : allKeys)
      {
         StateTransition<K> state = stateMachine.getStateTransition(key);
         if (state != null)
            states.put(key, state);
      }
      return states;
   }

   private static <K extends Enum<K>> Map<K, State> extractActiveStates(StateMachine<K, ? extends State> stateMachine)
   {
      Class<K> keyType = stateMachine.getStateKeyType();
      K[] allKeys = keyType.getEnumConstants();
      Map<K, State> states = new EnumMap<>(keyType);

      for (K key : allKeys)
      {
         State state = stateMachine.getState(key);
         if (state != null)
            states.put(key, state);
      }
      return states;
   }

   private void updateStateMachine()
   {
      if (oldStateDiagram)
      {
         this.repaint();
      }
      else
      {
         K currentStateKey = stateMachine.getCurrentStateKey();
         DefaultGraphCell currentStateCell = stateCells.get(currentStateKey);
         GraphConstants.setBackground(graph.getAttributes(currentStateCell), Color.RED);
         currentStateCell.setAttributes(graph.getAttributes(currentStateCell));

         for (Entry<K, DefaultGraphCell> entry : stateCells.entrySet())
         {
            if (entry.getKey() != currentStateKey)
            {
               DefaultGraphCell stateCell = entry.getValue();
               GraphConstants.setBackground(graph.getAttributes(stateCell), Color.CYAN);
               stateCell.setAttributes(graph.getAttributes(stateCell));
            }
         }

         graph.repaint();
         graph.refresh();

      }
   }

   @Override
   public void stateChanged(K oldState, K newState)
   {
      if (oldStateDiagram)
      {
         this.repaint();
      }
      else
      {
         if (oldState != null)
         { // At initialization, the state key is null.
            DefaultGraphCell oldStateCell = stateCells.get(oldState);
            GraphConstants.setBackground(graph.getAttributes(oldStateCell), Color.CYAN);
            oldStateCell.setAttributes(graph.getAttributes(oldStateCell));
         }

         DefaultGraphCell newStateCell = stateCells.get(newState);
         GraphConstants.setBackground(graph.getAttributes(newStateCell), Color.RED);
         newStateCell.setAttributes(graph.getAttributes(newStateCell));

         graph.repaint();
         this.repaint();
         graph.refresh();
      }
   }

   private void createArrow(K source, K target)
   {
      if (!arrowAlreadyExist(source, target))
      {

         DefaultGraphCell[] arrowCell = new DefaultGraphCell[1];
         DefaultEdge edge = new DefaultEdge();
         edge.setSource(stateCells.get(source).getChildAt(0));
         edge.setTarget(stateCells.get(target).getChildAt(0));
         arrowCell[0] = edge;
         int arrow = GraphConstants.ARROW_TECHNICAL;
         GraphConstants.setLineEnd(edge.getAttributes(), arrow);
         GraphConstants.setEndFill(edge.getAttributes(), true);
         graph.getGraphLayoutCache().insert(arrowCell);

         Set<K> targetSet = arrows.get(source);
         if (targetSet == null)
         {
            targetSet = EnumSet.noneOf(keyType);
            arrows.put(source, targetSet);
         }
         targetSet.add(target);
      }
   }

   private boolean arrowAlreadyExist(K source, K target)
   {
      return arrows.containsKey(source) && arrows.get(source).contains(target);
   }

   private DefaultGraphCell createCell(K stateKey, Point2D placement)
   {
      //      System.out.println(name + " " + placement + " " + index);
      String cellName = stateKey.toString();
      DefaultGraphCell stateCell = new DefaultGraphCell(new String(cellName));
      Font font = new Font("Arial", Font.PLAIN, 12);
      GraphConstants.setFont(stateCell.getAttributes(), font);
      GraphConstants.setAutoSize(stateCell.getAttributes(), true);
      GraphConstants.setOpaque(stateCell.getAttributes(), true);

      Color color = colorStateCell(stateKey);
      GraphConstants.setBackground(stateCell.getAttributes(), color);
      GraphConstants.setBounds(stateCell.getAttributes(), new Rectangle2D.Double(placement.getX(), placement.getY(), 0, 0));
      stateCell.addPort();

      return stateCell;
   }

   private void createStateTransitionDiagram(K sourceStateKey)
   {
      StateTransition<K> parentStateTransition = stateTransitions.get(sourceStateKey);

      for (K endStateKey : parentStateTransition)
      {
         createArrow(sourceStateKey, endStateKey);
      }
   }

   private Color colorStateCell(K stateKey)
   {
      if (stateKey == stateMachine.getCurrentStateKey())
      {
         return Color.RED;
      }
      else
      {
         return Color.CYAN;
      }
   }

   private void initializeJPanel()
   {
      initiate = true;
      graph.setGraphLayoutCache(new GraphLayoutCache(new DefaultGraphModel(), new DefaultCellViewFactory(), true));
      graph.setEditable(false);

      for (K key : stateKeys)
         stateCells.put(key, createCell(key, new Point2D(0, 0)));

      for (K key : stateKeys)
         createStateTransitionDiagram(key);

      graph.getGraphLayoutCache().insert(stateCells.values().toArray());

      JGraphFacade facade = new JGraphFacade(graph.getGraphLayoutCache());
      facade.setEdgePromotion(false);
      JGraphTreeLayout layout = new JGraphTreeLayout();
      layout.run(facade);
      Map<?, ?> map = facade.createNestedMap(true, true);
      graph.getGraphLayoutCache().edit(map);
   }

   private Dimension dimension = new Dimension();
   final static BasicStroke wideStroke = new BasicStroke(3.0f);

   private void oldStateMachineDiagram(Graphics graphics, double width, double height)
   {
      Graphics2D graphic2D = (Graphics2D) graphics;
      graphic2D.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);

      graphic2D.setStroke(wideStroke);

      // Determine the Ellipse Radii and Circumference:
      double Ry = 0.25 * height;
      double Rx = 0.25 * width;
      double circumference = approximateElipseCircumference(Rx, Ry);

      // Determine the Circle Radius:
      int circleRadiusBasedOnSpacing = (int) (circumference / (2.5 * numberOfStates));
      int circleRadiusBasedOnPanelSize = (int) (0.9 * Math.min(Rx, Ry));
      int circleRadius = Math.min(circleRadiusBasedOnSpacing, circleRadiusBasedOnPanelSize);

      // Draw a circle for each state:
      K currentStateKey = stateMachine.getCurrentStateKey();

      int stateIndex = 0;
      Map<K, Point2D> stateCenters = new EnumMap<>(keyType);

      for (K stateKey : stateKeys)
      {
         Point2D stateCenter = new Point2D();
         if (stateKey == currentStateKey)
            graphics.setColor(Color.RED);
         else
            graphic2D.setColor(Color.BLACK);

         double angle = 2.0 * Math.PI * stateIndex / numberOfStates;
         stateCenter.setX(Rx * Math.cos(angle) + width / 2.0);
         stateCenter.setY(Ry * Math.sin(angle) + height / 2.0);

         graphic2D.draw(new Ellipse2D.Double(stateCenter.getX() - circleRadius, stateCenter.getY() - circleRadius, circleRadius * 2, circleRadius * 2));
         String stateString = stateKey.toString();

         graphic2D.drawString(stateString, (int) stateCenter.getX() - 8 * stateString.length() / 2, (int) stateCenter.getY());
         stateIndex++;

         stateCenters.put(stateKey, stateCenter);
      }

      graphic2D.setColor(Color.BLACK);

      // Draw a line for each transition condition:
      for (K startKey : stateKeys)
      {
         StateTransition<K> stateTransition = stateTransitions.get(startKey);

         for (K endKey : stateTransition)
         {
            stateMachine.getState(endKey); // Just to make sure there is a state.

            Vector2D direction = new Vector2D();
            Point2D startCenter = stateCenters.get(startKey);
            Point2D endCenter = stateCenters.get(endKey);
            direction.sub(endCenter, startCenter);
            direction.normalize();
            direction.scale(circleRadius);

            double x1 = startCenter.getX() + direction.getX();
            double y1 = startCenter.getY() + direction.getY();
            double x2 = endCenter.getX() - direction.getX();
            double y2 = endCenter.getY() - direction.getY();
            graphic2D.draw(new Line2D.Double(x1, y1, x2, y2));
         }
      }
   }

   private boolean initiate;
   private double tempHeight = 0;
   private double tempWidth = 0;

   private void reScalingStateMachine(double width, double height)
   {
      if (initiate)
      {
         tempWidth = width;
         tempHeight = height;
         initiate = false;
      }

      if (tempWidth > width)
      {
         graph.setScale(graph.getScale() + (width - tempWidth) / 3000);
         tempWidth = width;
      }

      if (tempWidth < width)
      {
         graph.setScale(graph.getScale() + (width - tempWidth) / 3000);
         tempWidth = width;
      }

      if (tempHeight > height)
      {
         graph.setScale(graph.getScale() - (tempHeight - height) / 3000);
         tempHeight = height;
      }

      if (tempHeight < height)
      {
         graph.setScale(graph.getScale() + (height - tempHeight) / 3000);
         tempHeight = height;
      }
   }

   @Override
   public void paintComponent(Graphics g)
   {
      super.paintComponent(g);
      this.getSize(dimension);
      double width = dimension.getWidth();
      double height = dimension.getHeight();

      if (oldStateDiagram)
      {
         oldStateMachineDiagram(g, width, height);
      }
      else
      {
         graph.setBounds(0, 0, (int) width, (int) height);
         graph.setAlignmentX(CENTER_ALIGNMENT);
         reScalingStateMachine(width, height);
         this.add(graph);
      }
   }

   private static double approximateElipseCircumference(double a, double b)
   {
      double x = (a - b) / (a + b);

      return Math.PI * (a + b) * (1.0 + 3.0 * x * x / (10.0 + Math.sqrt(4.0 - 3.0 * x * x)));
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/converters/REAPlanarRegionsConverter.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.converters;

import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Point3D32;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.Vector3D32;
import us.ihmc.jOctoMap.node.NormalOcTreeNode;
import us.ihmc.robotEnvironmentAwareness.communication.packets.LineSegment3DMessage;
import us.ihmc.robotEnvironmentAwareness.communication.packets.OcTreeKeyMessage;
import us.ihmc.robotEnvironmentAwareness.communication.packets.PlanarRegionSegmentationMessage;
import us.ihmc.robotEnvironmentAwareness.planarRegion.PlanarRegionSegmentationNodeData;
import us.ihmc.robotEnvironmentAwareness.updaters.RegionFeaturesProvider;

public class REAPlanarRegionsConverter
{
   public static PlanarRegionSegmentationMessage[] createPlanarRegionSegmentationMessages(RegionFeaturesProvider regionFeaturesProvider)
   {
      return createPlanarRegionSegmentationMessages(regionFeaturesProvider.getSegmentationNodeData());
   }

   public static PlanarRegionSegmentationMessage[] createPlanarRegionSegmentationMessages(List<PlanarRegionSegmentationNodeData> regionsNodeData)
   {
      PlanarRegionSegmentationMessage[] messages = new PlanarRegionSegmentationMessage[regionsNodeData.size()];

      for (int regionIndex = 0; regionIndex < regionsNodeData.size(); regionIndex++)
      {
         PlanarRegionSegmentationNodeData nodeData = regionsNodeData.get(regionIndex);
         messages[regionIndex] = createPlanarRegionSegmentationMessage(nodeData);
      }
      return messages;
   }

   public static PlanarRegionSegmentationMessage createPlanarRegionSegmentationMessage(PlanarRegionSegmentationNodeData nodeData)
   {
      int regionId = nodeData.getId();
      Point3D32 origin = new Point3D32(nodeData.getOrigin());
      Vector3D32 normal = new Vector3D32(nodeData.getNormal());
      OcTreeKeyMessage[] nodeKeys = new OcTreeKeyMessage[nodeData.getNumberOfNodes()];
      Point3D32[] nodeHitLocations = new Point3D32[nodeData.getNumberOfNodes()];

      for (int nodeIndex = 0; nodeIndex < nodeData.getNumberOfNodes(); nodeIndex++)
      {
         NormalOcTreeNode node = nodeData.getNode(nodeIndex);
         OcTreeKeyMessage nodeKey = OcTreeMessageConverter.createOcTreeKeyMessage(node.getKeyCopy());
         nodeKeys[nodeIndex] = nodeKey;
         nodeHitLocations[nodeIndex] = new Point3D32(node.getHitLocationCopy());
      }
      PlanarRegionSegmentationMessage planarRegionNodeKeysMessage = createPlanarRegionSegmentationMessage(regionId, origin, normal, nodeKeys, nodeHitLocations);
      return planarRegionNodeKeysMessage;
   }

   public static LineSegment3DMessage[] createLineSegment3dMessages(RegionFeaturesProvider regionFeaturesProvider)
   {
      LineSegment3DMessage[] messages = new LineSegment3DMessage[regionFeaturesProvider.getNumberOfPlaneIntersections()];

      for (int i = 0; i < regionFeaturesProvider.getNumberOfPlaneIntersections(); i++)
      {
         messages[i] = new LineSegment3DMessage();
         messages[i].start = new Point3D32(regionFeaturesProvider.getIntersection(i).getFirstEndpoint());
         messages[i].end = new Point3D32(regionFeaturesProvider.getIntersection(i).getSecondEndpoint());
      }
      return messages;
   }

   public static PlanarRegionSegmentationMessage createPlanarRegionSegmentationMessage(int id, Point3D origin, Vector3D normal,
                                                                                       OcTreeKeyMessage[] regionNodeKeys, List<Point3D> hitLocations)
   {
      PlanarRegionSegmentationMessage message = new PlanarRegionSegmentationMessage();
      message.id = id;
      message.origin = new Point3D32(origin);
      message.normal = new Vector3D32(normal);
      message.nodeKeys = regionNodeKeys;
      message.hitLocations = hitLocations.stream().map(Point3D32::new).toArray(Point3D32[]::new);
      return message;
   }

   public static PlanarRegionSegmentationMessage createPlanarRegionSegmentationMessage(int id, Point3D32 origin, Vector3D32 normal,
                                                                                       OcTreeKeyMessage[] regionNodeKeys, Point3D32[] hitLocations)
   {
      PlanarRegionSegmentationMessage message = new PlanarRegionSegmentationMessage();
      message.id = id;
      message.origin = origin;
      message.normal = normal;
      message.nodeKeys = regionNodeKeys;
      message.hitLocations = hitLocations;
      return message;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/walking/HumanoidBodyPart.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.walking;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;

public enum HumanoidBodyPart
{
   @RosEnumValueDocumentation(documentation = "Request the chest to go back to a straight up configuration.")
   ARM,
   @RosEnumValueDocumentation(documentation = "Request the arm to go to a preconfigured home configuration that is elbow lightly flexed, forearm pointing forward, and upper pointing downward.")
   CHEST,
   @RosEnumValueDocumentation(documentation = "Request the pelvis to go back to between the feet, zero pitch and roll, and headed in the same direction as the feet.")
   PELVIS;

   public static final HumanoidBodyPart[] values = values();

   public boolean isRobotSideNeeded()
   {
      switch (this)
      {
      case ARM:
         return true;
      case CHEST:
      case PELVIS:
         return false;
      default:
         throw new RuntimeException("Should not get there.");
      }
   }

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static HumanoidBodyPart fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SensorNoiseParameters.java<|end_filename|>
package us.ihmc.sensorProcessing.simulatedSensors;

import us.ihmc.euclid.tuple3D.Vector3D;

public class SensorNoiseParameters
{
   private double jointPositionMeasurementStandardDeviation = 0.0;
   private double jointVelocityMeasurementStandardDeviation = 0.0;

   private double comAccelerationProcessNoiseStandardDeviation = 0.0;
   private double angularAccelerationProcessNoiseStandardDeviation = 0.0;

   private double orientationMeasurementStandardDeviation = 0.0;
   private double orientationMeasurementLatency = 0.0;

   private double angularVelocityMeasurementStandardDeviation = 0.0;
   private double angularVelocityMeasurmentLatency = 0.0;

   private double linearAccelerationMeasurementStandardDeviation = 0.0;

   private double angularVelocityBiasProcessNoiseStandardDeviation = 0.0;
   private double linearAccelerationBiasProcessNoiseStandardDeviation = 0.0;

   private double imuYawDriftAcceleration = 0.0;

   private final Vector3D initialLinearVelocityBias = new Vector3D();
   private final Vector3D initialAngularVelocityBias = new Vector3D();

   public double getJointPositionMeasurementStandardDeviation()
   {
      return this.jointPositionMeasurementStandardDeviation;
   }

   public double getJointVelocityMeasurementStandardDeviation()
   {
      return this.jointVelocityMeasurementStandardDeviation;
   }

   public void setJointPositionMeasurementStandardDeviation(double jointPositionMeasurementStandardDeviation)
   {
      this.jointPositionMeasurementStandardDeviation = jointPositionMeasurementStandardDeviation;
   }

   public void setJointVelocityMeasurementStandardDeviation(double jointVelocityMeasurementStandardDeviation)
   {
      this.jointVelocityMeasurementStandardDeviation = jointVelocityMeasurementStandardDeviation;
   }

   public double getComAccelerationProcessNoiseStandardDeviation()
   {
      return comAccelerationProcessNoiseStandardDeviation;
   }

   public void setComAccelerationProcessNoiseStandardDeviation(double comAccelerationProcessNoiseStandardDeviation)
   {
      this.comAccelerationProcessNoiseStandardDeviation = comAccelerationProcessNoiseStandardDeviation;
   }

   public double getAngularAccelerationProcessNoiseStandardDeviation()
   {
      return angularAccelerationProcessNoiseStandardDeviation;
   }

   public void setAngularAccelerationProcessNoiseStandardDeviation(double angularAccelerationProcessNoiseStandardDeviation)
   {
      this.angularAccelerationProcessNoiseStandardDeviation = angularAccelerationProcessNoiseStandardDeviation;
   }

   public double getOrientationMeasurementStandardDeviation()
   {
      return orientationMeasurementStandardDeviation;
   }

   public double getOrientationMeasurementLatency()
   {
      return orientationMeasurementLatency;
   }

   public double getAngularVelocityBiasProcessNoiseStandardDeviation()
   {
      return angularVelocityBiasProcessNoiseStandardDeviation;
   }

   public double getAngularVelocityMeasurementStandardDeviation()
   {
      return angularVelocityMeasurementStandardDeviation;
   }

   public double getAngularVelocityMeasurementLatency()
   {
      return angularVelocityMeasurmentLatency;
   }

   public void getInitialAngularVelocityBias(Vector3D vectorToPack)
   {
      vectorToPack.set(initialAngularVelocityBias);
   }

   public double getLinearAccelerationMeasurementStandardDeviation()
   {
      return linearAccelerationMeasurementStandardDeviation;
   }

   public double getLinearAccelerationBiasProcessNoiseStandardDeviation()
   {
      return linearAccelerationBiasProcessNoiseStandardDeviation;
   }

   public void getInitialLinearVelocityBias(Vector3D vectorToPack)
   {
      vectorToPack.set(initialLinearVelocityBias);
   }

   public void setOrientationMeasurementStandardDeviation(double orientationMeasurementStandardDeviation)
   {
      this.orientationMeasurementStandardDeviation = orientationMeasurementStandardDeviation;
   }

   public void setOrientationMeasurementLatency(double orientationMeasurementLatency)
   {
      this.orientationMeasurementLatency = orientationMeasurementLatency;
   }

   public void setAngularVelocityMeasurementLatency(double angularVelocityMeasurmentLatency)
   {
      this.angularVelocityMeasurmentLatency = angularVelocityMeasurmentLatency;
   }

   public void setAngularVelocityMeasurementStandardDeviation(double angularVelocityMeasurementStandardDeviation)
   {
      this.angularVelocityMeasurementStandardDeviation = angularVelocityMeasurementStandardDeviation;
   }

   public void setLinearAccelerationMeasurementStandardDeviation(double linearAccelerationMeasurementStandardDeviation)
   {
      this.linearAccelerationMeasurementStandardDeviation = linearAccelerationMeasurementStandardDeviation;
   }

   public void setAngularVelocityBiasProcessNoiseStandardDeviation(double angularVelocityBiasProcessNoiseStandardDeviation)
   {
      this.angularVelocityBiasProcessNoiseStandardDeviation = angularVelocityBiasProcessNoiseStandardDeviation;
   }

   public void setLinearAccelerationBiasProcessNoiseStandardDeviation(double linearAccelerationBiasProcessNoiseStandardDeviation)
   {
      this.linearAccelerationBiasProcessNoiseStandardDeviation = linearAccelerationBiasProcessNoiseStandardDeviation;
   }

   public void setInitialLinearVelocityBias(Vector3D initialLinearVelocityBias)
   {
      this.initialLinearVelocityBias.set(initialLinearVelocityBias);
   }

   public void setInitialAngularVelocityBias(Vector3D initialAngularVelocityBias)
   {
      this.initialAngularVelocityBias.set(initialAngularVelocityBias);
   }

   public void setIMUYawDriftAcceleration(double yawDriftAcceleration)
   {
      this.imuYawDriftAcceleration = yawDriftAcceleration;
   }

   public double getIMUYawDriftAcceleration()
   {
      return this.imuYawDriftAcceleration;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SimpleCoactiveBehaviorDataPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "SimpleCoactiveBehaviorDataPacket" defined in "SimpleCoactiveBehaviorDataPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from SimpleCoactiveBehaviorDataPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit SimpleCoactiveBehaviorDataPacket_.idl instead.
*
*/
public class SimpleCoactiveBehaviorDataPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::SimpleCoactiveBehaviorDataPacket_";

   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.SimpleCoactiveBehaviorDataPacket 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.SimpleCoactiveBehaviorDataPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1;
      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getKey().length() + 1;

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getKey().length() <= 255)
      cdr.write_type_d(data.getKey());else
          throw new RuntimeException("key field exceeds the maximum length");

      cdr.write_type_6(data.getValue());

   }

   public static void read(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_d(data.getKey());	
      data.setValue(cdr.read_type_6());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_d("key", data.getKey());
      ser.write_type_6("value", data.getValue());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_d("key", data.getKey());
      data.setValue(ser.read_type_6("value"));
   }

   public static void staticCopy(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket src, controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket createData()
   {
      return new controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket();
   }
   @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.SimpleCoactiveBehaviorDataPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket src, controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public SimpleCoactiveBehaviorDataPacketPubSubType newInstance()
   {
      return new SimpleCoactiveBehaviorDataPacketPubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/QuickHull3dWrapper.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.util.ArrayList;
import java.util.List;

import com.github.quickhull3d.QuickHull3D;

import us.ihmc.euclid.tuple3D.Point3D;

/**
 * Created by agrabertilton on 1/19/15.
 */
public class QuickHull3dWrapper
{
   QuickHull3D quickHull3D;
   List<Point3D> vertices = new ArrayList<Point3D>();
   List<HullFace> faces = new ArrayList<HullFace>();


   public QuickHull3dWrapper()
   {
      quickHull3D = new QuickHull3D();
   }

   public QuickHull3dWrapper(List<Point3D> point3dList)
   {
      quickHull3D = new QuickHull3D();
      build(point3dList);
   }

   public void build(List<Point3D> point3dList)
   {
      quickHull3D.build(getPointArrayFromPointList(point3dList));
      computeWrapperVerticesAndFaces();
   }

   public double getNumVertices() {return vertices.size();}

   public double getNumFaces() {return faces.size();}

   public List<Point3D> getVertices() {return vertices;}

   public List<HullFace> getFaces() {return faces;}

   private void computeWrapperVerticesAndFaces()
   {
      vertices = getPointListFromPointArray(quickHull3D.getVertices());
      faces.clear();
      int[][] faceArrays = quickHull3D.getFaces(QuickHull3D.CLOCKWISE);
      for (int i = 0; i < faceArrays.length; i++)
      {
         faces.add(new HullFace(vertices, faceArrays[i]));
      }
   }

   private com.github.quickhull3d.Point3d[] getPointArrayFromPointList(List<Point3D> point3dList)
   {
      com.github.quickhull3d.Point3d[] pointArray = new com.github.quickhull3d.Point3d[point3dList.size()];
      int index = 0;
      for (Point3D point3d : point3dList)
      {
         pointArray[index] = createPoint3d(point3d);
         index++;
      }
      return pointArray;
   }

   private List<Point3D> getPointListFromPointArray(com.github.quickhull3d.Point3d[] pointArray)
   {
      List<Point3D> point3dList = new ArrayList<Point3D>();
      for (com.github.quickhull3d.Point3d point3d : pointArray)
      {
         point3dList.add(createPoint3d(point3d));
      }
      return point3dList;
   }


   private com.github.quickhull3d.Point3d createPoint3d(Point3D point3d)
   {
      return new com.github.quickhull3d.Point3d(point3d.getX(), point3d.getY(), point3d.getZ());
   }

   private Point3D createPoint3d(com.github.quickhull3d.Point3d point3d)
   {
      return new Point3D(point3d.x, point3d.y, point3d.z);
   }

   public double getDistanceTolerance()
   {
      return quickHull3D.getDistanceTolerance();
   }

   public void setDistanceTolerance(double tolerance)
   {
      quickHull3D.setExplicitDistanceTolerance(tolerance);
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/DRCRobotModelFileInvestigator.java<|end_filename|>
package us.ihmc.atlas;

import java.io.File;
import java.io.PrintWriter;

import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.simulationconstructionset.FloatingRootJointRobot;

public class DRCRobotModelFileInvestigator
{
   public void writeModelFile(DRCRobotModel model)
   {
      try
      {
         String filename = "scsRobotModelOutputs/scsRobot_" + model;
         File file = new File(filename);

         PrintWriter printWriter = new PrintWriter(file);

         final FloatingRootJointRobot robot = model.createHumanoidFloatingRootJointRobot(false);

         printWriter.println(robot);
         printWriter.close();
      }
      catch (Exception e)
      {
         System.err.println("Caught exception with model " + model);
         e.printStackTrace();
      }
   }

   public void writeAllModelFiles()
   {
      for (String st : AtlasRobotModelFactory.getAvailableRobotModels())
      {
         writeModelFile(AtlasRobotModelFactory.createDRCRobotModel(st, RobotTarget.SCS, false));
      }
   }

   public static void main(String[] args)
   {
      DRCRobotModelFileInvestigator investigator = new DRCRobotModelFileInvestigator();
      investigator.writeAllModelFiles();
   }

}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/HandDesiredConfigurationTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.HandDesiredConfigurationMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.HandDesiredConfigurationBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HandConfiguration;
import us.ihmc.robotics.robotSide.RobotSide;

public class HandDesiredConfigurationTask extends BehaviorAction
{
   private final HandDesiredConfigurationMessage ihmcMessage;
   private final HandDesiredConfigurationBehavior behavior;

   public HandDesiredConfigurationTask(RobotSide robotSide, HandConfiguration handDesiredConfiguration,
                                       HandDesiredConfigurationBehavior handDesiredConfigurationBehavior)
   {
      super(handDesiredConfigurationBehavior);
      this.ihmcMessage = HumanoidMessageTools.createHandDesiredConfigurationMessage(robotSide, handDesiredConfiguration);
      this.behavior = handDesiredConfigurationBehavior;
   }

   @Override
   protected void setBehaviorInput()
   {
      behavior.setInput(ihmcMessage);
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/exception/PlanarRegionSegmentationException.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.exception;

public class PlanarRegionSegmentationException extends RuntimeException
{
   private static final long serialVersionUID = -5843579452186405801L;

   public PlanarRegionSegmentationException()
   {
   }

   public PlanarRegionSegmentationException(String message)
   {
      super(message);
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/bipedSupportPolygons/RectangularContactableBody.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.bipedSupportPolygons;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.commons.MathTools;

public class RectangularContactableBody extends ListOfPointsContactablePlaneBody
{
   public RectangularContactableBody(RigidBodyBasics rigidBody, ReferenceFrame soleFrame, double forward, double back, double left, double right)
   {
      super(rigidBody, soleFrame, createContactPoints(forward, back, left, right));
      MathTools.checkIntervalContains(forward, back, Double.POSITIVE_INFINITY);
      MathTools.checkIntervalContains(left, right, Double.POSITIVE_INFINITY);
   }

   private static List<Point2D> createContactPoints(double forward, double back, double left, double right)
   {
      ArrayList<Point2D> contactPoints = new ArrayList<Point2D>();
      contactPoints.add(new Point2D(forward, left));
      contactPoints.add(new Point2D(back, left));
      contactPoints.add(new Point2D(back, right));
      contactPoints.add(new Point2D(forward, right));
      return contactPoints;
   }
}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/model/QuadrupedInitialPositionParameters.java<|end_filename|>
package us.ihmc.quadrupedRobotics.model;

import us.ihmc.euclid.tuple3D.Point3D;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
import us.ihmc.robotics.partNames.QuadrupedJointName;

public interface QuadrupedInitialPositionParameters
{
   /**
    * Maps joint names to initial positions.
    *
    * @param joint
    *           the joint name to lookup.
    * @return the initial angle in radians.
    */
   double getInitialJointPosition(QuadrupedJointName joint);

   Point3D getInitialBodyPosition();

   Quaternion getInitialBodyOrientation();

   default void offsetInitialConfiguration(QuadrupedInitialOffsetAndYaw offset)
   {
      getInitialBodyPosition().add(offset.getAdditionalOffset());
      getInitialBodyOrientation().appendYawRotation(offset.getYaw());
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/ParameterSavingTools.java<|end_filename|>
package us.ihmc.parameterTuner;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;

import javax.xml.bind.JAXBContext;
import javax.xml.bind.JAXBException;
import javax.xml.bind.Marshaller;

import javafx.stage.FileChooser;
import us.ihmc.commons.PrintTools;
import us.ihmc.parameterTuner.guiElements.GuiParameter;
import us.ihmc.parameterTuner.guiElements.GuiParameterStatus;
import us.ihmc.parameterTuner.guiElements.GuiRegistry;
import us.ihmc.yoVariables.parameters.xml.Parameters;
import us.ihmc.yoVariables.parameters.xml.Registry;

public class ParameterSavingTools
{
   public static void save(File file, List<Registry> registries) throws IOException
   {
      Parameters parameterRoot = new Parameters();
      parameterRoot.setRegistries(registries);
      try
      {
         JAXBContext jaxbContext = JAXBContext.newInstance(Parameters.class);
         Marshaller jaxbMarshaller = jaxbContext.createMarshaller();
         jaxbMarshaller.setProperty(Marshaller.JAXB_FORMATTED_OUTPUT, true);
         FileOutputStream os = new FileOutputStream(file);
         jaxbMarshaller.marshal(parameterRoot, os);
         os.close();
      }
      catch (JAXBException e)
      {
         throw new IOException(e);
      }
      catch (FileNotFoundException e)
      {
         throw new IOException(e);
      }
   }

   public static FileChooser.ExtensionFilter getExtensionFilter()
   {
      return new FileChooser.ExtensionFilter("XML files (*.xml)", "*.xml");
   }

   public static List<GuiRegistry> merge(List<GuiRegistry> registriesA, List<GuiRegistry> registriesB)
   {
      Map<String, GuiRegistry> registryMapA = new LinkedHashMap<>();
      Map<String, GuiRegistry> registryMapB = new LinkedHashMap<>();
      registriesA.stream().forEach(registryA -> registryMapA.put(registryA.getUniqueName(), registryA));
      registriesB.stream().forEach(registryB -> registryMapB.put(registryB.getUniqueName(), registryB));

      Map<String, GuiRegistry> mergedRegistryMap = new LinkedHashMap<>();
      mergedRegistryMap.putAll(registryMapA);
      mergedRegistryMap.putAll(registryMapB);
      List<GuiRegistry> mergedRegistries = new ArrayList<>();
      mergedRegistryMap.values().forEach(registry -> {
         String key = registry.getUniqueName();
         GuiRegistry mergedRegistry = merge(registryMapA.get(key), registryMapB.get(key));
         mergedRegistries.add(mergedRegistry);
      });

      return mergedRegistries;
   }

   public static GuiRegistry merge(GuiRegistry registryA, GuiRegistry registryB)
   {
      if (registryA == null && registryB != null)
      {
         GuiRegistry merged = new GuiRegistry(registryB.getName(), null);
         add(registryB, merged);
         return merged;
      }
      if (registryA != null && registryB == null)
      {
         GuiRegistry merged = new GuiRegistry(registryA.getName(), null);
         add(registryA, merged);
         return merged;
      }
      if (!registryA.getUniqueName().equals(registryB.getUniqueName()))
      {
         throw new RuntimeException("Can not merge different regsitries.");
      }

      GuiRegistry merged = new GuiRegistry(registryA.getName(), null);
      merge(registryA, registryB, merged);
      return merged;
   }

   private static void merge(GuiRegistry registryA, GuiRegistry registryB, GuiRegistry merged)
   {
      if (registryA == null && registryB != null)
      {
         add(registryB, merged);
         return;
      }
      if (registryA != null && registryB == null)
      {
         add(registryA, merged);
         return;
      }
      if (!registryA.getUniqueName().equals(registryB.getUniqueName()))
      {
         throw new RuntimeException("Can not merge different regsitries.");
      }
      if (!registryA.getUniqueName().equals(merged.getUniqueName()))
      {
         throw new RuntimeException("Can not merge into different registry.");
      }

      Map<String, GuiParameter> parameterMapA = registryA.getParameterMap();
      Map<String, GuiParameter> parameterMapB = registryB.getParameterMap();
      Map<String, GuiParameter> mergedParameters = new LinkedHashMap<>();
      mergedParameters.putAll(parameterMapA);
      mergedParameters.putAll(parameterMapB);
      mergedParameters.values().forEach(parameter -> {
         GuiParameter newParameter = parameter.createCopy(merged);
         merged.addParameter(newParameter);
      });

      Map<String, GuiRegistry> registryMapA = registryA.getRegistryMap();
      Map<String, GuiRegistry> registryMapB = registryB.getRegistryMap();
      Map<String, GuiRegistry> mergedRegistries = new LinkedHashMap<>();
      mergedRegistries.putAll(registryMapA);
      mergedRegistries.putAll(registryMapB);
      mergedRegistries.values().forEach(registry -> {
         GuiRegistry mergedChild = new GuiRegistry(registry.getName(), merged);
         merged.addRegistry(mergedChild);
         String key = registry.getUniqueName();
         merge(registryMapA.get(key), registryMapB.get(key), mergedChild);
      });
   }

   private static void add(GuiRegistry registry, GuiRegistry registryToAddTo)
   {
      registry.getParameters().forEach(parameter -> {
         GuiParameter newParameter = parameter.createCopy(registryToAddTo);
         registryToAddTo.addParameter(newParameter);
      });
      registry.getRegistries().forEach(child -> {
         GuiRegistry newChild = new GuiRegistry(child.getName(), registryToAddTo);
         registryToAddTo.addRegistry(newChild);
         add(child, newChild);
      });
   }

   public static List<GuiRegistry> filterModified(List<GuiRegistry> registries)
   {
      List<GuiRegistry> filteredRegistries = new ArrayList<>();
      registries.forEach(registry -> {
         GuiRegistry filtered = filterModified(registry);
         if (!filtered.getAllParameters().isEmpty())
         {
            filteredRegistries.add(filtered);
         }
      });
      return filteredRegistries;
   }

   public static GuiRegistry filterModified(GuiRegistry registry)
   {
      return registry.createFullCopy(parameter -> parameter.getStatus() == GuiParameterStatus.MODIFIED);
   }

   public static List<GuiRegistry> findRootRegistries(List<GuiRegistry> registries, List<String> rootRegistryNames)
   {
      List<GuiRegistry> rootRegistries = new ArrayList<>();
      registries.forEach(registry -> recursiveAddMatchingRoots(registry, rootRegistryNames, rootRegistries));

      if (rootRegistries.size() != rootRegistryNames.size())
      {
         PrintTools.error("Could not find all root registries. Aborting save.");
         return Collections.emptyList();
      }

      return rootRegistries;
   }

   public static void recursiveAddMatchingRoots(GuiRegistry registry, List<String> rootRegistryNames, List<GuiRegistry> matching)
   {
      if (rootRegistryNames.contains(registry.getUniqueName()))
      {
         matching.add(registry);
      }
      else
      {
         registry.getRegistries().forEach(child -> recursiveAddMatchingRoots(child, rootRegistryNames, matching));
      }
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/bdi/BDIRobotBehavior.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.bdi;

public enum BDIRobotBehavior
{
   NONE,        
   FREEZE,      
   STAND_PREP,  
   STAND,       
   WALK,        
   STEP,        
   MANIPULATE,  
   USER,        
   CALIBRATE,
   SOFT_STOP;
   
   public static final BDIRobotBehavior[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static BDIRobotBehavior fromByte(byte enumAsByte)
   {
      if (enumAsByte == -1)
         return null;
      return values[enumAsByte];
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/manipulation/wholeBodyTrajectory/WholeBodyTrajectoryToolboxMessageTools.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory;

import java.util.Random;

import controller_msgs.msg.dds.WaypointBasedTrajectoryMessage;
import us.ihmc.commons.MathTools;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.screwTheory.SelectionMatrix6D;

public class WholeBodyTrajectoryToolboxMessageTools
{
   public static final Random random = new Random(1);

   public static interface FunctionTrajectory
   {
      public Pose3D compute(double time);
   }

   public static FunctionTrajectory createFunctionTrajectory(WaypointBasedTrajectoryMessage message)
   {
      return new FunctionTrajectory()
      {
         @Override
         public Pose3D compute(double time)
         {
            Pose3D current = new Pose3D();

            Pose3D previous = null;
            Pose3D next = null;
            double t0 = Double.NaN;
            double tf = Double.NaN;

            for (int i = 1; i < message.getWaypoints().size(); i++)
            {
               t0 = message.getWaypointTimes().get(i - 1);
               tf = message.getWaypointTimes().get(i);
               previous = message.getWaypoints().get(i - 1);
               next = message.getWaypoints().get(i);
               if (time < message.getWaypointTimes().get(i))
                  break;
            }

            double alpha = (time - t0) / (tf - t0);
            alpha = MathTools.clamp(alpha, 0.0, 1.0);
            current.interpolate(previous, next, alpha);

            return current;
         }
      };
   }

   public static WaypointBasedTrajectoryMessage createTrajectoryMessage(RigidBodyBasics endEffector, double t0, double tf, FunctionTrajectory trajectoryToDiscretize,
                                                                        SelectionMatrix6D selectionMatrix)
   {
      return createTrajectoryMessage(endEffector, t0, tf, 0.1, trajectoryToDiscretize, selectionMatrix);
   }

   public static WaypointBasedTrajectoryMessage createTrajectoryMessage(RigidBodyBasics endEffector, double t0, double tf, double timeResolution,
                                                                        FunctionTrajectory trajectoryToDiscretize, SelectionMatrix6D selectionMatrix)
   {
      int numberOfWaypoints = (int) Math.round((tf - t0) / timeResolution) + 1;
      timeResolution = (tf - t0) / (numberOfWaypoints - 1);

      double[] waypointTimes = new double[numberOfWaypoints];
      Pose3D[] waypoints = new Pose3D[numberOfWaypoints];

      for (int i = 0; i < numberOfWaypoints; i++)
      {
         double waypointTime = i * timeResolution + t0;

         waypointTimes[i] = waypointTime;
         waypoints[i] = trajectoryToDiscretize.compute(waypointTime);
      }

      return HumanoidMessageTools.createWaypointBasedTrajectoryMessage(endEffector, waypointTimes, waypoints, selectionMatrix);
   }

   public static double[] createDefaultExplorationAmplitudeArray(ConfigurationSpaceName... configurationSpaceNames)
   {
      double[] lowerLimit = new double[configurationSpaceNames.length];
      for (int i = 0; i < configurationSpaceNames.length; i++)
         lowerLimit[i] = configurationSpaceNames[i].getDefaultExplorationAmplitude();
      return lowerLimit;
   }

   public static double[] createDefaultExplorationUpperLimitArray(ConfigurationSpaceName... configurationSpaceNames)
   {
      double[] upperLimit = new double[configurationSpaceNames.length];
      for (int i = 0; i < configurationSpaceNames.length; i++)
         upperLimit[i] = configurationSpaceNames[i].getDefaultExplorationUpperLimit();
      return upperLimit;
   }

   public static double[] createDefaultExplorationLowerLimitArray(ConfigurationSpaceName... configurationSpaceNames)
   {
      double[] lowerLimit = new double[configurationSpaceNames.length];
      for (int i = 0; i < configurationSpaceNames.length; i++)
         lowerLimit[i] = configurationSpaceNames[i].getDefaultExplorationLowerLimit();
      return lowerLimit;
   }
}

<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/parameters/ValkyrieFeedbackControllerSettings.java<|end_filename|>
package us.ihmc.valkyrie.parameters;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.commonWalkingControlModules.configurations.GroupParameter;
import us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController.FeedbackControllerSettings;
import us.ihmc.robotics.partNames.ArmJointName;
import us.ihmc.robotics.robotSide.RobotSide;

public class ValkyrieFeedbackControllerSettings implements FeedbackControllerSettings
{
   private final ValkyrieJointMap jointMap;
   private final RobotTarget target;

   public ValkyrieFeedbackControllerSettings(ValkyrieJointMap jointMap, RobotTarget target)
   {
      this.jointMap = jointMap;
      this.target = target;
   }

   @Override
   public boolean enableIntegralTerm()
   {
      return false; // Saves about 130 YoVariables.
   }

   @Override
   public List<GroupParameter<Double>> getErrorVelocityFilterBreakFrequencies()
   {
      if (target == RobotTarget.SCS)
         return null;

      GroupParameter<Double> pelvisGroup = new GroupParameter<>(jointMap.getPelvisName(), 25.0);
      GroupParameter<Double> chestGroup = new GroupParameter<>(jointMap.getChestName(), 16.0);
      GroupParameter<Double> footGroup = new GroupParameter<>("foot", 16.0, new ArrayList<>());
      GroupParameter<Double> armJointsGroup = new GroupParameter<>("armJoints", 25.0, new ArrayList<>());

      for (RobotSide robotSide : RobotSide.values)
      {
         footGroup.getMemberNames().add(jointMap.getFootName(robotSide));

         for (ArmJointName armJointName : jointMap.getArmJointNames())
            armJointsGroup.getMemberNames().add(jointMap.getArmJointName(robotSide, armJointName));
      }

      return Arrays.asList(pelvisGroup, chestGroup, footGroup, armJointsGroup);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/joystick/EnumYoVariableJoystickEventListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.joystick;

import net.java.games.input.Component;
import net.java.games.input.Event;
import us.ihmc.yoVariables.variable.YoEnum;
import us.ihmc.tools.inputDevices.joystick.JoystickEventListener;

public class EnumYoVariableJoystickEventListener<T extends Enum<T>> implements JoystickEventListener
{
   private final YoEnum<T> yoEnum;
   private final Component component;
   private final T enumToSwitchTo;
   
   public EnumYoVariableJoystickEventListener(YoEnum<T> yoEnum, Component component, T enumToSwitchTo)
   {
      this.yoEnum = yoEnum;
      this.component = component;
      this.enumToSwitchTo = enumToSwitchTo;
   }
   
   @Override
   public void processEvent(Event event)
   {
      if (event.getComponent() == component)
      {
         yoEnum.set(enumToSwitchTo);
      }
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilterType.java<|end_filename|>
package us.ihmc.robotics.math.filters;

public enum SecondOrderFilterType
{
   LOW_PASS, NOTCH, HIGH_PASS
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/stateTransitionConditions/StopWalkingFromTransferCondition.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.stateTransitionConditions;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.states.TransferState;
import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.stateMachine.core.StateTransitionCondition;

public class StopWalkingFromTransferCondition implements StateTransitionCondition
{
   private final TransferState transferState;
   private final WalkingMessageHandler walkingMessageHandler;

   public StopWalkingFromTransferCondition(TransferState transferState, WalkingMessageHandler walkingMessageHandler)
   {
      this.transferState = transferState;
      this.walkingMessageHandler = walkingMessageHandler;
   }

   @Override
   public boolean testCondition(double timeInState)
   {
      RobotSide trailingFootSide = transferState.getTransferToSide().getOppositeSide();
      boolean noFootstep = !walkingMessageHandler.isNextFootstepFor(trailingFootSide);

      return noFootstep;
   }
}

<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/converter/CommandConversionTools.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.controllerAPI.converter;

import us.ihmc.humanoidRobotics.communication.controllerAPI.command.EuclideanTrajectoryControllerCommand;
import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand;
import us.ihmc.humanoidRobotics.communication.controllerAPI.command.SO3TrajectoryControllerCommand;

public class CommandConversionTools
{
   public static void convertToSE3(SO3TrajectoryControllerCommand command, SE3TrajectoryControllerCommand commandToPack)
   {
      commandToPack.setQueueableCommandVariables(command);
      commandToPack.setTrajectoryFrame(command.getTrajectoryFrame());
      commandToPack.getSelectionMatrix().clearLinearSelection();
      commandToPack.getSelectionMatrix().setAngularPart(command.getSelectionMatrix());
      commandToPack.getWeightMatrix().clearLinearWeights();
      commandToPack.getWeightMatrix().setAngularPart(command.getWeightMatrix());
      commandToPack.setUseCustomControlFrame(command.useCustomControlFrame());
      commandToPack.setControlFramePose(command.getControlFramePose());
      commandToPack.getTrajectoryPointList().setToOrientationTrajectoryIncludingFrame(command.getTrajectoryPointList());
   }

   public static void convertToSE3(EuclideanTrajectoryControllerCommand command, SE3TrajectoryControllerCommand commandToPack)
   {
      commandToPack.setQueueableCommandVariables(command);
      commandToPack.setTrajectoryFrame(command.getTrajectoryFrame());
      commandToPack.getSelectionMatrix().clearAngularSelection();
      commandToPack.getSelectionMatrix().setLinearPart(command.getSelectionMatrix());
      commandToPack.getWeightMatrix().clearAngularWeights();
      commandToPack.getWeightMatrix().setLinearPart(command.getWeightMatrix());
      commandToPack.setUseCustomControlFrame(command.useCustomControlFrame());
      commandToPack.setControlFramePose(command.getControlFramePose());
      commandToPack.getTrajectoryPointList().setToPositionTrajectoryIncludingFrame(command.getTrajectoryPointList());
   }

   public static void convertToSO3(SE3TrajectoryControllerCommand command, SO3TrajectoryControllerCommand commandToPack)
   {
      commandToPack.setQueueableCommandVariables(command);
      commandToPack.setTrajectoryFrame(command.getTrajectoryFrame());
      commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getAngularPart());
      commandToPack.getWeightMatrix().set(command.getWeightMatrix().getAngularPart());
      commandToPack.setUseCustomControlFrame(command.useCustomControlFrame());
      commandToPack.setControlFramePose(command.getControlFramePose());
      commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList());
   }

   public static void convertToEuclidean(SE3TrajectoryControllerCommand command, EuclideanTrajectoryControllerCommand commandToPack)
   {
      commandToPack.setQueueableCommandVariables(command);
      commandToPack.setTrajectoryFrame(command.getTrajectoryFrame());
      commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getLinearPart());
      commandToPack.getWeightMatrix().set(command.getWeightMatrix().getLinearPart());
      commandToPack.setUseCustomControlFrame(command.useCustomControlFrame());
      commandToPack.setControlFramePose(command.getControlFramePose());
      commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList());
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/manipulation/wholeBodyTrajectory/ConfigurationSpaceName.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory;

import gnu.trove.list.TByteList;

public enum ConfigurationSpaceName
{
   X, Y, Z, ROLL, PITCH, YAW, SO3;

   public static final ConfigurationSpaceName[] values = values();

   public double getDefaultExplorationUpperLimit()
   {
      switch (this)
      {
      case X:
      case Y:
      case Z:
         return 0.1;
      case ROLL:
      case PITCH:
      case YAW:
         return 0.3 * Math.PI;
      case SO3:
         return 1.0;
      default:
         throw new RuntimeException("Unexpected value: " + this);
      }
   }

   public double getDefaultExplorationLowerLimit()
   {
      switch (this)
      {
      case X:
      case Y:
      case Z:
         return -0.1;
      case ROLL:
      case PITCH:
      case YAW:
         return -0.3 * Math.PI;
      case SO3:
         return 0.0;
      default:
         throw new RuntimeException("Unexpected value: " + this);
      }
   }

   public double getDefaultExplorationAmplitude()
   {
      switch (this)
      {
      case X:
      case Y:
      case Z:
         return 0.15;
      case ROLL:
      case PITCH:
      case YAW:
         return 0.3 * Math.PI;
      case SO3:
         return 1.0;
      default:
         throw new RuntimeException("Unexpected value: " + this);
      }
   }

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static ConfigurationSpaceName fromByte(byte enumAsByte)
   {
      if (enumAsByte == -1)
         return null;
      return values[enumAsByte];
   }

   public static byte[] toBytes(ConfigurationSpaceName[] enumArray)
   {
      if (enumArray == null)
         return null;
      byte[] byteArray = new byte[enumArray.length];
      for (int i = 0; i < enumArray.length; i++)
         byteArray[i] = enumArray[i].toByte();
      return byteArray;
   }

   public static ConfigurationSpaceName[] fromBytes(TByteList enumListAsBytes)
   {
      if (enumListAsBytes == null)
         return null;
      ConfigurationSpaceName[] enumArray = new ConfigurationSpaceName[enumListAsBytes.size()];
      for (int i = 0; i < enumListAsBytes.size(); i++)
         enumArray[i] = fromByte(enumListAsBytes.get(i));
      return enumArray;
   }

   public static ConfigurationSpaceName[] fromBytes(byte[] enumArrayAsBytes)
   {
      if (enumArrayAsBytes == null)
         return null;
      ConfigurationSpaceName[] enumArray = new ConfigurationSpaceName[enumArrayAsBytes.length];
      for (int i = 0; i < enumArrayAsBytes.length; i++)
         enumArray[i] = fromByte(enumArrayAsBytes[i]);
      return enumArray;
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ClearDelayQueueMessage.java<|end_filename|>
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 clear the delay buffers on the controller, if you sent a message with a
       * delay and now you do not want them executed, use this command.
       */
public class ClearDelayQueueMessage extends Packet<ClearDelayQueueMessage> implements Settable<ClearDelayQueueMessage>, EpsilonComparable<ClearDelayQueueMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * If only a specific message type is to be cleared, use this field.
            * The integer represents the hash code of the message simple name.
            * The hash code of a string should be computed as in java.lang.String.hashCode().
            */
   public int class_simple_name_based_hash_code_;
   /**
            * If all the messages should be cleared, then this field should be set to true.
            */
   public boolean clear_all_delay_buffers_;

   public ClearDelayQueueMessage()
   {
   }

   public ClearDelayQueueMessage(ClearDelayQueueMessage other)
   {
      this();
      set(other);
   }

   public void set(ClearDelayQueueMessage other)
   {
      sequence_id_ = other.sequence_id_;

      class_simple_name_based_hash_code_ = other.class_simple_name_based_hash_code_;

      clear_all_delay_buffers_ = other.clear_all_delay_buffers_;

   }

   /**
            * 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_;
   }

   /**
            * If only a specific message type is to be cleared, use this field.
            * The integer represents the hash code of the message simple name.
            * The hash code of a string should be computed as in java.lang.String.hashCode().
            */
   public void setClassSimpleNameBasedHashCode(int class_simple_name_based_hash_code)
   {
      class_simple_name_based_hash_code_ = class_simple_name_based_hash_code;
   }
   /**
            * If only a specific message type is to be cleared, use this field.
            * The integer represents the hash code of the message simple name.
            * The hash code of a string should be computed as in java.lang.String.hashCode().
            */
   public int getClassSimpleNameBasedHashCode()
   {
      return class_simple_name_based_hash_code_;
   }

   /**
            * If all the messages should be cleared, then this field should be set to true.
            */
   public void setClearAllDelayBuffers(boolean clear_all_delay_buffers)
   {
      clear_all_delay_buffers_ = clear_all_delay_buffers;
   }
   /**
            * If all the messages should be cleared, then this field should be set to true.
            */
   public boolean getClearAllDelayBuffers()
   {
      return clear_all_delay_buffers_;
   }


   public static Supplier<ClearDelayQueueMessagePubSubType> getPubSubType()
   {
      return ClearDelayQueueMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ClearDelayQueueMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ClearDelayQueueMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.class_simple_name_based_hash_code_, other.class_simple_name_based_hash_code_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.clear_all_delay_buffers_, other.clear_all_delay_buffers_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ClearDelayQueueMessage)) return false;

      ClearDelayQueueMessage otherMyClass = (ClearDelayQueueMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.class_simple_name_based_hash_code_ != otherMyClass.class_simple_name_based_hash_code_) return false;

      if(this.clear_all_delay_buffers_ != otherMyClass.clear_all_delay_buffers_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ClearDelayQueueMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("class_simple_name_based_hash_code=");
      builder.append(this.class_simple_name_based_hash_code_);      builder.append(", ");
      builder.append("clear_all_delay_buffers=");
      builder.append(this.clear_all_delay_buffers_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/HeightMapBestFitPlaneCalculator.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.geometry.Plane3D;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.dataStructures.HeightMapWithPoints;

public class HeightMapBestFitPlaneCalculator
{
   private List<Point3D> pointList = new ArrayList<Point3D>();

   private final PlaneFitter planeFitter = new LeastSquaresZPlaneFitter();

   public HeightMapBestFitPlaneCalculator()
   {
   }

   public Plane3D calculatePlane(HeightMapWithPoints heightMap, Point2DBasics center, double kernelSizeX, double kernelSizeY) throws InsufficientDataException
   {
      if ((kernelSizeX == 0) || (kernelSizeY == 0))
         throw new RuntimeException("empty search region.");
      pointList = heightMap.getAllPointsWithinArea(center.getX(), center.getY(), kernelSizeX, kernelSizeY);

      Plane3D ret = new Plane3D();
      planeFitter.fitPlaneToPoints(center, pointList, ret);
      return ret;
   }

   public Plane3D calculatePlane(HeightMapWithPoints heightMap, Point2DBasics center, double xExtent, double yExtent, InclusionFunction<Point3D> kernelMask)
           throws InsufficientDataException
   {
      if ((xExtent == 0) || (yExtent == 0))
         throw new RuntimeException("empty search region.");
      pointList = heightMap.getAllPointsWithinArea(center.getX(), center.getY(), xExtent, yExtent, kernelMask);

      Plane3D ret = new Plane3D();
      planeFitter.fitPlaneToPoints(center, pointList, ret);
      return ret;
   }



   public List<Point3D> getPointList()
   {
      return this.pointList;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/extra/EventTrigger.java<|end_filename|>
package us.ihmc.robotics.stateMachine.extra;

import us.ihmc.robotics.stateMachine.factories.EventBasedStateMachineFactory;

/**
 * Trigger used to fire events which in turn will potentially trigger state transitions for a state
 * machine.
 * <p>
 * Use the {@link EventBasedStateMachineFactory} to configure and create an event-based state
 * machine, and to to create {@link EventTrigger}s.
 * </p>
 * 
 * @author Sylvain
 *
 */
public interface EventTrigger
{
   /**
    * Call this method to fire events at a state machine to trigger its state transitions.
    * 
    * @param event the event to be fired.
    */
   <E extends Enum<E>> void fireEvent(E event);
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/cart/CartRobotEnvironmentSimulation.java<|end_filename|>
package us.ihmc.exampleSimulations.cart;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.simulationConstructionSetTools.util.environments.CartRobotRacingEnvironment;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.RobotFromDescription;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.SimulationConstructionSetParameters;
import us.ihmc.simulationconstructionset.physics.CollisionHandler;
import us.ihmc.simulationconstructionset.physics.collision.DefaultCollisionHandler;
import us.ihmc.simulationconstructionset.physics.collision.simple.CollisionManager;

public class CartRobotEnvironmentSimulation
{
   public CartRobotEnvironmentSimulation()
   {
      double dt = 0.001;

      Vector3D startingPoint = new Vector3D(0.0, 0.0, 0.3);

      List<Robot> allSimulatedRobotList = new ArrayList<Robot>();

      CartRobotDescription robotDescription = new CartRobotDescription("rollingRobot");
      CartRobotCollisionMeshDefinitionDataHolder collisionMeshData = new CartRobotCollisionMeshDefinitionDataHolder();
      robotDescription.addCollisionMeshDefinitionData(collisionMeshData);

      SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters();
      parameters.setCreateGUI(true);
      parameters.setDataBufferSize(8000);

      // create robot.
      Robot cartRobot = new RobotFromDescription(robotDescription);
      FloatingJoint floatingJoint = (FloatingJoint) cartRobot.getRootJoints().get(0);
      floatingJoint.setPosition(startingPoint);
      allSimulatedRobotList.add(cartRobot);

      // robot controller.
      CartRobotController controller = new CartRobotController(cartRobot);
      cartRobot.setController(controller);

      // Stair Env Robot
      CartRobotRacingEnvironment environment = new CartRobotRacingEnvironment(false);

      // Scs
      SimulationConstructionSet scs = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), parameters);
      Graphics3DObject staticLinkGraphics = new Graphics3DObject();
      staticLinkGraphics.addCoordinateSystem(0.1);
      scs.addStaticLinkGraphics(staticLinkGraphics);
      scs.addStaticLinkGraphics(environment.getTerrainObject3D().getLinkGraphics());

      // simulate.
      double coefficientOfRestitution = 0.2;
      double coefficientOfFriction = 0.7;
      CollisionHandler collisionHandler = new DefaultCollisionHandler(coefficientOfRestitution, coefficientOfFriction);
      CollisionManager collisionManager = new CollisionManager(environment.getTerrainObject3D(), collisionHandler);
      scs.initializeShapeCollision(collisionManager);

      scs.setDT(dt, 1);
      scs.setFastSimulate(true);
      scs.setGroundVisible(false);

      scs.setCameraFix(0.0, 0.0, 0.8);
      scs.setCameraPosition(0.0, -8.0, 1.4);
      scs.startOnAThread();
   }

   public static void main(String[] args)
   {
      new CartRobotEnvironmentSimulation();
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/contactPoints/FrictionConeAnalysis.java<|end_filename|>
package us.ihmc.wholeBodyController.contactPoints;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import javax.swing.JCheckBox;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.JScrollPane;

import org.apache.commons.math3.stat.descriptive.SummaryStatistics;
import org.apache.commons.math3.util.Precision;

import us.ihmc.commons.PrintTools;
import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.plotting.artifact.CircleArtifact;
import us.ihmc.graphicsDescription.plotting.artifact.LineArtifact;
import us.ihmc.graphicsDescription.plotting.artifact.PolygonArtifact;
import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
import us.ihmc.graphicsDescription.yoGraphics.plotting.ArtifactList;
import us.ihmc.plotting.Plotter;
import us.ihmc.plotting.PlotterShowHideMenu;
import us.ihmc.robotics.geometry.ConvexPolygonScaler;

public class FrictionConeAnalysis
{
   private static final double friction = 0.8;
   private static final int vectors = 4;

   private static final boolean showPlotter = true;
   private static final int pointsOnCircle = 50;
   private static final double copGridResolution = 0.05;
   private static final boolean edgeOnly = false;
   private static final double viewRange = 0.5;

   /**
    * The cone calculator is taking care of rotating the friction cones. Examples are:</br>
    * {@link NoOffsetFrictionConeCalculator}</br>
    * {@link InteriorAngleFrictionConeCalculator}</br>
    * {@link CenteredFrictionConeCalculator}
    */
   private static final FrictionConeRotationCalculator coneCalculator = new InteriorAngleFrictionConeCalculator();

   private final SummaryStatistics statistics = new SummaryStatistics();

   public FrictionConeAnalysis()
   {
      List<Point2D> contactPoints = createFootShape();
      List<Point2D> copSamples = createCopGrid(contactPoints, copGridResolution, edgeOnly);

      FrictionConeContactForceSolver solver = new FrictionConeContactForceSolver(contactPoints, coneCalculator);

      Map<Point2D, List<Vector2D>> resultMap = new HashMap<>();
      Map<Point2D, List<Vector2D>> referenceMap = new HashMap<>();
      double percent = computePercentAcieved(solver, copSamples, resultMap, referenceMap);

      PrintTools.info("Mean: " + Precision.round(100.0 * percent, 2) + "%");
      if (showPlotter)
      {
         visualizeAndSleep(contactPoints, copSamples, referenceMap, resultMap, coneCalculator);
      }
   }

   private double computePercentAcieved(FrictionConeContactForceSolver solver, List<Point2D> copSamples, Map<Point2D, List<Vector2D>> resultMap,
                                        Map<Point2D, List<Vector2D>> referenceMap)
   {
      statistics.clear();

      for (Point2D cop : copSamples)
      {
         double fullArea = Math.PI * friction * friction;
         double achievedArea = 0.0;

         List<Vector2D> reference = new ArrayList<>();
         List<Vector2D> achieved = new ArrayList<>();

         for (int i = 0; i < pointsOnCircle; i++)
         {
            double angle = 2.0 * Math.PI * i / pointsOnCircle;
            double fx = Math.cos(angle) * friction;
            double fy = Math.sin(angle) * friction;

            Vector2D force = new Vector2D(fx, fy);
            Vector2D achievedForce = new Vector2D();
            if (solver.solveForFixedCoPAndDirection(vectors, friction, cop, force, achievedForce))
            {
               double achievedLength = achievedForce.length();
               achievedArea += Math.PI * achievedLength * achievedLength / pointsOnCircle;
               achieved.add(achievedForce);
            }
            reference.add(force);

            if (referenceMap != null)
            {
               referenceMap.put(cop, reference);
            }
            if (resultMap != null)
            {
               resultMap.put(cop, achieved);
            }
         }

         double percent = achievedArea / fullArea;
         statistics.addValue(percent);
      }

      return statistics.getMean();
   }

   public static void visualizeAndSleep(List<Point2D> contactPoints, List<Point2D> copSamples, Map<Point2D, List<Vector2D>> referenceMap,
                                        Map<Point2D, List<Vector2D>> resultMap, FrictionConeRotationCalculator coneCalculator)
   {
      YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry();

      for (int contactIdx = 0; contactIdx < contactPoints.size(); contactIdx++)
      {
         Point2D contactPoint = contactPoints.get(contactIdx);

         for (int vectorIdx = 0; vectorIdx < vectors; vectorIdx++)
         {
            Vector3D vectorToPack = new Vector3D();
            coneCalculator.packVector(contactPoints, contactIdx, vectors, vectorIdx, friction, vectorToPack);

            Vector2D direction = new Vector2D(vectorToPack);
            direction.scale(2.0 * copGridResolution / friction);
            Point2D endPoint = new Point2D(contactPoint);
            endPoint.add(direction);
            LineArtifact contactVector = new LineArtifact("Vector" + vectorIdx + "At" + contactIdx, contactPoint, endPoint);
            contactVector.setColor(Color.BLUE);
            contactVector.setLevel(4);
            graphicsListRegistry.registerArtifact("Contact State", contactVector);
         }
      }

      ConvexPolygon2D supportPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(contactPoints));
      PolygonArtifact polygonViz = new PolygonArtifact("SupportPolygon", true, Color.LIGHT_GRAY, supportPolygon);
      polygonViz.setLevel(1);
      graphicsListRegistry.registerArtifact("Contact State", polygonViz);

      int copCount = 0;
      for (Point2D cop : copSamples)
      {
         CircleArtifact copViz = new CircleArtifact("CoP" + copCount, cop.getX(), cop.getY(), 0.01, true, Color.BLUE);
         copViz.setLevel(4);
         graphicsListRegistry.registerArtifact("CoPs", copViz);

         int vectorCount = 0;
         for (Vector2D desired : referenceMap.get(cop))
         {
            desired.scale(0.5 * copGridResolution / friction);
            Point2D endPoint = new Point2D(cop);
            endPoint.add(desired);
            LineArtifact desiredForce = new LineArtifact("DesiredForce" + vectorCount + "At" + copCount, cop, endPoint);
            desiredForce.setColor(Color.RED);
            desiredForce.setLevel(2);
            graphicsListRegistry.registerArtifact("Forces", desiredForce);
            vectorCount++;
         }

         vectorCount = 0;
         for (Vector2D achieved : resultMap.get(cop))
         {
            achieved.scale(0.5 * copGridResolution / friction);
            Point2D endPoint = new Point2D(cop);
            endPoint.add(achieved);
            LineArtifact achievedForce = new LineArtifact("AchievedForce" + vectorCount + "At" + copCount, cop, endPoint);
            achievedForce.setColor(Color.GREEN);
            achievedForce.setLevel(3);
            graphicsListRegistry.registerArtifact("Forces", achievedForce);
            vectorCount++;
         }

         copCount++;
      }

      showPlotter(graphicsListRegistry, FrictionConeAnalysis.class.getSimpleName());
   }

   private static void showPlotter(YoGraphicsListRegistry yoGraphicsListRegistry, String windowName)
   {
      Plotter plotter = new Plotter();
      plotter.setViewRange(viewRange);

      ArrayList<ArtifactList> artifactLists = new ArrayList<>();
      yoGraphicsListRegistry.getRegisteredArtifactLists(artifactLists);
      for (ArtifactList artifactList : artifactLists)
      {
         artifactList.setVisible(true);
      }

      JFrame frame = new JFrame(windowName);
      Dimension preferredSize = new Dimension(1000, 650);
      frame.setPreferredSize(preferredSize);

      JCheckBox doneBox = new JCheckBox("Done");
      PlotterShowHideMenu plotterShowHideMenu = new PlotterShowHideMenu(plotter);
      plotter.addArtifactsChangedListener(plotterShowHideMenu);

      JPanel menuFrame = new JPanel();
      menuFrame.add(plotterShowHideMenu, BorderLayout.LINE_START);
      JScrollPane scrollPane = new JScrollPane(menuFrame);

      frame.add(scrollPane, BorderLayout.EAST);
      frame.add(doneBox, BorderLayout.SOUTH);
      frame.add(plotter.getJPanel(), BorderLayout.CENTER);

      frame.setSize(preferredSize);
      frame.setVisible(true);

      yoGraphicsListRegistry.addArtifactListsToPlotter(plotter);

      while (!doneBox.isSelected())
      {
         try
         {
            Thread.sleep(100);
         }
         catch (InterruptedException ex)
         {
         }
      }

      frame.setVisible(false);
      frame.dispose();
   }

   public static List<Point2D> createCopGrid(List<Point2D> contactPoints, double gridResolution, boolean edgeOnly)
   {
      ConvexPolygon2D supportPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(contactPoints));
      ConvexPolygon2D shrunkPolygon = new ConvexPolygon2D(supportPolygon);
      ConvexPolygonScaler scaler = new ConvexPolygonScaler();
      scaler.scaleConvexPolygon(supportPolygon, 0.9 * gridResolution, shrunkPolygon);

      List<Point2D> copGrid = new ArrayList<>();
      int startX = (int) (supportPolygon.getMinX() / gridResolution) - 1;
      int endX = (int) (supportPolygon.getMaxX() / gridResolution) + 1;
      int startY = (int) (supportPolygon.getMinY() / gridResolution) - 1;
      int endY = (int) (supportPolygon.getMaxY() / gridResolution) + 1;
      for (int xIdx = startX; xIdx <= endX; xIdx++)
      {
         for (int yIdx = startY; yIdx <= endY; yIdx++)
         {
            double x = gridResolution * xIdx;
            double y = gridResolution * yIdx;
            Point2D cop = new Point2D(x, y);
            boolean insidePoint = shrunkPolygon.isPointInside(cop, -1.0e-5) && edgeOnly;
            if (supportPolygon.isPointInside(cop, 1.0e-5) && !insidePoint)
            {
               copGrid.add(cop);
            }
         }
      }
      return copGrid;
   }

   public static List<Point2D> createFootShape()
   {
      List<Point2D> contactPoints = new ArrayList<>();

      contactPoints.add(new Point2D(0.2, 0.1));
      contactPoints.add(new Point2D(0.2, -0.1));
      contactPoints.add(new Point2D(-0.2, -0.1));
      contactPoints.add(new Point2D(-0.2, 0.1));

      moveCentroidToOrigin(contactPoints);
      return contactPoints;
   }

   private static final void moveCentroidToOrigin(List<Point2D> points)
   {
      ConvexPolygon2D footPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points));
      Point2DReadOnly centroid = footPolygon.getCentroid();
      for (Point2D point : points)
      {
         point.sub(centroid);
      }
   }

   public static void main(String[] args)
   {
      new FrictionConeAnalysis();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/IntrinsicParametersMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "IntrinsicParametersMessage" defined in "IntrinsicParametersMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from IntrinsicParametersMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit IntrinsicParametersMessage_.idl instead.
*
*/
public class IntrinsicParametersMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.IntrinsicParametersMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::IntrinsicParametersMessage_";

   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.IntrinsicParametersMessage 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.IntrinsicParametersMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      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);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.IntrinsicParametersMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.IntrinsicParametersMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      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);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getRadial().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.IntrinsicParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_2(data.getWidth());

      cdr.write_type_2(data.getHeight());

      cdr.write_type_6(data.getFx());

      cdr.write_type_6(data.getFy());

      cdr.write_type_6(data.getSkew());

      cdr.write_type_6(data.getCx());

      cdr.write_type_6(data.getCy());

      if(data.getRadial().size() <= 100)
      cdr.write_type_e(data.getRadial());else
          throw new RuntimeException("radial field exceeds the maximum length");

      cdr.write_type_6(data.getT1());

      cdr.write_type_6(data.getT2());

   }

   public static void read(controller_msgs.msg.dds.IntrinsicParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setWidth(cdr.read_type_2());
      	
      data.setHeight(cdr.read_type_2());
      	
      data.setFx(cdr.read_type_6());
      	
      data.setFy(cdr.read_type_6());
      	
      data.setSkew(cdr.read_type_6());
      	
      data.setCx(cdr.read_type_6());
      	
      data.setCy(cdr.read_type_6());
      	
      cdr.read_type_e(data.getRadial());	
      data.setT1(cdr.read_type_6());
      	
      data.setT2(cdr.read_type_6());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.IntrinsicParametersMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_2("width", data.getWidth());
      ser.write_type_2("height", data.getHeight());
      ser.write_type_6("fx", data.getFx());
      ser.write_type_6("fy", data.getFy());
      ser.write_type_6("skew", data.getSkew());
      ser.write_type_6("cx", data.getCx());
      ser.write_type_6("cy", data.getCy());
      ser.write_type_e("radial", data.getRadial());
      ser.write_type_6("t1", data.getT1());
      ser.write_type_6("t2", data.getT2());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.IntrinsicParametersMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setWidth(ser.read_type_2("width"));
      data.setHeight(ser.read_type_2("height"));
      data.setFx(ser.read_type_6("fx"));
      data.setFy(ser.read_type_6("fy"));
      data.setSkew(ser.read_type_6("skew"));
      data.setCx(ser.read_type_6("cx"));
      data.setCy(ser.read_type_6("cy"));
      ser.read_type_e("radial", data.getRadial());
      data.setT1(ser.read_type_6("t1"));
      data.setT2(ser.read_type_6("t2"));
   }

   public static void staticCopy(controller_msgs.msg.dds.IntrinsicParametersMessage src, controller_msgs.msg.dds.IntrinsicParametersMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.IntrinsicParametersMessage createData()
   {
      return new controller_msgs.msg.dds.IntrinsicParametersMessage();
   }
   @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.IntrinsicParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.IntrinsicParametersMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.IntrinsicParametersMessage src, controller_msgs.msg.dds.IntrinsicParametersMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public IntrinsicParametersMessagePubSubType newInstance()
   {
      return new IntrinsicParametersMessagePubSubType();
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosPoint32Publisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import geometry_msgs.Point32;
import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics;

/**
 *
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class RosPoint32Publisher extends RosTopicPublisher<geometry_msgs.Point32>
{
   public RosPoint32Publisher(boolean latched)
   {
      super(geometry_msgs.Point32._TYPE, latched);
   }

   public void publish(float x, float y, float z)
   {
      Point32 message = getMessage();
      message.setX(x);
      message.setY(y);
      message.setZ(z);
      publish(message);
   }

   public void publish(Point3DBasics point3f)
   {
      publish(point3f.getX32(), point3f.getY32(), point3f.getZ32());
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulatedSensors/SimulatedMotionNodeIMURawSensorReader.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulatedSensors;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly;
import us.ihmc.robotics.math.corruptors.NoisyYoDouble;
import us.ihmc.robotics.math.corruptors.NoisyYoRotationMatrix;
import us.ihmc.robotics.sensors.RawIMUSensorsInterface;

public class SimulatedMotionNodeIMURawSensorReader extends SimulatedIMURawSensorReader
{
   private static final double GRAVITY = 9.80665; // MotionNode's definition of g, as given on p. 5 of 'MotionNode SDK Reference'

   // Data taken from http://www.motionnode.com/MotionNode_Specification.pdf
   public static final double DT = 0.01; // Sampling rate
   private static final boolean ORIENTATION_IS_NOISY = true;
   private static final double ORIENTATION_STANDARD_DEVIATION = 0.0; // output is filtered with an unknown bandwith
   private static final boolean ORIENTATION_USE_BIAS = true;
   private static final double ORIENTATION_BIAS_ROTATION_ANGLE_MAX = 1.0 / 180.0 * Math.PI; // for low dynamics: 1.0 / 180.0 * Math.PI, for high dynamics: 4.0 / 180.0 * Math.PI, TODO: make this number depending on the accelerometer values
   private static final double ORIENTATION_BIAS_ROTATION_ANGLE_MIN = -ORIENTATION_BIAS_ROTATION_ANGLE_MAX;
   private static final double ORIENTATION_BIAS_ROTATION_ANGLE_DELTA = 0.0002; // our choice for simulation
   private static final double ORIENTATION_BIAS_DIRECTION_HEIGHT_DELTA = 0.001; // our choice for simulation
   private static final double ORIENTATION_BIAS_DIRECTION_ANGLE_DELTA = ORIENTATION_BIAS_DIRECTION_HEIGHT_DELTA * Math.PI;

   private static final boolean ACCEL_IS_NOISY = true;
   private static final double ACCEL_STANDARD_DEVIATION = 0.1; // ?
   private static final boolean ACCEL_USE_BIAS = true;
   private static final double ACCEL_BIAS_MAX = 0.003 * GRAVITY; // depending on 2g or 6g range
   private static final double ACCEL_BIAS_MIN = -ACCEL_BIAS_MAX;
   private static final double ACCEL_BIAS_DELTA = ACCEL_BIAS_MAX / 100.0; // our choice for simulation

   private static final boolean GYRO_IS_NOISY = true;
   private static final double GYRO_STANDARD_DEVIATION = 0.05; // ?
   private static final boolean GYRO_USE_BIAS = true;
   private static final double GYRO_BIAS_MAX = Double.POSITIVE_INFINITY;
   private static final double GYRO_BIAS_MIN = -GYRO_BIAS_MAX;
   private static final double GYRO_BIAS_DELTA = 0.1 / 180.0 * Math.PI * DT;
   
   private final RotationMatrix imuMountingOffset;
   private final double localGravityZ;

   public SimulatedMotionNodeIMURawSensorReader(RawIMUSensorsInterface rawSensors, int imuIndex, RigidBodyBasics rigidBody, ReferenceFrame imuFrame,
         RigidBodyBasics rootBody, SpatialAccelerationReadOnly rootAcceleration, RotationMatrix imuMountingOffset, double localGravityPositiveZ)
   {
      super(rawSensors, imuIndex, rigidBody, imuFrame, rootBody, rootAcceleration);
      
      this.imuMountingOffset = imuMountingOffset;
      this.localGravityZ = localGravityPositiveZ;
   }

   @Override
   protected void initializeNoise()
   {
      rotationMatrix.setGaussianNoise(ORIENTATION_STANDARD_DEVIATION);
      rotationMatrix.setBiasOfRotationAngle(NoisyYoRotationMatrix.DEFAULT_BIAS_ROTATION_ANGLE, ORIENTATION_BIAS_ROTATION_ANGLE_MAX, ORIENTATION_BIAS_ROTATION_ANGLE_MIN, ORIENTATION_BIAS_ROTATION_ANGLE_DELTA);
      rotationMatrix.setBiasOfDirectionHeight(NoisyYoRotationMatrix.DEFAULT_BIAS_DIRECTION_HEIGHT, NoisyYoRotationMatrix.DEFAULT_BIAS_DIRECTION_HEIGHT_MAX, NoisyYoRotationMatrix.DEFAULT_BIAS_DIRECTION_HEIGHT_MIN, ORIENTATION_BIAS_DIRECTION_HEIGHT_DELTA);
      rotationMatrix.setBiasOfDirectionAngle(NoisyYoRotationMatrix.DEFAULT_BIAS_DIRECTION_ANGLE, NoisyYoRotationMatrix.DEFAULT_BIAS_DIRECTION_ANGLE_MAX, NoisyYoRotationMatrix.DEFAULT_BIAS_DIRECTION_ANGLE_MIN, ORIENTATION_BIAS_DIRECTION_ANGLE_DELTA);
      rotationMatrix.setBiasRandomlyBetweenMinAndMax();
      rotationMatrix.setBias(ORIENTATION_USE_BIAS);
      rotationMatrix.setIsNoisy(ORIENTATION_IS_NOISY);

      initializeGaussianNoise(accelList, ACCEL_IS_NOISY, ACCEL_STANDARD_DEVIATION, ACCEL_USE_BIAS, 0.0, ACCEL_BIAS_MAX, ACCEL_BIAS_MIN, ACCEL_BIAS_DELTA, true);
      initializeGaussianNoise(gyroList, GYRO_IS_NOISY, GYRO_STANDARD_DEVIATION, GYRO_USE_BIAS, 0.0, GYRO_BIAS_MAX, GYRO_BIAS_MIN, GYRO_BIAS_DELTA, false);
      // TODO add compass
   }

   // Make sure that simulateIMU() is only executed at original sample rate!
   @Override
   protected void simulateIMU()
   {
      // TODO: Add time delay, internal filters, sensor range clipping, nonlinearities

      rotationMatrix.update(perfM00.getDoubleValue(), perfM01.getDoubleValue(), perfM02.getDoubleValue(), perfM10.getDoubleValue(), perfM11.getDoubleValue(),
            perfM12.getDoubleValue(), perfM20.getDoubleValue(), perfM21.getDoubleValue(), perfM22.getDoubleValue());
      
      //12112012 added mounting orientation offset.
      RotationMatrix noisyRotationInWorld = rotationMatrix.getMatrix3d();
      noisyRotationInWorld.multiplyTransposeOther(imuMountingOffset);
      
      accelX.update(perfAccelX.getDoubleValue());
      accelY.update(perfAccelY.getDoubleValue());
      accelZ.update(perfAccelZ.getDoubleValue());
      
      //12112012 Acceleration output in worldFrame instead of imu frame
      FrameVector3D noisyAcceleration = new FrameVector3D(imuFrame, accelX.getDoubleValue(), accelY.getDoubleValue(), accelZ.getDoubleValue());
      noisyAcceleration.changeFrame(worldFrame);
      
      //12112012 Acceleration subtract gravity
      noisyAcceleration.sub(new FrameVector3D(worldFrame, 0.0, 0.0, localGravityZ));
      
      accelX.set(noisyAcceleration.getX());
      accelY.set(noisyAcceleration.getY());
      accelZ.set(noisyAcceleration.getZ());
      
      
      gyroX.update(perfGyroX.getDoubleValue());
      gyroY.update(perfGyroY.getDoubleValue());
      gyroZ.update(perfGyroZ.getDoubleValue());

      compassX.update(perfCompassX.getDoubleValue());
      compassY.update(perfCompassY.getDoubleValue());
      compassZ.update(perfCompassZ.getDoubleValue());
   }

   private void initializeGaussianNoise(NoisyYoDouble[] list, boolean isNoisy, double standardDeviation, boolean useBias, double bias, double biasMax,
         double biasMin, double biasDelta, boolean setBiasRandomlyBetweenMinAndMax)
   {
      for (NoisyYoDouble i : list)
      {
         i.setIsNoisy(isNoisy);
         i.setGaussianNoise(standardDeviation);

         if (useBias)
         {
            i.setBias(bias, biasMax, biasMin, biasDelta);
         }
         else
         {
            i.setBias(false);
         }
         
         if (setBiasRandomlyBetweenMinAndMax)
         {
            i.setBiasRandomlyBetweenMinAndMax();
         }
      }
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/InvalidPacketNotificationPacket.java<|end_filename|>
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 notifies the user that a previously sent message was rejected by the whole-body controller.
       */
public class InvalidPacketNotificationPacket extends Packet<InvalidPacketNotificationPacket> implements Settable<InvalidPacketNotificationPacket>, EpsilonComparable<InvalidPacketNotificationPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public java.lang.StringBuilder packet_class_simple_name_;
   public java.lang.StringBuilder error_message_;

   public InvalidPacketNotificationPacket()
   {
      packet_class_simple_name_ = new java.lang.StringBuilder(255);
      error_message_ = new java.lang.StringBuilder(255);
   }

   public InvalidPacketNotificationPacket(InvalidPacketNotificationPacket other)
   {
      this();
      set(other);
   }

   public void set(InvalidPacketNotificationPacket other)
   {
      sequence_id_ = other.sequence_id_;

      packet_class_simple_name_.setLength(0);
      packet_class_simple_name_.append(other.packet_class_simple_name_);

      error_message_.setLength(0);
      error_message_.append(other.error_message_);

   }

   /**
            * 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 void setPacketClassSimpleName(java.lang.String packet_class_simple_name)
   {
      packet_class_simple_name_.setLength(0);
      packet_class_simple_name_.append(packet_class_simple_name);
   }

   public java.lang.String getPacketClassSimpleNameAsString()
   {
      return getPacketClassSimpleName().toString();
   }
   public java.lang.StringBuilder getPacketClassSimpleName()
   {
      return packet_class_simple_name_;
   }

   public void setErrorMessage(java.lang.String error_message)
   {
      error_message_.setLength(0);
      error_message_.append(error_message);
   }

   public java.lang.String getErrorMessageAsString()
   {
      return getErrorMessage().toString();
   }
   public java.lang.StringBuilder getErrorMessage()
   {
      return error_message_;
   }


   public static Supplier<InvalidPacketNotificationPacketPubSubType> getPubSubType()
   {
      return InvalidPacketNotificationPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return InvalidPacketNotificationPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(InvalidPacketNotificationPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.packet_class_simple_name_, other.packet_class_simple_name_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.error_message_, other.error_message_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof InvalidPacketNotificationPacket)) return false;

      InvalidPacketNotificationPacket otherMyClass = (InvalidPacketNotificationPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!us.ihmc.idl.IDLTools.equals(this.packet_class_simple_name_, otherMyClass.packet_class_simple_name_)) return false;

      if (!us.ihmc.idl.IDLTools.equals(this.error_message_, otherMyClass.error_message_)) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("InvalidPacketNotificationPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("packet_class_simple_name=");
      builder.append(this.packet_class_simple_name_);      builder.append(", ");
      builder.append("error_message=");
      builder.append(this.error_message_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/ComplexNumber.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import us.ihmc.commons.MathTools;

import java.util.List;

/**
 *   Complex implements a complex number and defines complex
 *   arithmetic and mathematical functions
 *   Last Updated February 27, 2001
 *   Copyright 1997-2001
 *   @version 1.0
 *   @author <NAME>
 */
public class ComplexNumber
{
   private static final double TAU = Math.PI * 2.0;
   private double real, imag;

   public static ComplexNumber[] getComplexArray(int n)
   {
      ComplexNumber[] tempComplex = new ComplexNumber[n];
      for (int i = 0; i < tempComplex.length; i++)
         tempComplex[i] = new ComplexNumber();
      return tempComplex;
   }

   public static void copyComplexArray(ComplexNumber[] arrayToPack, ComplexNumber[] arrayToCopy)
   {
      if (arrayToPack.length < arrayToCopy.length)
         throw new RuntimeException("Insufficient size for copying complex number array, available: " + arrayToPack.length + " needed: " + arrayToCopy.length);
      int index = 0;
      for (; index < arrayToCopy.length; index++)
         arrayToPack[index].set(arrayToCopy[index]);

      for (; index < arrayToPack.length; index++)
         arrayToPack[index].set(0.0, 0.0);
   }

   public ComplexNumber()
   {
      this(0, 0);
   }

   /**
    *   Constructs the complex number z = u + i*v
    *   @param u Real part
    *   @param v imag part
    */
   public ComplexNumber(double real, double imag)
   {
      this.real = real;
      this.imag = imag;
   }

   public ComplexNumber(ComplexNumber complexNumber)
   {
      this.real = complexNumber.real;
      this.imag = complexNumber.imag;
   }

   public void set(double real, double imag)
   {
      this.real = real;
      this.imag = imag;
   }

   public void set(ComplexNumber other)
   {
      set(other.real, other.imag);
   }

   public void setReal(double real)
   {
      this.real = real;
   }

   public void setImaginary(double imag)
   {
      this.imag = imag;
   }

   public void setToPurelyReal(double real)
   {
      this.real = real;
      this.imag = 0;
   }

   public void setFromEuler(double magnitude, double argument)
   {
      this.real = magnitude * Math.cos(argument);
      this.imag = magnitude * Math.sin(argument);
   }

   public void setToZero()
   {
      this.real = 0.0;
      this.imag = 0.0;
   }


   /**
    *      Real part of this Complex number
    *      (the x-coordinate in rectangular coordinates).
    *      @return Re[z] where z is this Complex number.
    */
   public double real()
   {
      return real;
   }

   /**
    *   imag part of this Complex number
    *   (the y-coordinate in rectangular coordinates).
    *   @return Im[z] where z is this Complex number.
    */
   public double imag()
   {
      return imag;
   }

   /**
    *   Modulus of this Complex number
    *   (the distance from the origin in polar coordinates).
    *   @return |z| where z is this Complex number.
    */
   public double magnitude()
   {
      if ((real != 0.0) || (imag != 0.0))
      {
         return Math.sqrt(real * real + imag * imag);
      }
      else
      {
         return 0.0;
      }
   }


   public double magnitudeSquared()
   {
      if ((real != 0.0) || (imag != 0.0))
      {
         return (real * real + imag * imag);
      }
      else
      {
         return 0.0;
      }
   }

   /**
    *   Argument of this Complex number
    *   (the angle in radians with the x-axis in polar coordinates).
    *   @return arg(z) where z is this Complex number.
    */
   public double angle()
   {
      return Math.atan2(imag, real);
   }

   /**
    *   Complex conjugate of this Complex number
    *   (the conjugate of x+i*y is x-i*y).
    *   @return z-bar where z is this Complex number.
    */
   public ComplexNumber conj()
   {
      return new ComplexNumber(real, -imag);
   }

   /**
    *   Addition of ComplexNumbers (doesn't change this ComplexNumber).
    *   <br>(x+i*y) + (s+i*t) = (x+s)+i*(y+t).
    *   @param w is the number to add.
    *   @return z+w where z is this ComplexNumber.
    */
   public ComplexNumber plus(ComplexNumber w)
   {
      ComplexNumber complexNumber = new ComplexNumber(this);
      complexNumber.plusAndStore(w);
      return complexNumber;
   }

   public void plusAndStore(ComplexNumber c1, ComplexNumber c2)
   {
      set(c1);
      plusAndStore(c2);
   }

   public void plusAndStore(ComplexNumber other)
   {
      plusAndStore(other.real, other.imag);
   }

   public void plusAndStore(double real, double imag)
   {
      this.real += real;
      this.imag += imag;
   }

   public ComplexNumber plus(double d)
   {
      ComplexNumber complexNumber = new ComplexNumber(this);
      complexNumber.plusAndStore(d, 0.0);
      return complexNumber;
   }

   /**
    *   Subtraction of ComplexNumbers (doesn't change this ComplexNumber).
    *   <br>(x+i*y) - (s+i*t) = (x-s)+i*(y-t).
    *   @param w is the number to subtract.
    *   @return z-w where z is this ComplexNumber.
    */
   public ComplexNumber minus(ComplexNumber w)
   {
      ComplexNumber complexNumber = new ComplexNumber(this);
      complexNumber.minusAndStore(w);
      return complexNumber;
   }

   public ComplexNumber minus(double d)
   {
      ComplexNumber complexNumber = new ComplexNumber(this);
      complexNumber.minusAndStore(d, 0.0);
      return complexNumber;
   }

   public void minusAndStore(ComplexNumber c1, ComplexNumber c2)
   {
      set(c1);
      minusAndStore(c2);
   }

   public void minusAndStore(ComplexNumber other)
   {
      minusAndStore(other.real, other.imag);
   }

   public void minusAndStore(double real, double imag)
   {
      this.real -= real;
      this.imag -= imag;
   }
   /**
    *   ComplexNumber multiplication (doesn't change this ComplexNumber).
    *   @param w is the number to multiply by.
    *   @return z*w where z is this ComplexNumber.
    */
   public ComplexNumber times(ComplexNumber w)
   {
      ComplexNumber complexNumber = new ComplexNumber(this);
      complexNumber.timesAndStore(w);
      return complexNumber;
   }


   /**
    * ComplexNumber multiplication by a real number (doesn't change this ComplexNumber).
    * @param w is the number to multiply by.
    * @return z*w where z is this ComplexNumber.
    */
   public ComplexNumber times(double w)
   {
      ComplexNumber complexNumber = new ComplexNumber(this);
      complexNumber.timesAndStore(w, 0.0);
      return complexNumber;
   }


   /**
    *   ComplexNumber multiplication.
    *   @param w is the number to multiply by.
    *   @return z*w where z is this ComplexNumber.
    */
   public void timesAndStore(ComplexNumber other)
   {
      this.timesAndStore(other.real, other.imag);
   }

   public void timesAndStore(double real, double imag)
   {
      double tempVal1 = this.real * real - this.imag * imag;
      this.imag = this.real * imag + real * this.imag;
      this.real = tempVal1;
   }

   public void timesAndStore(ComplexNumber c1, ComplexNumber c2)
   {
      set(c1);
      timesAndStore(c2);
   }

   /**
    *   Division of ComplexNumbers (doesn't change this ComplexNumber).
    *   <br>(x+i*y)/(s+i*t) = ((x*s+y*t) + i*(y*s-y*t)) / (s^2+t^2)
    *   @param w is the number to divide by
    *   @return new ComplexNumber z/w where z is this ComplexNumber
    */
   public ComplexNumber dividedBy(ComplexNumber w)
   {
      double den = MathTools.square(w.magnitude());
      
      return new ComplexNumber((real * w.real() + imag * w.imag()) / den, (imag * w.real() - real * w.imag()) / den);
   }

   public void scale(double scalar)
   {
      this.real *= scalar;
      this.imag *= scalar;
   }

   /**
    *   ComplexNumber exponential (doesn't change this ComplexNumber).
    *   @return exp(z) where z is this ComplexNumber.
    */
   public ComplexNumber exp()
   {
      return new ComplexNumber(Math.exp(real) * Math.cos(imag), Math.exp(real) * Math.sin(imag));
   }

   /**
    *   Principal branch of the ComplexNumber logarithm of this ComplexNumber.
    *   (doesn't change this ComplexNumber).
    *   The principal branch is the branch with -pi < arg <= pi.
    *   @return log(z) where z is this ComplexNumber.
    */
   public ComplexNumber log()
   {
      return new ComplexNumber(Math.log(this.magnitude()), this.angle());
   }

   /**
    *   ComplexNumber square root (doesn't change this ComplexNumber).
    *   Computes the principal branch of the square root, which
    *   is the value with 0 <= arg < pi.
    *   @return sqrt(z) where z is this ComplexNumber.
    */
   public ComplexNumber sqrt()
   {
      double r = Math.sqrt(this.magnitude());
      double theta = this.angle() / 2;

      return new ComplexNumber(r * Math.cos(theta), r * Math.sin(theta));
   }

   // Real cosh function (used to compute ComplexNumber trig functions)
   private double cosh(double theta)
   {
      return (Math.exp(theta) + Math.exp(-theta)) / 2;
   }

   // Real sinh function (used to compute ComplexNumber trig functions)
   private double sinh(double theta)
   {
      return (Math.exp(theta) - Math.exp(-theta)) / 2;
   }

   /**
    *   Sine of this ComplexNumber (doesn't change this ComplexNumber).
    *   <br>sin(z) = (exp(i*z)-exp(-i*z))/(2*i).
    *   @return sin(z) where z is this ComplexNumber.
    */
   public ComplexNumber sin()
   {
      return new ComplexNumber(cosh(imag) * Math.sin(real), sinh(imag) * Math.cos(real));
   }

   /**
    *   Cosine of this ComplexNumber (doesn't change this ComplexNumber).
    *   <br>cos(z) = (exp(i*z)+exp(-i*z))/ 2.
    *   @return cos(z) where z is this ComplexNumber.
    */
   public ComplexNumber cos()
   {
      return new ComplexNumber(cosh(imag) * Math.cos(real), -sinh(imag) * Math.sin(real));
   }

   /**
    *   Hyperbolic sine of this ComplexNumber
    *   (doesn't change this ComplexNumber).
    *   <br>sinh(z) = (exp(z)-exp(-z))/2.
    *   @return sinh(z) where z is this ComplexNumber.
    */
   public ComplexNumber sinh()
   {
      return new ComplexNumber(sinh(real) * Math.cos(imag), cosh(real) * Math.sin(imag));
   }

   /**
    *   Hyperbolic cosine of this ComplexNumber
    *   (doesn't change this ComplexNumber).
    *   <br>cosh(z) = (exp(z) + exp(-z)) / 2.
    *   @return cosh(z) where z is this ComplexNumber.
    */
   public ComplexNumber cosh()
   {
      return new ComplexNumber(cosh(real) * Math.cos(imag), sinh(real) * Math.sin(imag));
   }

   /**
    *   Tangent of this ComplexNumber (doesn't change this ComplexNumber).
    *   <br>tan(z) = sin(z)/cos(z).
    *   @return tan(z) where z is this ComplexNumber.
    */
   public ComplexNumber tan()
   {
      return (this.sin()).dividedBy(this.cos());
   }

   /**
    *   Negative of this ComplexNumber (chs stands for change sign).
    *   This produces a new ComplexNumber and doesn't change
    *   this ComplexNumber.
    *   <br>-(x+i*y) = -x-i*y.
    *   @return -z where z is this ComplexNumber.
    */
   public ComplexNumber changeSign()
   {
      return new ComplexNumber(-real, -imag);
   }

   public boolean epsilonEquals(double realNumber, double epsilon)
   {
      if (Math.abs(this.real - realNumber) > epsilon)
         return false;
      if (Math.abs(this.imag) > epsilon)
         return false;

      return true;
   }

   public boolean epsilonEquals(ComplexNumber complexNumber, double epsilon)
   {
      if (Math.abs(this.real - complexNumber.real()) > epsilon)
         return false;
      if (Math.abs(this.imag - complexNumber.imag()) > epsilon)
         return false;

      return true;
   }

   public void getRoots(ComplexNumber root1, ComplexNumber root2)
   {
      double tempVal1 = Math.sqrt(magnitude());
      double tempVal2 = angle();
      root1.setFromEuler(tempVal1, tempVal2 / 2.0);
      root2.setFromEuler(tempVal1, (tempVal2 + 2.0 * Math.PI) / 2.0);
   }

   public void getRoots(List<ComplexNumber> rootsToPack, int n)
   {
      double tempVal1 = Math.pow(magnitude(), 1.0 / n);
      double tempVal2 = angle() / n;
      for (int i = 0; i < n; i++)
         rootsToPack.get(i).setFromEuler(tempVal1, tempVal2 + ((double) TAU * i) / (double) n);
   }

   /**
    *   String representation of this ComplexNumber.
    *   @return x+i*y, x-i*y, x, or i*y as appropriate.
    */
   public String toString()
   {
      if ((real != 0) && (imag > 0))
      {
         return real + " + " + imag + "i";
      }

      if ((real != 0) && (imag < 0))
      {
         return real + " - " + (-imag) + "i";
      }

      if (imag == 0)
      {
         return String.valueOf(real);
      }

      if (real == 0)
      {
         return imag + "i";
      }

      // shouldn't get here (unless Inf or NaN)
      return real + " + i*" + imag;

   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/reflect/RecursiveObjectComparer.java<|end_filename|>
package us.ihmc.tools.reflect;

import java.lang.reflect.Array;
import java.lang.reflect.Field;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.Map;

import us.ihmc.tools.containers.ContainerTools;

/**
 * @author Twan
 *
 */
public class RecursiveObjectComparer
{
   private Object topLevelObject1;
   private Object topLevelObject2;
   private final int maxDepth;
   private final int maxSize;

   private final ArrayList<Field> differingFields = new ArrayList<Field>();
   private final ArrayList<Object[]> secondLowestLevelDifferenceList = new ArrayList<Object[]>();
   private final LinkedHashMap<Object, ArrayList<Object>> secondLowestLevelDifferenceMap = new LinkedHashMap<Object, ArrayList<Object>>();
   private final ArrayList<String> secondLowestLevelDifferenceLocations = new ArrayList<String>();
   private final ArrayList<String[]> lowestLevelDifferencesList = new ArrayList<String[]>();

   private LinkedHashMap<Object, ArrayList<Object>> previouslyProcessedDifferentObjectPairs = new LinkedHashMap<Object, ArrayList<Object>>();
   private LinkedHashMap<Object, ArrayList<Object>> previouslyProcessedEqualObjectPairs = new LinkedHashMap<Object, ArrayList<Object>>();
   private LinkedHashMap<Object, ArrayList<Object>> objectStack = new LinkedHashMap<Object, ArrayList<Object>>();
   private long primitivesProcessed = 0;
   private long fieldsProcessed = 0;
   private final StringBuffer differenceLocationBuffer = new StringBuffer();


   private LinkedHashMap<Object, ArrayList<Object>> objectPairsToIgnore = new LinkedHashMap<Object, ArrayList<Object>>();
   private LinkedHashSet<Field> fieldsToIgnore = new LinkedHashSet<Field>();
   private LinkedHashSet<Class<?>> classesToIgnore = new LinkedHashSet<Class<?>>();
   private StringFieldMatcher stringFieldMatchersToIgnore = new StringFieldMatcher();


   private final boolean isTopLevel;

   /**
    * top level constructor
    */
   public RecursiveObjectComparer(int maxDepth, int maxSize)
   {
      this.maxDepth = maxDepth;
      this.maxSize = maxSize;
      this.isTopLevel = true;
   }

   /**
    * child level constructor (for comparing keys)
    */
   private RecursiveObjectComparer(RecursiveObjectComparer parentComparer, int maxDepth, int maxSize)
   {
      this.maxDepth = maxDepth;
      this.maxSize = maxSize;
      this.isTopLevel = false;

      // link to parent comparer
      this.classesToIgnore = parentComparer.classesToIgnore;    // will not be changed by the child comparer
      this.fieldsToIgnore = parentComparer.fieldsToIgnore;    // will not be changed by the child comparer
      this.stringFieldMatchersToIgnore = parentComparer.stringFieldMatchersToIgnore;
      this.previouslyProcessedEqualObjectPairs = parentComparer.previouslyProcessedEqualObjectPairs;    // stuff will be added by this comparer, but that's to parentComparers benefit
      this.previouslyProcessedDifferentObjectPairs = parentComparer.previouslyProcessedDifferentObjectPairs;    // stuff will be added by this comparer, but that's to parentComparers benefit
      this.objectStack = parentComparer.objectStack;    // have to add this, otherwise loops won't be resolved correctly.
   }

   public boolean compare(Object object1, Object object2) throws IllegalArgumentException, IllegalAccessException
   {
      this.topLevelObject1 = object1;
      this.topLevelObject2 = object2;
      boolean objectsEqual = recursivelyCompareObjects(topLevelObject1, topLevelObject2, maxDepth, null);
      doPostCompareSanityChecks();
   
//      if (!objectsEqual && this.differingFields.isEmpty())
//      {
//         throw new RuntimeException("(ret == false) && this.differingFields.isEmpty()");
//      }
//      if (objectsEqual && !differingFields.isEmpty())
//      {
//         throw new RuntimeException("(ret == true) && !differingFields.isEmpty()");
//      }
      
      return objectsEqual;
   }

   public void clear()
   {
      differingFields.clear();
      secondLowestLevelDifferenceList.clear();
      secondLowestLevelDifferenceMap.clear();
      secondLowestLevelDifferenceLocations.clear();
      lowestLevelDifferencesList.clear();

      previouslyProcessedDifferentObjectPairs.clear();
      previouslyProcessedEqualObjectPairs.clear();
      objectStack.clear();
      primitivesProcessed = 0;
      fieldsProcessed = 0;
      differenceLocationBuffer.setLength(0);
   }

   public boolean isTopLevel()
   {
      return isTopLevel;
   }

   public long getPrimitivesProcessed()
   {
      return primitivesProcessed;
   }

   public long getFieldsProcessed()
   {
      return fieldsProcessed;
   }

   public ArrayList<Field> getDifferingFields()
   {
      return differingFields;
   }

   public ArrayList<Object[]> getLowestLevelObjectDifferenceList()
   {
      return secondLowestLevelDifferenceList;
   }

   public HashMap<Object, ArrayList<Object>> getLowestLevelObjectDifferenceMap()
   {
      return secondLowestLevelDifferenceMap;
   }

   public ArrayList<String> getLowestLevelObjectDifferenceLocations()
   {
      return secondLowestLevelDifferenceLocations;
   }

   public void addFieldToIgnore(Field fieldToIgnore)
   {
      this.fieldsToIgnore.add(fieldToIgnore);
   }

   public void addFieldsToIgnore(Collection<Field> fieldsToIgnore)
   {
      this.fieldsToIgnore.addAll(fieldsToIgnore);
   }

   public void addStringFieldsToIgnore(StringFieldMatcher stringFieldMatcherToIgnore)
   {
      if (stringFieldMatcherToIgnore != null)
      {
         this.stringFieldMatchersToIgnore.combine(stringFieldMatcherToIgnore);
      }
   }
   
   public void addClassToIgnore(Class<?> classToIgnore)
   {
      this.classesToIgnore.add(classToIgnore);
   }

   public void addClassesToIgnore(Collection<? extends Class<?>> classesToIgnore)
   {
      this.classesToIgnore.addAll(classesToIgnore);
   }

   public void addObjectPairsToIgnore(Map<Object, ArrayList<Object>> additionalObjectPairsToIgnore)
   {
      for (Object key : additionalObjectPairsToIgnore.keySet())
      {
         if (this.objectPairsToIgnore.containsKey(key))
         {
            this.objectPairsToIgnore.get(key).addAll(additionalObjectPairsToIgnore.get(key));
         }
         else
         {
            this.objectPairsToIgnore.put(key, new ArrayList<Object>(additionalObjectPairsToIgnore.get(key)));
         }
      }
   }

   private void doPostCompareSanityChecks()
   {
      // everything is processed at this point, so the object stack should be empty
      if (isTopLevel())
      {
         if (!objectStack.isEmpty())
         {
            throw new RuntimeException("object stack should be empty. It contains\n: " + objectStack);
         }
      }

      // size checks
      int size = differingFields.size();
      if (secondLowestLevelDifferenceList.size() != size)
      {
         throw new RuntimeException("Sizes don't match");
      }

      if (secondLowestLevelDifferenceLocations.size() != size)
      {
         throw new RuntimeException("Sizes don't match");
      }

      if (lowestLevelDifferencesList.size() != size)
      {
         throw new RuntimeException("Sizes don't match");
      }
   }

   private boolean recursivelyCompareObjects(Object object1, Object object2, int depthToGo, Field callingField)
           throws IllegalArgumentException, IllegalAccessException
   {
      boolean ret;
      if (hasMaximumNumberOfDifferencesBeenReached())
         ret = true;
      else if (areObjectsIdenticalOrBothNull(object1, object2))
         ret = true;
      else if (isExactlyOneObjectNull(object1, object2))
      {
         storeExactlyOneNullDifference(object1, object2, callingField);
         ret = false;
      }
      // at this point, the objects cannot be null, so we can get their types
      else if (areObjectsOfDifferentClasses(object1, object2))
      {
         storeDifferentClassesDifference(object1, object2, callingField);
         ret = false;
      }
      else
      {
         Class<?> objectType = object1.getClass();    // doesn't matter which one we use, since they're the same at this point

         if (isClassAnException(objectType))
            ret = true;
         else if (isObjectPairAnException(object1, object2))
            ret = true;
         else if (alreadyKnowObjectsAreDifferent(object1, object2))
         {
            ret = false;
         }
         else if (alreadyKnowObjectsAreEqual(object1, object2))
            ret = true;
         else if (areObjectsAlreadyBeingCompared(object1, object2))
         {
            // resolve loops
            ret = true;
         }
         else
         {
            addObjectPairToObjectStack(object1, object2);

            // After this point, the object pair that's just been put on the objectStack must be taken off it before we return

            // put the root class name at the start of the location strings
            if (callingField == null)
            {
               differenceLocationBuffer.append(objectType.getName());
            }

            if (objectType.isArray())
            {
               // handle arrays
               ret = handleArrays(object1, object2, depthToGo, callingField);
               doBookKeeping(object1, object2, ret);
            }
            else if (Map.class.isAssignableFrom(objectType))
            {
               // handle maps
               ret = handleMaps(object1, object2, depthToGo, callingField);
               doBookKeeping(object1, object2, ret);
            }
            else
            {
               // handle other kinds of objects
               ret = compareFields(object1, object2, depthToGo);
            }
         }
      }

      return ret;
   }

   private void doBookKeeping(Object object1, Object object2, boolean equal)
   {
      if (equal)
      {
         registerEqualObjects(object1, object2);
      }
      else
      {
         registerDifferentObjects(object1, object2);
      }

      removeFromObjectStack(object1, object2);
   }

   private boolean compareFields(Object object1, Object object2, int depthToGo)
   {
      if (depthToGo > 0)
      {
         try
         {
            boolean result = tryToCompareFields(object1, object2, depthToGo);
            return result;
         }
         catch (IllegalAccessException e)
         {
            e.printStackTrace();
            removeFromObjectStack(object1, object2);

            return false;
         }
      }
      else
      {
         removeFromObjectStack(object1, object2);

         return true;    // don't know, actually...
      }
   }

   private boolean tryToCompareFields(Object object1, Object object2, int depthToGo) throws IllegalAccessException
   {
      ArrayList<Field> allFields = ReflectionTools.getAllFields(object1.getClass());

      boolean equal = true;
      for (Field field : allFields)
      {
         if (fieldsToIgnore.contains(field))
         {
            continue;
         }
         
         if (stringFieldMatchersToIgnore != null)
         {
            if (stringFieldMatchersToIgnore.matches(object1)) 
            {
               continue;
            }
         }

         int oldLocationBufferLength = differenceLocationBuffer.length();
         differenceLocationBuffer.append("." + getFieldString(field));

         field.setAccessible(true);
         Class<?> fieldType = field.getType();
         boolean isPrimitive = fieldType.isPrimitive();

         if (isPrimitive)
         {
            if (!ReflectionTools.isPrimitiveFieldContentTheSame(object1, object2, field))
            {
               storePrimitiveDifference(object1, object2, field);
               equal = false;
            }

            primitivesProcessed++;
         }
         else
         {
            Object fieldContent1 = field.get(object1);
            Object fieldContent2 = field.get(object2);

            boolean fieldEqual = recursivelyCompareObjects(fieldContent1, fieldContent2, depthToGo - 1, field);
            if (!fieldEqual)
            {
               equal = false;    // design choice: don't return here: keep going to find more differences
            }
         }

         differenceLocationBuffer.setLength(oldLocationBufferLength);
         fieldsProcessed++;
      }

      doBookKeeping(object1, object2, equal);

      return equal;
   }


   private void addObjectPairToObjectStack(Object object1, Object object2)
   {
      ArrayList<Object> objectStackEntry = objectStack.get(object1);
      if (objectStackEntry == null)
      {
         objectStackEntry = new ArrayList<Object>();
         objectStack.put(object1, objectStackEntry);
      }

      objectStackEntry.add(object2);
   }

   private boolean areObjectsAlreadyBeingCompared(Object object1, Object object2)
   {
      ArrayList<Object> objectStackEntry = objectStack.get(object1);
      if (objectStackEntry == null)
         return false;

      boolean objectsAlreadyBeingCompared = objectStackEntry.contains(object2);

      return objectsAlreadyBeingCompared;
   }


   private boolean alreadyKnowObjectsAreEqual(Object object1, Object object2)
   {
      boolean alreadyKnowObjectsAreEqual = (previouslyProcessedEqualObjectPairs.get(object1) != null)
                                           && previouslyProcessedEqualObjectPairs.get(object1).contains(object2);

      return alreadyKnowObjectsAreEqual;
   }

   private boolean alreadyKnowObjectsAreDifferent(Object object1, Object object2)
   {
      boolean alreadyKnowObjectsAreDifferent = (previouslyProcessedDifferentObjectPairs.get(object1) != null)
                                               && previouslyProcessedDifferentObjectPairs.get(object1).contains(object2);

      return alreadyKnowObjectsAreDifferent;
   }

   private boolean isObjectPairAnException(Object object1, Object object2)
   {
      ArrayList<Object> exceptionEntry = objectPairsToIgnore.get(object1);

      return ((exceptionEntry != null) && exceptionEntry.contains(object2));
   }

   private boolean isClassAnException(Class<?> objectType)
   {
      // check if the class is an exception
      for (Class<?> exceptionClass : classesToIgnore)
      {
         if (exceptionClass.isAssignableFrom(objectType))
         {
            return true;
         }
      }

      return false;
   }

   private static boolean areObjectsOfDifferentClasses(Object object1, Object object2)
   {
      Class<?> object1Type = object1.getClass();
      Class<?> object2Type = object2.getClass();

      return (object1Type != object2Type);
   }

   private boolean hasMaximumNumberOfDifferencesBeenReached()
   {
      return (differingFields.size() >= maxSize);
   }

   private static boolean areObjectsIdenticalOrBothNull(Object object1, Object object2)
   {
      return (object1 == object2);
   }

   private static boolean isExactlyOneObjectNull(Object object1, Object object2)
   {
      boolean object1Null = object1 == null;
      boolean object2Null = object2 == null;

      if ((object1Null &&!object2Null) || (!object1Null && object2Null))
      {
         return true;
      }

      return false;
   }

   private void registerEqualObjects(Object object1, Object object2)
   {
      ArrayList<Object> entry = previouslyProcessedEqualObjectPairs.get(object1);
      if (entry == null)
      {
         ArrayList<Object> arrayList = new ArrayList<Object>();
         arrayList.add(object2);
         previouslyProcessedEqualObjectPairs.put(object1, arrayList);
      }
      else
      {
         entry.add(object2);
      }
   }

   private void registerDifferentObjects(Object object1, Object object2)
   {
      ArrayList<Object> entry = previouslyProcessedDifferentObjectPairs.get(object1);
      if (entry == null)
      {
         ArrayList<Object> arrayList = new ArrayList<Object>();
         arrayList.add(object2);
         previouslyProcessedDifferentObjectPairs.put(object1, arrayList);
      }
      else
      {
         entry.add(object2);
      }
   }

   private void removeFromObjectStack(Object object1, Object object2)
   {
      ArrayList<Object> objectStackEntry = objectStack.get(object1);
      if (objectStackEntry != null)
      {
         ContainerTools.removeByReference(objectStackEntry, object2);

         if (objectStackEntry.isEmpty())
         {
            objectStack.remove(object1);
         }
      }
      else
      {
         throw new RuntimeException("Object not found!");
      }
   }

   private void storePrimitiveDifference(Object object1, Object object2, Field callingField) throws IllegalArgumentException, IllegalAccessException
   {
      log(object1, object2, callingField);

      String[] primitiveDifference = getPrimitiveDifference(object1, object2, callingField);
      lowestLevelDifferencesList.add(primitiveDifference);
   }

   private void storePrimitiveDifferenceForArrays(Object array1, Object array2, Field callingField, int arrayIndex)
           throws IllegalArgumentException, IllegalAccessException
   {
      log(array1, array2, callingField);

      String[] primitiveDifference = getPrimitiveDifferenceForArrays(array1, array2, arrayIndex);
      lowestLevelDifferencesList.add(primitiveDifference);
   }

   private void storeArrayLengthDifference(Object array1, Object array2, Field callingField)
   {
      log(array1, array2, callingField);
      
      String[] differenceString = getLengthDifferenceForArrays(array1, array2);
      lowestLevelDifferencesList.add(differenceString);
   }

   private void storeExactlyOneNullDifference(Object object1, Object object2, Field callingField)
   {
      log(object1, object2, callingField);

      String[] primitiveDifference = new String[2];
      primitiveDifference[0] = object1 == null ? "null" : "not null";
      primitiveDifference[1] = object2 == null ? "null" : "not null";
      lowestLevelDifferencesList.add(primitiveDifference);
   }

   private void storeDifferentClassesDifference(Object object1, Object object2, Field callingField)
   {
      log(object1, object2, callingField);

      String[] primitiveDifference = new String[2];
      primitiveDifference[0] = object1.getClass().toString();
      primitiveDifference[1] = object2.getClass().toString();
      lowestLevelDifferencesList.add(primitiveDifference);
   }

   private void log(Object object1, Object object2, Field callingField)
   {
      differingFields.add(callingField);

      Object[] objectDifference = {object1, object2};
      secondLowestLevelDifferenceList.add(objectDifference);

      secondLowestLevelDifferenceLocations.add(differenceLocationBuffer.toString());

      ArrayList<Object> lowestLevelObjectDifferenceMapEntry = secondLowestLevelDifferenceMap.get(object1);
      if (lowestLevelObjectDifferenceMapEntry == null)
      {
         ArrayList<Object> arrayList = new ArrayList<Object>();
         arrayList.add(object2);
         secondLowestLevelDifferenceMap.put(object1, arrayList);
      }
      else
      {
         lowestLevelObjectDifferenceMapEntry.add(object2);
      }
   }

   private static String[] getPrimitiveDifference(Object object1, Object object2, Field callingField) throws IllegalAccessException
   {
      String[] primitiveDifference = new String[2];
      primitiveDifference[0] = ReflectionTools.getStringRepresentationOfFieldContent(object1, callingField);
      primitiveDifference[1] = ReflectionTools.getStringRepresentationOfFieldContent(object2, callingField);

      return primitiveDifference;
   }

   private static String[] getPrimitiveDifferenceForArrays(Object array1, Object array2, int arrayIndex)
   {
      String[] primitiveDifference = new String[2];
      primitiveDifference[0] = ReflectionTools.getStringRepresentationOfArrayEntry(array1, arrayIndex);
      primitiveDifference[1] = ReflectionTools.getStringRepresentationOfArrayEntry(array2, arrayIndex);

      return primitiveDifference;
   }

   private String[] getLengthDifferenceForArrays(Object array1, Object array2)
   {
      String[] differenceString = new String[2];
      differenceString[0] = "length = " + Array.getLength(array1);
      differenceString[1] = "length = " + Array.getLength(array2);
      return differenceString;
   }

   private String getFieldString(Field field)
   {
      String fieldString = field.toString();
      int dotIndex = fieldString.lastIndexOf(".");

      return fieldString.substring(dotIndex + 1);
   }

   private boolean handleArrays(Object array1, Object array2, int depthToGo, Field callingField) throws IllegalArgumentException, IllegalAccessException
   {
      int arrayLength = Array.getLength(array1);
      if (arrayLength != Array.getLength(array2))
      {
         storeArrayLengthDifference(array1, array2, callingField);
         return false;
      }
      else
      {
         boolean isPrimitiveArray = array1.getClass().getComponentType().isPrimitive();
         boolean equal = true;
         for (int i = 0; i < arrayLength; i++)
         {
            int oldLocationBufferLength = differenceLocationBuffer.length();
            differenceLocationBuffer.append("[" + i + "]");
            boolean entryEqual;
            if (isPrimitiveArray)
            {
               entryEqual = ReflectionTools.isPrimitiveArrayEntryTheSame(array1, array2, i);

               if (!entryEqual)
               {
                  storePrimitiveDifferenceForArrays(array1, array2, callingField, i);
               }
            }
            else
            {
               entryEqual = recursivelyCompareObjects(Array.get(array1, i), Array.get(array2, i), depthToGo - 1, callingField);
            }

            if (!entryEqual)
            {
               equal = false;

               // design choice: don't return here; keep going to find more differences
            }

            differenceLocationBuffer.setLength(oldLocationBufferLength);
         }

         return equal;
      }
   }

   private boolean handleMaps(Object map1, Object map2, int depthToGo, Field callingField) throws IllegalArgumentException, IllegalAccessException
   {
      Map<?, ?> map1Cast = (Map<?, ?>) map1;
      Map<?, ?> map2Cast = (Map<?, ?>) map2;

      if (map1Cast.size() != map2Cast.size())
      {
         return false;
      }

      Object[] keySet1 = map1Cast.keySet().toArray();
      Object[] keySet2 = map2Cast.keySet().toArray();

      boolean equal = true;
      key1Loop:
      for (int i = 0; i < keySet1.length; i++)
      {
         Object key1 = keySet1[i];
         boolean keyFound = false;
         for (int j = 0; j < keySet2.length; j++)
         {
            Object key2 = keySet2[j];
            boolean keyEqual = compareKeys(key1, key2, Integer.MAX_VALUE);
            if (keyEqual)
            {
               // correct key found; now compare the corresponding values
               keyFound = true;
               Object value1 = map1Cast.get(key1);
               Object value2 = map2Cast.get(key2);
               boolean valueEqual = recursivelyCompareObjects(value1, value2, depthToGo - 1, callingField);
               if (!valueEqual)
               {
                  equal = false;
               }

               continue key1Loop;    // design choice: don't return here: keep going to find more differences
            }
         }

         // correct key not found
         if (!keyFound && isTopLevel())
         {
//          System.err.println("Could not find map2's counterpart of map1's key " + key1);
            equal = false;
         }
      }

      return equal;
   }

   private boolean compareKeys(Object key1, Object key2, int depthToGo) throws IllegalArgumentException, IllegalAccessException
   {
      RecursiveObjectComparer keyComparer = new RecursiveObjectComparer(this, depthToGo - 1, Integer.MAX_VALUE);
      boolean equal = keyComparer.compare(key1, key2);

      return equal;
   }

   @Override
   public String toString()
   {
      StringBuffer buf = new StringBuffer();

      int maxDifferingFields = 5000;
      int nDifferingFields = differingFields.size();
      int fieldsToBePrinted = Math.min(nDifferingFields, maxDifferingFields);
      for (int i = 0; i < fieldsToBePrinted; i++)
      {
         Field field = differingFields.get(i);
         String location = secondLowestLevelDifferenceLocations.get(i);

         buf.append("Field: " + field + "\n");
         buf.append("Location: " + location + "\n");

         String[] primitiveDifference = lowestLevelDifferencesList.get(i);    // should be synchronized with differingFields (not very nice, but it's late)
         Object[] secondLowestLevelDifference = secondLowestLevelDifferenceList.get(i);
         for (int j = 0; j < primitiveDifference.length; j++)
         {
            buf.append("   Primitive " + (j) + ": " + primitiveDifference[j]);
            
            Object containingObject = secondLowestLevelDifference[j];
            buf.append("   Containing object " + (j) + ": " + containingObject);
            if (containingObject != null)
            {
               buf.append("   Containing object class " + (j) + ": " + containingObject.getClass());               
            }
            buf.append("\n");
         }
      }

      if (fieldsToBePrinted < nDifferingFields)
      {
         buf.append("Output truncated. First " + maxDifferingFields + " differing fields shown.\n");
      }

      buf.append("\n");
      buf.append("Differing field types:\n");
      LinkedHashSet<Field> differingFieldsSet = new LinkedHashSet<Field>(differingFields);
      for (Field field : differingFieldsSet)
      {
         if (field != null)
         {
            buf.append(field.toGenericString());
         }
         else
         {
            buf.append("null");
         }
         buf.append("\n");
      }

      buf.append("\n");
      buf.append("Total number of differing fields: " + nDifferingFields + "\n");

      return buf.toString();
   }
}


<|start_filename|>ihmc-simulation-toolkit/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/InverseDynamicsJointsFromSCSRobotGenerator.java<|end_filename|>
package us.ihmc.sensorProcessing.simulatedSensors;

import java.util.Collection;
import java.util.concurrent.ConcurrentLinkedQueue;

import us.ihmc.euclid.matrix.Matrix3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.mecano.multiBodySystem.PrismaticJoint;
import us.ihmc.mecano.multiBodySystem.RevoluteJoint;
import us.ihmc.mecano.multiBodySystem.RigidBody;
import us.ihmc.mecano.multiBodySystem.SixDoFJoint;
import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.SpatialAcceleration;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Joint;
import us.ihmc.simulationconstructionset.Link;
import us.ihmc.simulationconstructionset.OneDegreeOfFreedomJoint;
import us.ihmc.simulationconstructionset.PinJoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.SliderJoint;

public class InverseDynamicsJointsFromSCSRobotGenerator
{
   private final SCSToInverseDynamicsJointMap scsToInverseDynamicsJointMap = new SCSToInverseDynamicsJointMap();

   private final RigidBodyBasics elevator;

   /**
    * Using the given SCS robot, generates an equivalent robot model using the
    * {@code InverseDynamicsJoint}s.
    * 
    * @param robot the simulated robot to use. Not modified.
    */
   //TODO: Add SliderJoints
   public InverseDynamicsJointsFromSCSRobotGenerator(Robot robot)
   {
      elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame());

      ConcurrentLinkedQueue<Joint> jointQueue = new ConcurrentLinkedQueue<Joint>();
      jointQueue.addAll(robot.getRootJoints());

      while (!jointQueue.isEmpty())
      {
         Joint polledJoint = jointQueue.poll();

         // Link parameters:
         Link link = polledJoint.getLink();

         Matrix3D momentOfInertia = new Matrix3D();
         link.getMomentOfInertia(momentOfInertia);

         double mass = link.getMass();

         Vector3D comOffset = new Vector3D();
         link.getComOffset(comOffset);

         RigidBodyBasics parentIDBody = getParentIDBody(polledJoint, elevator);

         if (polledJoint instanceof FloatingJoint)
         {
            FloatingJoint currentJoint = (FloatingJoint) polledJoint;

            FloatingJointBasics currentIDJoint = new SixDoFJoint(currentJoint.getName(), elevator);
            new RigidBody(link.getName(), currentIDJoint, momentOfInertia, mass, comOffset);

            scsToInverseDynamicsJointMap.addLinkedJoints(currentJoint, currentIDJoint);
         }
         else if (polledJoint instanceof PinJoint)
         {
            PinJoint currentJoint = (PinJoint) polledJoint;

            Vector3D jointAxis = new Vector3D();
            currentJoint.getJointAxis(jointAxis);

            Vector3D jointOffset = new Vector3D();
            currentJoint.getOffset(jointOffset);

            RevoluteJoint currentIDJoint = new RevoluteJoint(currentJoint.getName(), parentIDBody, jointOffset, jointAxis);
            currentIDJoint.setJointLimitLower(currentJoint.getJointLowerLimit());
            currentIDJoint.setJointLimitUpper(currentJoint.getJointUpperLimit());

            new RigidBody(link.getName(), currentIDJoint, momentOfInertia, mass, comOffset);

            scsToInverseDynamicsJointMap.addLinkedJoints(currentJoint, currentIDJoint);
         }
         else if (polledJoint instanceof SliderJoint)
         {
            SliderJoint currentJoint = (SliderJoint) polledJoint;

            Vector3D jointAxis = new Vector3D();
            currentJoint.getJointAxis(jointAxis);

            Vector3D jointOffset = new Vector3D();
            currentJoint.getOffset(jointOffset);

            PrismaticJoint currentIDJoint = new PrismaticJoint(currentJoint.getName(), parentIDBody, jointOffset, jointAxis);
            currentIDJoint.setJointLimitLower(currentJoint.getJointLowerLimit());
            currentIDJoint.setJointLimitUpper(currentJoint.getJointUpperLimit());

            new RigidBody(link.getName(), currentIDJoint, momentOfInertia, mass, comOffset);

            scsToInverseDynamicsJointMap.addLinkedJoints(currentJoint, currentIDJoint);
         }
         else
         {
            throw new RuntimeException();
         }

         jointQueue.addAll(polledJoint.getChildrenJoints());
      }

      elevator.updateFramesRecursively();
   }

   /**
    * Gets map from the simulated robot joints to the {@code InverseDynamicsJoint}s.
    * 
    * @return the map from simulated joint to {@code InverseDynamicsJoint}s.
    */
   public SCSToInverseDynamicsJointMap getSCSToInverseDynamicsJointMap()
   {
      return scsToInverseDynamicsJointMap;
   }

   /**
    * Gets the root body to which is attached the very first {@code InverseDynamicsJoint}.
    * 
    * @return the elevator.
    */
   public RigidBodyBasics getElevator()
   {
      return elevator;
   }

   private RigidBodyBasics getParentIDBody(Joint polledJoint, RigidBodyBasics elevator)
   {
      Joint parentJoint = polledJoint.getParentJoint();

      if (parentJoint == null)
         return elevator;

      return scsToInverseDynamicsJointMap.getRigidBody(parentJoint);
   }

   private final SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration();

   private final FrameVector3D linearVelocity = new FrameVector3D(ReferenceFrame.getWorldFrame());
   private final FrameVector3D angularVelocity = new FrameVector3D(ReferenceFrame.getWorldFrame());

   private final FrameVector3D originAcceleration = new FrameVector3D();
   private final FrameVector3D angularAcceleration = new FrameVector3D();

   private final RigidBodyTransform positionAndRotation = new RigidBodyTransform();

   /**
    * Update the configuration state of the {@code InverseDynamicsJoint}s using the state of the
    * simulated joints.
    * 
    * @param updateRootJoints whether the state of the root joints should be updated or not.
    */
   public void updateInverseDynamicsRobotModelFromRobot(boolean updateRootJoints)
   {
      // First update joint angles:
      Collection<OneDegreeOfFreedomJoint> pinJoints = scsToInverseDynamicsJointMap.getSCSOneDegreeOfFreedomJoints();
      for (OneDegreeOfFreedomJoint pinJoint : pinJoints)
      {
         if (updateRootJoints || (pinJoint.getParentJoint() != null))
         {
            OneDoFJointBasics revoluteJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(pinJoint);

            double jointPosition = pinJoint.getQYoVariable().getDoubleValue();
            double jointVelocity = pinJoint.getQDYoVariable().getDoubleValue();
            revoluteJoint.setQ(jointPosition);
            revoluteJoint.setQd(jointVelocity);
         }
      }

      if (updateRootJoints)
      {
         Collection<? extends FloatingJoint> floatingJoints = scsToInverseDynamicsJointMap.getFloatingJoints();
         for (FloatingJoint floatingJoint : floatingJoints)
         {
            FloatingJointBasics sixDoFJoint = scsToInverseDynamicsJointMap.getInverseDynamicsSixDoFJoint(floatingJoint);

            floatingJoint.getTransformToWorld(positionAndRotation);
            sixDoFJoint.setJointConfiguration(positionAndRotation);
         }
      }

      // Then update all the frames. They will be needed for further computation:
      elevator.updateFramesRecursively();

      if (updateRootJoints)
      {
         Collection<? extends FloatingJoint> floatingJoints = scsToInverseDynamicsJointMap.getFloatingJoints();
         for (FloatingJoint floatingJoint : floatingJoints)
         {
            FloatingJointBasics sixDoFJoint = scsToInverseDynamicsJointMap.getInverseDynamicsSixDoFJoint(floatingJoint);
            //                     referenceFrames.updateFrames();

            ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint();
            ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint();

            floatingJoint.getVelocity(linearVelocity);
            linearVelocity.changeFrame(pelvisFrame);

            floatingJoint.getAngularVelocity(angularVelocity, pelvisFrame);

            Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, angularVelocity, linearVelocity);
            sixDoFJoint.setJointTwist(bodyTwist);
            sixDoFJoint.updateFramesRecursively();
         }
      }
   }

   /**
    * Updates an the state of the simulated robot with the state from the Inverse Dynamics Model.
    *
    * @param updateRootJoints whether the root joints' state should also be updated.
    * @param updatePositions whether the joint positions should be copied over to the SCS robot or
    *           not.
    * @param updateVelocities whether the joint velocities should be copied over to the SCS robot or
    *           not.
    * @param updateAccelerations whether the joint accelerations should be copied over to the SCS
    *           robot or not.
    * @param updateTorques whether the joint forces/torques should be copied over to the SCS robot
    *           or not.
    */
   public void updateRobotFromInverseDynamicsRobotModel(boolean updateRootJoints, boolean updatePositions, boolean updateVelocities,
                                                        boolean updateAccelerations, boolean updateTorques)
   {

      Collection<OneDegreeOfFreedomJoint> pinJoints = scsToInverseDynamicsJointMap.getSCSOneDegreeOfFreedomJoints();
      for (OneDegreeOfFreedomJoint pinJoint : pinJoints)
      {
         if (updateRootJoints || (pinJoint.getParentJoint() != null))
         {
            OneDoFJointBasics revoluteJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(pinJoint);

            double jointPosition;
            double jointVelocity;
            double jointAcceleration;
            double jointTorque;

            if (updatePositions)
            {
               jointPosition = revoluteJoint.getQ();
               pinJoint.setQ(jointPosition);
            }

            if (updateAccelerations)
            {
               jointAcceleration = revoluteJoint.getQdd();
               pinJoint.setQdd(jointAcceleration);
            }

            if (updateVelocities)
            {
               jointVelocity = revoluteJoint.getQd();
               pinJoint.setQd(jointVelocity);
            }

            if (updateTorques)
            {
               jointTorque = revoluteJoint.getTau();
               pinJoint.setTau(jointTorque);
            }

         }
      }

      if (updateRootJoints)
      {
         Collection<? extends FloatingJoint> floatingJoints = scsToInverseDynamicsJointMap.getFloatingJoints(); //floatingToSixDofToJointMap.keySet();
         for (FloatingJoint floatingJoint : floatingJoints)
         {
            FloatingJointBasics sixDoFJoint = scsToInverseDynamicsJointMap.getInverseDynamicsSixDoFJoint(floatingJoint); //floatingToSixDofToJointMap.get(floatingJoint);
            RigidBodyTransform rotationAndTranslation = new RigidBodyTransform();
            if (updatePositions)
            {
               sixDoFJoint.getJointConfiguration(rotationAndTranslation);
               floatingJoint.setRotationAndTranslation(rotationAndTranslation);
            }

            ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint();

            if (updateVelocities)
            {
               linearVelocity.setIncludingFrame(pelvisFrame, sixDoFJoint.getJointTwist().getLinearPart());
               linearVelocity.changeFrame(ReferenceFrame.getWorldFrame());
               floatingJoint.setVelocity(linearVelocity);
               floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart());
            }

            if (updateAccelerations)
            {
               spatialAccelerationVector.setIncludingFrame(sixDoFJoint.getJointAcceleration());
               originAcceleration.setIncludingFrame(spatialAccelerationVector.getLinearPart());
               originAcceleration.changeFrame(ReferenceFrame.getWorldFrame());
               floatingJoint.setAcceleration(originAcceleration);
               angularAcceleration.setIncludingFrame(spatialAccelerationVector.getAngularPart());
               floatingJoint.setAngularAccelerationInBody(angularAcceleration);
            }
         }
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ReachingManifoldMessage.java<|end_filename|>
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;

/**
       * Message used to encode the manifold of an object to be used as input by a reaching motion planner.
       * Main usage is the IHMC WholeBodyTrajectoryToolbox.
       */
public class ReachingManifoldMessage extends Packet<ReachingManifoldMessage> implements Settable<ReachingManifoldMessage>, EpsilonComparable<ReachingManifoldMessage>
{
   public static final byte CONFIGURATION_SPACE_NAME_X = (byte) 0;
   public static final byte CONFIGURATION_SPACE_NAME_Y = (byte) 1;
   public static final byte CONFIGURATION_SPACE_NAME_Z = (byte) 2;
   public static final byte CONFIGURATION_SPACE_NAME_ROLL = (byte) 3;
   public static final byte CONFIGURATION_SPACE_NAME_PITCH = (byte) 4;
   public static final byte CONFIGURATION_SPACE_NAME_YAW = (byte) 5;
   public static final byte CONFIGURATION_SPACE_NAME_SO3 = (byte) 6;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int end_effector_hash_code_;
   public us.ihmc.euclid.tuple3D.Point3D manifold_origin_position_;
   public us.ihmc.euclid.tuple4D.Quaternion manifold_origin_orientation_;
   public us.ihmc.idl.IDLSequence.Byte  manifold_configuration_space_names_;
   public us.ihmc.idl.IDLSequence.Double  manifold_lower_limits_;
   public us.ihmc.idl.IDLSequence.Double  manifold_upper_limits_;

   public ReachingManifoldMessage()
   {
      manifold_origin_position_ = new us.ihmc.euclid.tuple3D.Point3D();
      manifold_origin_orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
      manifold_configuration_space_names_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9");

      manifold_lower_limits_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      manifold_upper_limits_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

   }

   public ReachingManifoldMessage(ReachingManifoldMessage other)
   {
      this();
      set(other);
   }

   public void set(ReachingManifoldMessage other)
   {
      sequence_id_ = other.sequence_id_;

      end_effector_hash_code_ = other.end_effector_hash_code_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.manifold_origin_position_, manifold_origin_position_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.manifold_origin_orientation_, manifold_origin_orientation_);
      manifold_configuration_space_names_.set(other.manifold_configuration_space_names_);
      manifold_lower_limits_.set(other.manifold_lower_limits_);
      manifold_upper_limits_.set(other.manifold_upper_limits_);
   }

   /**
            * 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 void setEndEffectorHashCode(int end_effector_hash_code)
   {
      end_effector_hash_code_ = end_effector_hash_code;
   }
   public int getEndEffectorHashCode()
   {
      return end_effector_hash_code_;
   }


   public us.ihmc.euclid.tuple3D.Point3D getManifoldOriginPosition()
   {
      return manifold_origin_position_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getManifoldOriginOrientation()
   {
      return manifold_origin_orientation_;
   }


   public us.ihmc.idl.IDLSequence.Byte  getManifoldConfigurationSpaceNames()
   {
      return manifold_configuration_space_names_;
   }


   public us.ihmc.idl.IDLSequence.Double  getManifoldLowerLimits()
   {
      return manifold_lower_limits_;
   }


   public us.ihmc.idl.IDLSequence.Double  getManifoldUpperLimits()
   {
      return manifold_upper_limits_;
   }


   public static Supplier<ReachingManifoldMessagePubSubType> getPubSubType()
   {
      return ReachingManifoldMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ReachingManifoldMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ReachingManifoldMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.end_effector_hash_code_, other.end_effector_hash_code_, epsilon)) return false;

      if (!this.manifold_origin_position_.epsilonEquals(other.manifold_origin_position_, epsilon)) return false;
      if (!this.manifold_origin_orientation_.epsilonEquals(other.manifold_origin_orientation_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.manifold_configuration_space_names_, other.manifold_configuration_space_names_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.manifold_lower_limits_, other.manifold_lower_limits_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.manifold_upper_limits_, other.manifold_upper_limits_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ReachingManifoldMessage)) return false;

      ReachingManifoldMessage otherMyClass = (ReachingManifoldMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.end_effector_hash_code_ != otherMyClass.end_effector_hash_code_) return false;

      if (!this.manifold_origin_position_.equals(otherMyClass.manifold_origin_position_)) return false;
      if (!this.manifold_origin_orientation_.equals(otherMyClass.manifold_origin_orientation_)) return false;
      if (!this.manifold_configuration_space_names_.equals(otherMyClass.manifold_configuration_space_names_)) return false;
      if (!this.manifold_lower_limits_.equals(otherMyClass.manifold_lower_limits_)) return false;
      if (!this.manifold_upper_limits_.equals(otherMyClass.manifold_upper_limits_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ReachingManifoldMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("end_effector_hash_code=");
      builder.append(this.end_effector_hash_code_);      builder.append(", ");
      builder.append("manifold_origin_position=");
      builder.append(this.manifold_origin_position_);      builder.append(", ");
      builder.append("manifold_origin_orientation=");
      builder.append(this.manifold_origin_orientation_);      builder.append(", ");
      builder.append("manifold_configuration_space_names=");
      builder.append(this.manifold_configuration_space_names_);      builder.append(", ");
      builder.append("manifold_lower_limits=");
      builder.append(this.manifold_lower_limits_);      builder.append(", ");
      builder.append("manifold_upper_limits=");
      builder.append(this.manifold_upper_limits_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ManipulationAbortedStatus.java<|end_filename|>
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 sent by the controller to notify the user that the current manipulation task has been aborted.
       */
public class ManipulationAbortedStatus extends Packet<ManipulationAbortedStatus> implements Settable<ManipulationAbortedStatus>, EpsilonComparable<ManipulationAbortedStatus>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;

   public ManipulationAbortedStatus()
   {
   }

   public ManipulationAbortedStatus(ManipulationAbortedStatus other)
   {
      this();
      set(other);
   }

   public void set(ManipulationAbortedStatus 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<ManipulationAbortedStatusPubSubType> getPubSubType()
   {
      return ManipulationAbortedStatusPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ManipulationAbortedStatusPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ManipulationAbortedStatus 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 ManipulationAbortedStatus)) return false;

      ManipulationAbortedStatus otherMyClass = (ManipulationAbortedStatus) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ManipulationAbortedStatus {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-mocap/src/main/java/optiTrack/MocapFrameDataPacket.java<|end_filename|>
package optiTrack;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;

public class MocapFrameDataPacket
{
   public short messageID;
   public short payloadSize;
   public int frameNumber;
   public int numberOfDataSets;
   public int numberOfRigidBodies;
   public int rigidBodyId;
   public float posX;
   public float posY;
   public float posZ;
   public float rotX;
   public float rotY;
   public float rotZ;
   public float rotW;
   public int nRigidMarkers;

   public int numberOfUnidentifiedMarkers;
   public float uMarkerX;
   public float uMarkerY;
   public float uMarkerZ;

   public Vector3D[] markerPosition;
   public int[] markerIds;
   public float[] markerSizes;

   public int numberOfSkeletons;
   public float latency;
   public int numberOfLabeledMarkers;

   public float meanMarkerError;

   public boolean isTracked = true;

   public int type = 0;

   public int numberOfMarkers;

   private static MocapRigidBody rigidBody;

   private static boolean DEBUG = false;

   private static ArrayList<MocapRigidBody> listfOfRigidbodies;

   public static ArrayList<MocapRigidBody> createFromBytes(byte[] bytes) throws IOException
   {
      listfOfRigidbodies = new ArrayList<>();

      final MocapFrameDataPacket data = new MocapFrameDataPacket();
      final ByteBuffer buf = ByteBuffer.wrap(bytes);

      buf.order(ByteOrder.LITTLE_ENDIAN);

      data.messageID = buf.getShort();
      data.payloadSize = buf.getShort();

      if (DEBUG)
      {
         System.out.println(">>>>>>>>>>>>>>>>>>>>>>>>>START MESSAGE");
         System.out.println("MOCAP Info:");
         System.out.println("Mssg ID: " + data.messageID);
         System.out.println("Mssg Payload Size: " + data.payloadSize);
      }

      if (data.messageID == 4)
      {
         System.out.println("Description Message");
         data.numberOfDataSets = buf.getInt();

         System.out.println("# of datasets: " + data.numberOfDataSets);

         for (int i = 0; i <= data.numberOfDataSets; i++)
         {
            System.out.println("Desc. Type: " + buf.getInt());
         }

         data.type = buf.getInt();

         System.out.println("Type: " + data.type);

      }

      if (data.messageID == 7)
      {
         data.frameNumber = buf.getInt();
         data.numberOfDataSets = buf.getInt();

         if (DEBUG)
         {
            System.out.println("Message type: FrameData Message");
            System.out.println("Frame #: " + data.frameNumber);
            System.out.println("# of datasets: " + data.numberOfDataSets);
            System.out.println("# of unidentified markers: " + data.numberOfUnidentifiedMarkers);
            System.out.println("# of rigid bodies: " + data.numberOfRigidBodies);
         }

         for (int i = 0; i < data.numberOfDataSets; i++)
         {
         }

         data.numberOfUnidentifiedMarkers = buf.getInt();

         for (int i = 0; i < data.numberOfUnidentifiedMarkers; i++)
         {
            data.uMarkerX = buf.getFloat();
            data.uMarkerY = buf.getFloat();
            data.uMarkerZ = buf.getFloat();
         }

         data.numberOfRigidBodies = buf.getInt();

         for (int i = 0; i < data.numberOfRigidBodies; i++)
         {
            data.rigidBodyId = buf.getInt();
            data.posX = buf.getFloat();
            data.posY = buf.getFloat();
            data.posZ = buf.getFloat();
            data.rotX = buf.getFloat();
            data.rotY = buf.getFloat();
            data.rotZ = buf.getFloat();
            data.rotW = buf.getFloat();
            data.nRigidMarkers = buf.getInt();

            data.markerPosition = new Vector3D[data.nRigidMarkers];


            for (int j = 0; j < data.nRigidMarkers; j++)
            {
               data.markerPosition[j] = new Vector3D(buf.getFloat(), buf.getFloat(), buf.getFloat());
            }

            data.markerIds = new int[data.nRigidMarkers];

            for (int j = 0; j < data.nRigidMarkers; j++)
            {
               data.markerIds[j] = buf.getInt();
            }

            data.markerSizes = new float[data.nRigidMarkers];

            for (int j = 0; j < data.nRigidMarkers; j++)
            {
               data.markerSizes[j] = buf.getFloat();
            }

            short params = 10;
            params = buf.getShort();

            data.isTracked = params != 0;

            data.meanMarkerError = buf.getFloat();

            ArrayList<MocapMarker> listfOfMarkersForThisRB = new ArrayList<>();

            for (int j = 0; j < data.nRigidMarkers; j++)
            {
               MocapMarker mocapMarker = new MocapMarker(data.markerIds[j], data.markerPosition[j], data.markerSizes[j]);
               listfOfMarkersForThisRB.add(mocapMarker);
            }

            rigidBody = new MocapRigidBody(data.rigidBodyId, new Vector3D(data.posX, data.posY, data.posZ),
                                           new Quaternion(data.rotX, data.rotY, data.rotZ, data.rotW), listfOfMarkersForThisRB, data.isTracked);

            listfOfRigidbodies.add(rigidBody);

            if (DEBUG)
            {
               System.out.println("\nInformation for rigid body " + i + ": ");
               rigidBody.toString();

//             System.out.println("Tracking: " + (params & 0x01));

            }
         }

         data.numberOfSkeletons = buf.getInt();
         data.numberOfLabeledMarkers = buf.getInt();
         data.latency = buf.getFloat();

         if (DEBUG)
         {
            System.out.println("# of Skeletons: " + data.numberOfSkeletons);
            System.out.println("Labeled Markers: " + data.numberOfLabeledMarkers);
            System.out.println("Latency: " + data.latency);

            System.out.println("<<<<<<<<<<<<<<<<<<<<<<END MESSAGE");
         }
      }

      return listfOfRigidbodies;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlMode.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controlModules.rigidBody;

public enum RigidBodyControlMode
{
   JOINTSPACE,
   TASKSPACE,
   USER,
   LOADBEARING,
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "WholeBodyTrajectoryToolboxConfigurationMessage" defined in "WholeBodyTrajectoryToolboxConfigurationMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from WholeBodyTrajectoryToolboxConfigurationMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit WholeBodyTrajectoryToolboxConfigurationMessage_.idl instead.
*
*/
public class WholeBodyTrajectoryToolboxConfigurationMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::WholeBodyTrajectoryToolboxConfigurationMessage_";

   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.WholeBodyTrajectoryToolboxConfigurationMessage 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.WholeBodyTrajectoryToolboxConfigurationMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType.getCdrSerializedSize(data.getInitialConfiguration(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_2(data.getNumberOfInitialGuesses());

      cdr.write_type_2(data.getMaximumExpansionSize());

      controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType.write(data.getInitialConfiguration(), cdr);
   }

   public static void read(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setNumberOfInitialGuesses(cdr.read_type_2());
      	
      data.setMaximumExpansionSize(cdr.read_type_2());
      	
      controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType.read(data.getInitialConfiguration(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_2("number_of_initial_guesses", data.getNumberOfInitialGuesses());
      ser.write_type_2("maximum_expansion_size", data.getMaximumExpansionSize());
      ser.write_type_a("initial_configuration", new controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType(), data.getInitialConfiguration());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setNumberOfInitialGuesses(ser.read_type_2("number_of_initial_guesses"));
      data.setMaximumExpansionSize(ser.read_type_2("maximum_expansion_size"));
      ser.read_type_a("initial_configuration", new controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType(), data.getInitialConfiguration());

   }

   public static void staticCopy(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage src, controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage createData()
   {
      return new controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage();
   }
   @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.WholeBodyTrajectoryToolboxConfigurationMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage src, controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public WholeBodyTrajectoryToolboxConfigurationMessagePubSubType newInstance()
   {
      return new WholeBodyTrajectoryToolboxConfigurationMessagePubSubType();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WalkOverTerrainGoalPacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class WalkOverTerrainGoalPacket extends Packet<WalkOverTerrainGoalPacket> implements Settable<WalkOverTerrainGoalPacket>, EpsilonComparable<WalkOverTerrainGoalPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.tuple3D.Point3D position_;
   public us.ihmc.euclid.tuple4D.Quaternion orientation_;

   public WalkOverTerrainGoalPacket()
   {
      position_ = new us.ihmc.euclid.tuple3D.Point3D();
      orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
   }

   public WalkOverTerrainGoalPacket(WalkOverTerrainGoalPacket other)
   {
      this();
      set(other);
   }

   public void set(WalkOverTerrainGoalPacket other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.position_, position_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.orientation_, orientation_);
   }

   /**
            * 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 us.ihmc.euclid.tuple3D.Point3D getPosition()
   {
      return position_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getOrientation()
   {
      return orientation_;
   }


   public static Supplier<WalkOverTerrainGoalPacketPubSubType> getPubSubType()
   {
      return WalkOverTerrainGoalPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WalkOverTerrainGoalPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WalkOverTerrainGoalPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.position_.epsilonEquals(other.position_, epsilon)) return false;
      if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WalkOverTerrainGoalPacket)) return false;

      WalkOverTerrainGoalPacket otherMyClass = (WalkOverTerrainGoalPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.position_.equals(otherMyClass.position_)) return false;
      if (!this.orientation_.equals(otherMyClass.orientation_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WalkOverTerrainGoalPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("position=");
      builder.append(this.position_);      builder.append(", ");
      builder.append("orientation=");
      builder.append(this.orientation_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/FisheyePacket.java<|end_filename|>
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;

public class FisheyePacket extends Packet<FisheyePacket> implements Settable<FisheyePacket>, EpsilonComparable<FisheyePacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public controller_msgs.msg.dds.VideoPacket video_packet_;

   public FisheyePacket()
   {
      video_packet_ = new controller_msgs.msg.dds.VideoPacket();
   }

   public FisheyePacket(FisheyePacket other)
   {
      this();
      set(other);
   }

   public void set(FisheyePacket other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.VideoPacketPubSubType.staticCopy(other.video_packet_, video_packet_);
   }

   /**
            * 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 controller_msgs.msg.dds.VideoPacket getVideoPacket()
   {
      return video_packet_;
   }


   public static Supplier<FisheyePacketPubSubType> getPubSubType()
   {
      return FisheyePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return FisheyePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(FisheyePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.video_packet_.epsilonEquals(other.video_packet_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof FisheyePacket)) return false;

      FisheyePacket otherMyClass = (FisheyePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.video_packet_.equals(otherMyClass.video_packet_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("FisheyePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("video_packet=");
      builder.append(this.video_packet_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/initialSetup/SquaredUpDRCDemo01Robot.java<|end_filename|>
package us.ihmc.avatar.initialSetup;

import us.ihmc.euclid.tuple3D.Vector3D;

public class SquaredUpDRCDemo01Robot extends OffsetAndYawRobotInitialSetup
{
	   public SquaredUpDRCDemo01Robot(double offsetX, double offsetY)
	   {
	      super(new Vector3D(offsetX, offsetY, 0.0), Math.atan2(offsetY, offsetX));
	   }
	   public SquaredUpDRCDemo01Robot(double r, double theta, double psi)
	   {
		  super(new Vector3D(r*Math.cos(theta), r*Math.sin(theta), 0.0), theta+psi);
	   }
	   public SquaredUpDRCDemo01Robot(double z, double r, double theta, double psi)
	   {
		  super(z,new Vector3D(r*Math.cos(theta), r*Math.sin(theta), 0.0), theta+psi);
	   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepSnapper/FootstepValueFunction.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepSnapper;

import controller_msgs.msg.dds.FootstepDataMessage;

/**
 * Created by agrabertilton on 1/19/15.
 */
public interface FootstepValueFunction
{
   public double getFootstepValue(FootstepDataMessage footstepData);
   public void updateFunction();
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/ros/AtlasOrderedJointMap.java<|end_filename|>
package us.ihmc.atlas.ros;

import java.util.LinkedHashMap;
import java.util.Map;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class AtlasOrderedJointMap
{
   /*
    * Copied from
    * https://bitbucket.org/osrf/drcsim/raw/default/ros/atlas_msgs/msg
    * /AtlasState.msg
    */

   public final static int back_bkz = 0;
   public final static int back_bky = 1;
   public final static int back_bkx = 2;
   public final static int neck_ry = 3;
   public final static int l_leg_hpz = 4;
   public final static int l_leg_hpx = 5;
   public final static int l_leg_hpy = 6;
   public final static int l_leg_kny = 7;
   public final static int l_leg_aky = 8;
   public final static int l_leg_akx = 9;
   public final static int r_leg_hpz = 10;
   public final static int r_leg_hpx = 11;
   public final static int r_leg_hpy = 12;
   public final static int r_leg_kny = 13;
   public final static int r_leg_aky = 14;
   public final static int r_leg_akx = 15;
   public final static int l_arm_shz = 16;
   public final static int l_arm_shx = 17;
   public final static int l_arm_ely = 18;
   public final static int l_arm_elx = 19;
   public final static int l_arm_wry = 20;
   public final static int l_arm_wrx = 21;
   public final static int l_arm_wry2 = 22;
   public final static int r_arm_shz = 23;
   public final static int r_arm_shx = 24;
   public final static int r_arm_ely = 25;
   public final static int r_arm_elx = 26;
   public final static int r_arm_wry = 27;
   public final static int r_arm_wrx = 28;
   public final static int r_arm_wry2 = 29;

   public final static int numberOfJoints = r_arm_wry2 + 1;

   public static final String[] jointNames = new String[numberOfJoints];
   static
   {
      jointNames[back_bkz] = "back_bkz";
      jointNames[back_bky] = "back_bky";
      jointNames[back_bkx] = "back_bkx";
      jointNames[neck_ry] = "neck_ry";
      jointNames[l_leg_hpz] = "l_leg_hpz";
      jointNames[l_leg_hpx] = "l_leg_hpx";
      jointNames[l_leg_hpy] = "l_leg_hpy";
      jointNames[l_leg_kny] = "l_leg_kny";
      jointNames[l_leg_aky] = "l_leg_aky";
      jointNames[l_leg_akx] = "l_leg_akx";
      jointNames[r_leg_hpz] = "r_leg_hpz";
      jointNames[r_leg_hpx] = "r_leg_hpx";
      jointNames[r_leg_hpy] = "r_leg_hpy";
      jointNames[r_leg_kny] = "r_leg_kny";
      jointNames[r_leg_aky] = "r_leg_aky";
      jointNames[r_leg_akx] = "r_leg_akx";
      jointNames[l_arm_shz] = "l_arm_shz";
      jointNames[l_arm_shx] = "l_arm_shx";
      jointNames[l_arm_ely] = "l_arm_ely";
      jointNames[l_arm_elx] = "l_arm_elx";
      jointNames[l_arm_wry] = "l_arm_wry";
      jointNames[l_arm_wrx] = "l_arm_wrx";
      jointNames[l_arm_wry2] = "l_arm_wry2";
      jointNames[r_arm_shz] = "r_arm_shz";
      jointNames[r_arm_shx] = "r_arm_shx";
      jointNames[r_arm_ely] = "r_arm_ely";
      jointNames[r_arm_elx] = "r_arm_elx";
      jointNames[r_arm_wry] = "r_arm_wry";
      jointNames[r_arm_wrx] = "r_arm_wrx";
      jointNames[r_arm_wry2] = "r_arm_wry2";
   }

   public static final LinkedHashMap<String, Integer> nameToIndexMap = new LinkedHashMap<String, Integer>();
   static
   {
      for (int i = 0; i < jointNames.length; i++)
      {
         nameToIndexMap.put(jointNames[i], i);
      }
   }

   public static final SideDependentList<String[]> forcedSideDependentJointNames = new SideDependentList<String[]>();
   static
   {
      String[] jointNamesRight = new String[numberOfJoints];
      jointNamesRight[back_bkz] = jointNames[back_bkz];
      jointNamesRight[back_bky] = jointNames[back_bky];
      jointNamesRight[back_bkx] = jointNames[back_bkx];
      jointNamesRight[neck_ry] = jointNames[neck_ry];
      jointNamesRight[l_leg_hpz] = jointNames[r_leg_hpz];
      jointNamesRight[l_leg_hpx] = jointNames[r_leg_hpx];
      jointNamesRight[l_leg_hpy] = jointNames[r_leg_hpy];
      jointNamesRight[l_leg_kny] = jointNames[r_leg_kny];
      jointNamesRight[l_leg_aky] = jointNames[r_leg_aky];
      jointNamesRight[l_leg_akx] = jointNames[r_leg_akx];
      jointNamesRight[r_leg_hpz] = jointNames[r_leg_hpz];
      jointNamesRight[r_leg_hpx] = jointNames[r_leg_hpx];
      jointNamesRight[r_leg_hpy] = jointNames[r_leg_hpy];
      jointNamesRight[r_leg_kny] = jointNames[r_leg_kny];
      jointNamesRight[r_leg_aky] = jointNames[r_leg_aky];
      jointNamesRight[r_leg_akx] = jointNames[r_leg_akx];
      jointNamesRight[l_arm_shz] = jointNames[r_arm_shz];
      jointNamesRight[l_arm_shx] = jointNames[r_arm_shx];
      jointNamesRight[l_arm_ely] = jointNames[r_arm_ely];
      jointNamesRight[l_arm_elx] = jointNames[r_arm_elx];
      jointNamesRight[l_arm_wry] = jointNames[r_arm_wry];
      jointNamesRight[l_arm_wrx] = jointNames[r_arm_wrx];
      jointNamesRight[l_arm_wry2] = jointNames[r_arm_wry2];
      jointNamesRight[r_arm_shz] = jointNames[r_arm_shz];
      jointNamesRight[r_arm_shx] = jointNames[r_arm_shx];
      jointNamesRight[r_arm_ely] = jointNames[r_arm_ely];
      jointNamesRight[r_arm_elx] = jointNames[r_arm_elx];
      jointNamesRight[r_arm_wry] = jointNames[r_arm_wry];
      jointNamesRight[r_arm_wrx] = jointNames[r_arm_wrx];
      jointNamesRight[r_arm_wry2] = jointNames[r_arm_wry2];

      forcedSideDependentJointNames.put(RobotSide.RIGHT, jointNamesRight);

      String[] jointNamesLeft = new String[numberOfJoints];
      jointNamesLeft[back_bkz] = jointNames[back_bkz];
      jointNamesLeft[back_bky] = jointNames[back_bky];
      jointNamesLeft[back_bkx] = jointNames[back_bkx];
      jointNamesLeft[neck_ry] = jointNames[neck_ry];
      jointNamesLeft[l_leg_hpz] = jointNames[l_leg_hpz];
      jointNamesLeft[l_leg_hpx] = jointNames[l_leg_hpx];
      jointNamesLeft[l_leg_hpy] = jointNames[l_leg_hpy];
      jointNamesLeft[l_leg_kny] = jointNames[l_leg_kny];
      jointNamesLeft[l_leg_aky] = jointNames[l_leg_aky];
      jointNamesLeft[l_leg_akx] = jointNames[l_leg_akx];
      jointNamesLeft[r_leg_hpz] = jointNames[l_leg_hpz];
      jointNamesLeft[r_leg_hpx] = jointNames[l_leg_hpx];
      jointNamesLeft[r_leg_hpy] = jointNames[l_leg_hpy];
      jointNamesLeft[r_leg_kny] = jointNames[l_leg_kny];
      jointNamesLeft[r_leg_aky] = jointNames[l_leg_aky];
      jointNamesLeft[r_leg_akx] = jointNames[l_leg_akx];
      jointNamesLeft[l_arm_shz] = jointNames[l_arm_shz];
      jointNamesLeft[l_arm_shx] = jointNames[l_arm_shx];
      jointNamesLeft[l_arm_ely] = jointNames[l_arm_ely];
      jointNamesLeft[l_arm_elx] = jointNames[l_arm_elx];
      jointNamesLeft[l_arm_wry] = jointNames[l_arm_wry];
      jointNamesLeft[l_arm_wrx] = jointNames[l_arm_wrx];
      jointNamesLeft[l_arm_wry2] = jointNames[l_arm_wry2];
      jointNamesLeft[r_arm_shz] = jointNames[l_arm_shz];
      jointNamesLeft[r_arm_shx] = jointNames[l_arm_shx];
      jointNamesLeft[r_arm_ely] = jointNames[l_arm_ely];
      jointNamesLeft[r_arm_elx] = jointNames[l_arm_elx];
      jointNamesLeft[r_arm_wry] = jointNames[l_arm_wry];
      jointNamesLeft[r_arm_wrx] = jointNames[l_arm_wrx];
      jointNamesLeft[r_arm_wry2] = jointNames[l_arm_wry2];

      forcedSideDependentJointNames.put(RobotSide.LEFT, jointNamesLeft);
   }

   public static OneDoFJointBasics[] getJointMap(Map<String, OneDoFJointBasics> jointsByName)
   {
      OneDoFJointBasics[] joints = new OneDoFJointBasics[numberOfJoints];
      for (int i = 0; i < numberOfJoints; i++)
      {
         joints[i] = jointsByName.get(jointNames[i]);
      }

      return joints;
   }

   public static String getLeftFootForceSensorName()
   {
      return jointNames[l_leg_akx];
   }

   public static String getRightFootForceSensorName()
   {
      return jointNames[r_leg_akx];
   }

   public static String getLeftHandForceSensorName()
   {
      return jointNames[l_arm_wry2];
   }

   public static String getRightHandForceSensorName()
   {
      return jointNames[r_arm_wry2];
   }

   public static String getHokuyoJointName()
   {
      return "hokuyo_joint";
   }

   public static String[] getForceSensors()
   {
      return new String[] { getLeftFootForceSensorName(), getRightFootForceSensorName(), getLeftHandForceSensorName(), getRightHandForceSensorName() };
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/JoystickCustomizationFilter.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick;

import net.java.games.input.Component.Identifier;
import us.ihmc.tools.inputDevices.joystick.mapping.JoystickMapping;

public class JoystickCustomizationFilter implements JoystickComponentFilter
{
   private final JoystickMapping mapping;
   
   private final boolean invert;
   private final int exponent;
   private final double restToZeroCorrection;
   
   /** The percentage about zero within which the input value should be ignored. */
   private final double deadzone;

   public JoystickCustomizationFilter(JoystickMapping channel, boolean invert)
   {
      this(channel, invert, 0.0);
   }

   public JoystickCustomizationFilter(JoystickMapping channel, boolean invert, double deadzone)
   {
      this(channel, invert, deadzone, 1);
   }

   public JoystickCustomizationFilter(JoystickMapping channel, boolean invert, double deadzone, int exponent)
   {
      this(channel, invert, deadzone, exponent, 0.0);
   }
   
   public JoystickCustomizationFilter(JoystickMapping channel, boolean invert, double deadzone, int exponent, double restToZeroCorrection)
   {
      if (exponent % 2 == 0)
      {
         throw new IllegalArgumentException("Only odd functions are allowed");
      }

      this.mapping = channel;
      this.invert = invert;
      this.deadzone = deadzone;
      this.exponent = exponent;
      this.restToZeroCorrection = restToZeroCorrection;
   }

   @Override
   public double apply(double value)
   {
      value += restToZeroCorrection;
      
      // If the value is within the deadzone, ignore it. Otherwise, offset the value to the deadzone limit and rescale
      // it to full scale.
      if (Math.abs(value) < deadzone)
      {
         value = 0.0;
      }
      else
      {
         value = (value - (Math.signum(value) * deadzone)) / (1.0 - deadzone);
      }

      return Math.pow((invert ? -1 : 1) * value, exponent);
   }

   @Override
   public Identifier getIdentifier()
   {
      return mapping.getIdentifier();
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/ros/RosAtlasAuxiliaryRobotDataPublisher.java<|end_filename|>
package us.ihmc.atlas.ros;

import java.net.URI;
import java.util.LinkedHashMap;
import java.util.concurrent.ArrayBlockingQueue;

import controller_msgs.msg.dds.AtlasAuxiliaryRobotData;
import us.ihmc.humanoidRobotics.communication.packets.manipulation.AtlasElectricMotorPacketEnum;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.publisher.RosBoolPublisher;
import us.ihmc.utilities.ros.publisher.RosDoublePublisher;
import us.ihmc.utilities.ros.publisher.RosInt64Publisher;

/**
 *
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class RosAtlasAuxiliaryRobotDataPublisher implements Runnable
{
   private final ArrayBlockingQueue<AtlasAuxiliaryRobotData> availableAtlasAuxiliaryData = new ArrayBlockingQueue<>(30);
   private final RosMainNode rosMainNode;

   private final LinkedHashMap<AtlasElectricMotorPacketEnum, RosBoolPublisher> electricForearmEnabledPublishers = new LinkedHashMap<>();
   private final LinkedHashMap<AtlasElectricMotorPacketEnum, RosDoublePublisher> electricForearmTemperaturePublishers = new LinkedHashMap<>();
   private final LinkedHashMap<AtlasElectricMotorPacketEnum, RosDoublePublisher> electricForearmCurrentPublishers = new LinkedHashMap<>();

   private final RosDoublePublisher pumpInletPressurePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher pumpSupplyPressurePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher airSumpPressurePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher pumpSupplyTemperaturePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher pumpRPMPublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher motorTemperaturePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher motorDriverTemperaturePublisher = new RosDoublePublisher(false);

   private final RosBoolPublisher batteryChargingPublisher = new RosBoolPublisher(false);
   private final RosDoublePublisher batteryVoltagePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher batteryCurrentPublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher remainingBatteryTimePublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher remainingAmpHoursPublisher = new RosDoublePublisher(false);
   private final RosDoublePublisher remainingChargePercentagePublisher = new RosDoublePublisher(false);
   private final RosInt64Publisher cycleCountPublisher = new RosInt64Publisher(false);

   public RosAtlasAuxiliaryRobotDataPublisher(RosMainNode rosMainNode, String rosNameSpace)
   {
      this.rosMainNode = rosMainNode;
      initialize(rosMainNode, rosNameSpace);
   }

   public RosAtlasAuxiliaryRobotDataPublisher(URI rosuri, String nameSpace)
   {
      this.rosMainNode = new RosMainNode(rosuri, nameSpace + getClass().getName());
      initialize(rosMainNode, nameSpace);
   }

   private void initialize(RosMainNode rosMainNode, String rosNameSpace)
   {
      setupElectricForearmPublishers(rosNameSpace);
      setupPumpPublishers(rosNameSpace);
      setupBatteryPublishers(rosMainNode, rosNameSpace);

      Thread thread = new Thread(this, getClass().getName());
      thread.start();
   }

   private void setupBatteryPublishers(RosMainNode rosMainNode, String rosNameSpace)
   {
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/charging", batteryChargingPublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/voltage", batteryVoltagePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/current", batteryCurrentPublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/time_remaining", remainingBatteryTimePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/amp_hours_remaining", remainingAmpHoursPublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/charge_percentage_remaining", remainingChargePercentagePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/battery/cycle_count", cycleCountPublisher);
   }

   private void setupPumpPublishers(String rosNameSpace)
   {
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/pump_inlet_pressure", pumpInletPressurePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/pump_supply_pressure", pumpSupplyPressurePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/air_sump_pressure", airSumpPressurePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/pump_supply_temperature", pumpSupplyTemperaturePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/pump_rpm", pumpRPMPublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/motor_temperature", motorTemperaturePublisher);
      rosMainNode.attachPublisher(rosNameSpace + "/output/pump/motor_driver_temperature", motorDriverTemperaturePublisher);
   }

   private void setupElectricForearmPublishers(String rosNameSpace)
   {
      for (AtlasElectricMotorPacketEnum value : AtlasElectricMotorPacketEnum.values)
      {
         RosBoolPublisher boolPublisher = new RosBoolPublisher(false);
         electricForearmEnabledPublishers.put(value, boolPublisher);
         rosMainNode.attachPublisher(rosNameSpace + "/output/electric_forearms/" + value.toString().toLowerCase() + "/enabled", boolPublisher);

         RosDoublePublisher tempPublisher = new RosDoublePublisher(false);
         electricForearmTemperaturePublishers.put(value, tempPublisher);
         rosMainNode.attachPublisher(rosNameSpace + "/output/electric_forearms/" + value.toString().toLowerCase() + "/temperature", tempPublisher);

         RosDoublePublisher currentPublisher = new RosDoublePublisher(false);
         electricForearmCurrentPublishers.put(value, currentPublisher);
         rosMainNode.attachPublisher(rosNameSpace + "/output/electric_forearms/" + value.toString().toLowerCase() + "/drive_current", currentPublisher);
      }
   }

   @Override
   public void run()
   {
      while (true)
      {
         AtlasAuxiliaryRobotData auxiliaryRobotData;
         try
         {
            auxiliaryRobotData = availableAtlasAuxiliaryData.take();
         }
         catch (InterruptedException e)
         {
            // Continue on
            continue;
         }
         if (rosMainNode.isStarted())
         {
            publishElectricForearmData(auxiliaryRobotData);

            publishPumpData(auxiliaryRobotData);

            publishBatteryData(auxiliaryRobotData);
         }
      }
   }

   private void publishElectricForearmData(AtlasAuxiliaryRobotData auxiliaryRobotData)
   {
      for (AtlasElectricMotorPacketEnum value : AtlasElectricMotorPacketEnum.values)
      {
         RosBoolPublisher rosBoolPublisher = electricForearmEnabledPublishers.get(value);
         rosBoolPublisher.publish(auxiliaryRobotData.getElectricJointEnabledArray().get(value.getId()) == 1);

         RosDoublePublisher temperaturePublisher = electricForearmTemperaturePublishers.get(value);
         temperaturePublisher.publish(auxiliaryRobotData.getElectricJointTemperatures().get(value.getId()));

         RosDoublePublisher currentPublisher = electricForearmCurrentPublishers.get(value);
         currentPublisher.publish(auxiliaryRobotData.getElectricJointCurrents().get(value.getId()));
      }
   }

   private void publishPumpData(AtlasAuxiliaryRobotData auxiliaryRobotData)
   {
      pumpInletPressurePublisher.publish(auxiliaryRobotData.getPumpInletPressure());
      pumpSupplyPressurePublisher.publish(auxiliaryRobotData.getPumpSupplyPressure());
      airSumpPressurePublisher.publish(auxiliaryRobotData.getAirSumpPressure());
      pumpSupplyTemperaturePublisher.publish(auxiliaryRobotData.getPumpSupplyTemperature());
      pumpRPMPublisher.publish(auxiliaryRobotData.getPumpRpm());
      motorTemperaturePublisher.publish(auxiliaryRobotData.getMotorTemperature());
      motorDriverTemperaturePublisher.publish(auxiliaryRobotData.getMotorDriverTemperature());
   }

   private void publishBatteryData(AtlasAuxiliaryRobotData auxiliaryRobotData)
   {
      batteryChargingPublisher.publish(auxiliaryRobotData.getBatteryCharging());
      batteryVoltagePublisher.publish(auxiliaryRobotData.getBatteryVoltage());
      batteryCurrentPublisher.publish(auxiliaryRobotData.getBatteryCurrent());
      remainingBatteryTimePublisher.publish(auxiliaryRobotData.getRemainingBatteryTime());
      remainingAmpHoursPublisher.publish(auxiliaryRobotData.getRemainingAmpHours());
      remainingChargePercentagePublisher.publish(auxiliaryRobotData.getRemainingChargePercentage());
      cycleCountPublisher.publish(auxiliaryRobotData.getBatteryCycleCount());
   }

   public void receivedPacket(AtlasAuxiliaryRobotData auxiliaryRobotData)
   {
      if (!availableAtlasAuxiliaryData.offer(auxiliaryRobotData))
      {
         availableAtlasAuxiliaryData.clear();
      }
   }
}


<|start_filename|>ihmc-mocap/src/main/java/us/ihmc/vicon/ViconJavaInterface.java<|end_filename|>
package us.ihmc.vicon;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.tuple3D.Point3D;

public class ViconJavaInterface
{
   public static native boolean ViconConnect(String ip);

   public static native void ViconClose();

   public static native boolean ViconIsConnected();

   public static native String ViconGetLastError();

   public static native void ViconGetFrame();

   public static native int ViconGetNumMarkers();

   public static native String ViconGetMarkerName(int id);

   public static native Point3D ViconGetMarker(String markerName);

   public static native int ViconGetNumBodies();

   public static native String ViconGetBodyName(int id);

//   public static native Pose ViconGetBodyAngleAxis(String bodyName);
//
//   public static native Pose ViconGetBodyEulerAngles(String bodyName);

   public static native QuaternionPose ViconGetBodyQuaternion(String bodyName);

   public static native RotationMatrix ViconGetBodyRotationMatrix(String bodyName);

   public static native double ViconGetFrameTimeStamp();

   public static native int ViconGetNumLocalBodies();

//   public static native Pose ViconGetLocalBodyAngleAxis(String bodyName);
//
//   public static native Pose ViconGetLocalBodyEulerAngles(String bodyName);
//
//   public Pose getPose(String bodyName)
//   {
//      ViconGetFrame();
//
//      return ViconGetBodyAngleAxis(bodyName);
//   }

   public void test()
   {
      System.out.println("received test request");
   }


   static
   {
      try
      {
         System.loadLibrary("Vicon");
      }
      catch (UnsatisfiedLinkError e)
      {
         e.printStackTrace();
      }
   }

   public static void main(String[] args)
   {
      ViconConnect("10.4.1.100");

      boolean connected = ViconIsConnected();
      if (connected)
         System.out.println("successfully connected");
      else
         System.out.println("not connected");

      ViconGetFrame();

//    int numMarkers = ViconGetNumMarkers();
//    System.out.println("numMarkers=" + numMarkers);
//
//    int z = 0;
//    while(z == 0)
//    {
//        for (int i = 0; i < numMarkers; i++)
//        {
//            String name = ViconGetMarkerName(i);
//            Point3D position = ViconGetMarker(name);
//            System.out.println("marker " + i + " is " + name + ": " + position);
//        }
//
//        try
//        {
//            Thread.sleep(1000);
//        }
//        catch (InterruptedException ex)
//        {
//        }
//    }
//    int numBodies = ViconGetNumBodies();
//    System.out.println("numBodies=" + numBodies);
//
//    for(int i=0; i < numBodies; i++)
//    {
//        String name = ViconGetBodyName(i);
//        Pose pose = ViconGetBodyAngleAxis(name);
//        System.out.println("body " + i + " is " + name + ": " + pose);
//    }


      @SuppressWarnings("unused")
      long start = System.currentTimeMillis();
//      int count = 0;
      String name = ViconGetBodyName(0);
      while (true)
      {
         long startTime = System.currentTimeMillis();
         ViconGetFrame();
         QuaternionPose pose = ViconGetBodyQuaternion(name);
         long endTime = System.currentTimeMillis();
//         count++;
         System.out.println(name + ": " + pose + " in " + (endTime-startTime) + " ms");

//       if(count >= 20)
//       {
//           long now = System.currentTimeMillis();
//           System.out.println("read " + count + " in " + (now - start));
//           start = now;
//           count = 0;
//       }


         try
         {
            Thread.sleep(100);
         }
         catch (InterruptedException ex)
         {
         }
      }

//    Pose poseEuler = ViconGetBodyEulerAngles("one");
//    System.out.println("poseE = " + poseEuler);
//
//    Pose poseQuaternion = ViconGetBodyQuaternion("one");
//    System.out.println("poseQ = " + poseQuaternion);

//    ViconJavaInterface.ViconClose();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/frames/MovingWalkingReferenceFrame.java<|end_filename|>
package us.ihmc.humanoidRobotics.frames;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
import us.ihmc.robotics.screwTheory.MovingMidFootZUpGroundFrame;

/**
 * {@code MovingWalkingReferenceFrame} was originally implemented in
 * {@code HumanoidReferenceFrames}.
 * <p>
 * It represents the need of having some simple reference frame moving along the walking path
 * somewhere in between in the feet and moving forward in similar way as the robot is progressing.
 * </p>
 * <p>
 * This implementation starts off a {@code MovingMidFootZUpGroundFrame} and adjust it to have the
 * same progression as the pelvis.
 * </p>
 */
public class MovingWalkingReferenceFrame extends MovingReferenceFrame
{
   private final MovingReferenceFrame pelvisFrame;
   private final MovingMidFootZUpGroundFrame midFootZUpGroundFrame;

   private final FramePoint3D pelvisPosition = new FramePoint3D();
   private final FramePose3D pose = new FramePose3D();

   public MovingWalkingReferenceFrame(String name, MovingReferenceFrame pelvisFrame, MovingMidFootZUpGroundFrame midFootZUpGroundFrame)
   {
      super(name, pelvisFrame.getRootFrame(), true);

      if (pelvisFrame == midFootZUpGroundFrame)
         throw new IllegalArgumentException("The frames have to be different.");
      pelvisFrame.verifySameRoots(midFootZUpGroundFrame);

      this.pelvisFrame = pelvisFrame;
      this.midFootZUpGroundFrame = midFootZUpGroundFrame;
   }

   @Override
   protected void updateTransformToParent(RigidBodyTransform transformToParent)
   {
      pelvisPosition.setToZero(pelvisFrame);
      pelvisPosition.changeFrame(midFootZUpGroundFrame);
      pose.setToZero(midFootZUpGroundFrame);
      pose.setX(pelvisPosition.getX());
      pose.changeFrame(getParent());
      pose.get(transformToParent);
   }

   private final FrameVector3D linearPelvisVelocity = new FrameVector3D();
   private final FrameVector3D linearVelocity = new FrameVector3D();
   private final FrameVector3D angularVelocity = new FrameVector3D();
   private final Twist twistOfPelvisRelativeToMidFootFrame = new Twist();
   private final Vector3D offset = new Vector3D();
   private final Vector3D linearVelocityOffset = new Vector3D();

   @Override
   protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack)
   {
      twistRelativeToParentToPack.setToZero(this, getParent(), this);

      TwistReadOnly twistOfMidFootFrame = midFootZUpGroundFrame.getTwistOfFrame();
      angularVelocity.setIncludingFrame(twistOfMidFootFrame.getAngularPart());
      linearVelocity.setIncludingFrame(twistOfMidFootFrame.getLinearPart());

      pelvisFrame.getTwistRelativeToOther(midFootZUpGroundFrame, twistOfPelvisRelativeToMidFootFrame);

      linearPelvisVelocity.setIncludingFrame(twistOfPelvisRelativeToMidFootFrame.getLinearPart());
      linearPelvisVelocity.changeFrame(this);

      angularVelocity.changeFrame(this);
      linearVelocity.changeFrame(this);
      linearVelocity.add(linearPelvisVelocity.getX(), 0.0, 0.0);

      offset.set(pelvisPosition.getX(), 0.0, 0.0);
      linearVelocityOffset.cross(angularVelocity, offset);

      linearVelocity.add(linearVelocityOffset);

      twistRelativeToParentToPack.getAngularPart().set(angularVelocity);
      twistRelativeToParentToPack.getLinearPart().set(linearVelocity);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/globalParameters/BooleanGlobalParameter.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.globalParameters;

import us.ihmc.yoVariables.variable.YoBoolean;

public class BooleanGlobalParameter extends GlobalParameter
{
   public BooleanGlobalParameter(String name, String description, boolean value, GlobalParameterChangedListener listener)
   {
      super(listener);

      yoVariable = new YoBoolean(name, description, registry);
      ((YoBoolean)yoVariable).set(value);

      if (changedListener != null)
         changedListener.globalParameterCreated(this);
   }

   protected BooleanGlobalParameter(String name, String description, GlobalParameter[] parents, GlobalParameterChangedListener listener)
   {
      super(parents, listener);

      yoVariable = new YoBoolean(name, description, registry);

      if (changedListener != null)
         changedListener.globalParameterCreated(this);
   }

   protected int getNumberOfCharactersForDisplay()
   {
      return "false".length();
   }

   public boolean getValue()
   {
      return ((YoBoolean)yoVariable).getBooleanValue();
   }


   public void set(boolean value)
   {
      set(value, "");
   }

   public void set(boolean value, String comment)
   {
      verifyNoParents();
      setBooleanValue(value, comment);
   }

   public void setOnlyIfChange(boolean value)
   {
      setOnlyIfChange(value, "");
   }

   public void setOnlyIfChange(boolean value, String comment)
   {
      verifyNoParents();

      // check if the value is the same as the current value
      if (value == ((YoBoolean)yoVariable).getBooleanValue())
         return;
      else
         setBooleanValue(value, comment);
   }


   @Override
   public String getValueInStringFormat()
   {
      String ret = Boolean.toString(((YoBoolean)yoVariable).getBooleanValue());

      return padWithSpaces(ret, numberOfCharactersForDisplay);
   }



   protected void setBooleanValue(boolean newValue, String comment)
   {
      boolean previousValue = ((YoBoolean)yoVariable).getBooleanValue();
      ((YoBoolean)yoVariable).set(newValue);

      if (changedListener != null)
      {
         changedListener.booleanValueChanged(this, comment, previousValue, newValue);
      }

      updateChildren(comment);
   }




}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/RobotConnection.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication;

public interface RobotConnection
{
   public boolean isConnected();
   
   public void attemptConnectionToHost();
   public void disconnect();
   
   public void pause();
   
   
   public void setRecord(boolean record);
   
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ValveLocationPacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class ValveLocationPacket extends Packet<ValveLocationPacket> implements Settable<ValveLocationPacket>, EpsilonComparable<ValveLocationPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.geometry.Pose3D valve_pose_in_world_;
   public double valve_radius_;

   public ValveLocationPacket()
   {
      valve_pose_in_world_ = new us.ihmc.euclid.geometry.Pose3D();
   }

   public ValveLocationPacket(ValveLocationPacket other)
   {
      this();
      set(other);
   }

   public void set(ValveLocationPacket other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.valve_pose_in_world_, valve_pose_in_world_);
      valve_radius_ = other.valve_radius_;

   }

   /**
            * 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 us.ihmc.euclid.geometry.Pose3D getValvePoseInWorld()
   {
      return valve_pose_in_world_;
   }

   public void setValveRadius(double valve_radius)
   {
      valve_radius_ = valve_radius;
   }
   public double getValveRadius()
   {
      return valve_radius_;
   }


   public static Supplier<ValveLocationPacketPubSubType> getPubSubType()
   {
      return ValveLocationPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ValveLocationPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ValveLocationPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.valve_pose_in_world_.epsilonEquals(other.valve_pose_in_world_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.valve_radius_, other.valve_radius_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ValveLocationPacket)) return false;

      ValveLocationPacket otherMyClass = (ValveLocationPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.valve_pose_in_world_.equals(otherMyClass.valve_pose_in_world_)) return false;
      if(this.valve_radius_ != otherMyClass.valve_radius_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ValveLocationPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("valve_pose_in_world=");
      builder.append(this.valve_pose_in_world_);      builder.append(", ");
      builder.append("valve_radius=");
      builder.append(this.valve_radius_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/SolarPanelEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.List;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationConstructionSetTools.robotController.ContactController;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;


public class SolarPanelEnvironment implements CommonAvatarEnvironmentInterface
{
   private final CombinedTerrainObject3D combinedTerrainObject;
      
   private final float SOLAR_WIDTH = 0.635f;
   private final float SOLAR_LENGTH = 0.635f;
   private final float SOLAR_HEIGHT = 0.025f;
   private final float SOLAR_GROUND_HEIGHT = 1f;
   private final float PITCH = -0.785398f; // 45 degrees
   private final float FORWARD_OFFSET = 1f;
   


   
   public SolarPanelEnvironment()
   {
      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject.addTerrainObject(setUpGround("Ground"));
      
      Graphics3DObject linkGraphics = new Graphics3DObject();

      //    linkGraphics.rotate(Math.PI / 2, Axis.Y);
      //    linkGraphics.rotate(Math.toRadians(-courseAngleDeg), Axis.X);
     //
    
    
      linkGraphics.translate(new Vector3D(FORWARD_OFFSET, 0, SOLAR_GROUND_HEIGHT));
      linkGraphics.rotate(PITCH, new Vector3D(0, 1, 0));
      linkGraphics.scale(new Vector3D(SOLAR_WIDTH,SOLAR_LENGTH,SOLAR_HEIGHT));
      linkGraphics.addModelFile("models/solarPanel.obj");

      combinedTerrainObject.addStaticLinkGraphics(linkGraphics); 
   }
   
   private CombinedTerrainObject3D setUpGround(String name)
   {
      CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);
      combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.Gray());     
      
      return combinedTerrainObject;
   }
   
   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return null;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
      ContactController contactController = new ContactController();
      contactController.setContactParameters(100000.0, 100.0, 0.5, 0.3);
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
   
   

}


<|start_filename|>robot-environment-awareness/src/visualizers/java/us/ihmc/octoMap/SearchNodeNeighborsVisualizer.java<|end_filename|>
package us.ihmc.octoMap;

import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;

import javafx.application.Application;
import javafx.scene.Group;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.Box;
import javafx.stage.Stage;
import us.ihmc.jOctoMap.key.OcTreeKey;
import us.ihmc.jOctoMap.tools.OcTreeKeyConversionTools;
import us.ihmc.jOctoMap.tools.OcTreeKeyTools;
import us.ihmc.javaFXToolkit.scenes.View3DFactory;

public class SearchNodeNeighborsVisualizer extends Application
{
   private static final Color NEIGHBOR_COLOR = new Color(Color.YELLOW.getRed(), Color.YELLOW.getGreen(), Color.YELLOW.getBlue(), 0.0);
   private final List<OcTreeKey> neighbors;
   private final int depth = 13;
   private final double resolution = 0.025;
   private final int treeDepth = 16;
   private final double searchRadius = 0.50;

   public SearchNodeNeighborsVisualizer()
   {
      double nodeSize = OcTreeKeyConversionTools.computeNodeSize(depth, resolution, treeDepth);
      Point3D nodeCenter = new Point3D(nodeSize, nodeSize, nodeSize);
      nodeCenter.scale(0.5);

      OcTreeKey key = OcTreeKeyConversionTools.coordinateToKey(nodeCenter, depth, resolution, treeDepth);
      neighbors = OcTreeKeyTools.computeNeighborKeys(key, depth, resolution, treeDepth, searchRadius);
   }

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      primaryStage.setTitle("OcTree Visualizer");

      View3DFactory view3dFactory = new View3DFactory(800, 600);
      view3dFactory.addCameraController();
      view3dFactory.addWorldCoordinateSystem(0.3);

      for (int i = 0; i < neighbors.size(); i++)
      {
         OcTreeKey ocTreeKey = neighbors.get(i);
         double boxSize = OcTreeKeyConversionTools.computeNodeSize(depth, resolution, treeDepth);
         Point3D boxCenter = OcTreeKeyConversionTools.keyToCoordinate(ocTreeKey, depth, resolution, treeDepth);
         addFreeBox(boxSize, boxCenter, view3dFactory.getRoot());
      }

      primaryStage.setScene(view3dFactory.getScene());
      primaryStage.show();
   }

   public void addFreeBox(double size, Point3D center, Group root)
   {
      addBox(size, center, root, NEIGHBOR_COLOR);
   }

   private void addBox(double size, Point3D center, Group root, Color color)
   {
      Box box = new Box(size, size, size);
      box.setTranslateX(center.getX());
      box.setTranslateY(center.getY());
      box.setTranslateZ(center.getZ());
      PhongMaterial material = new PhongMaterial();
      material.setDiffuseColor(color);
      box.setMaterial(material);
      root.getChildren().add(box);
   }

   public static void main(String[] args)
   {
      Application.launch(args);
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/producers/VideoSource.java<|end_filename|>
package us.ihmc.communication.producers;

import us.ihmc.robotics.robotSide.RobotSide;

/**
 * Make a new enum when you need a new video source.
 * 
 * @author <NAME>
 */
public enum VideoSource
{
   MULTISENSE_LEFT_EYE,
   MULTISENSE_RIGHT_EYE,
   FISHEYE_LEFT,
   FISHEYE_RIGHT,
   CV_THRESHOLD,
   IMAGE_PROCESSING_BEHAVIOR,
   AWARE_FACE_TRACKER,
   ;
   
   public static final VideoSource[] values = values();
   
   public static VideoSource getFisheyeSourceFromRobotSide(RobotSide robotSide)
   {
      if (robotSide == RobotSide.LEFT)
      {
         return FISHEYE_LEFT;
      }
      else
      {
         return FISHEYE_RIGHT;
      }
   }
   
   public static VideoSource getMultisenseSourceFromRobotSide(RobotSide robotSide)
   {
      if (robotSide == RobotSide.LEFT)
      {
         return MULTISENSE_LEFT_EYE;
      }
      else
      {
         return MULTISENSE_RIGHT_EYE;
      }
   }

   public RobotSide getRobotSide()
   {
      switch (this)
      {
      case MULTISENSE_LEFT_EYE:
      case FISHEYE_LEFT:
         return RobotSide.LEFT;
      case MULTISENSE_RIGHT_EYE:
      case FISHEYE_RIGHT:
         return RobotSide.RIGHT;
      default:
         throw new RuntimeException(this.name() + " is not associated with a side");
      }
   }

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static VideoSource fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/beetle/controller/HexapodMomentumOptimizationSettings.java<|end_filename|>
package us.ihmc.exampleSimulations.beetle.controller;

import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.ControllerCoreOptimizationSettings;
import us.ihmc.euclid.tuple2D.Vector2D;

public class HexapodMomentumOptimizationSettings implements ControllerCoreOptimizationSettings
{
   private final int nBasisVectorsPerContactPoint = 4;
   private final int nContactPointsPerContactableBody = 1;
   private final int nContactableBodies = 6;

   private final double jointAccelerationWeight = 0.005;
   private final double jointJerkWeight = 1.0E-7;
   private final double rhoWeight = 0.00001;
   private final double rhoMin = 4.0;
   private final double rhoRateDefaultWeight = 2.0E-9;
   private final double rhoRateHighWeight = 5.0E-8;
   private final Vector2D copWeight = new Vector2D(0.001, 0.002);
   private final Vector2D copRateDefaultWeight = new Vector2D(0.0000002, 0.0000002);
   private final Vector2D copRateHighWeight = new Vector2D(0.000025, 0.0001);

   /** @inheritDoc */
   @Override
   public double getJointAccelerationWeight()
   {
      return jointAccelerationWeight;
   }

   /** @inheritDoc */
   @Override
   public double getJointJerkWeight()
   {
      return jointJerkWeight;
   }

   /** @inheritDoc */
   @Override
   public double getRhoWeight()
   {
      return rhoWeight;
   }

   /** @inheritDoc */
   @Override
   public double getRhoMin()
   {
      return rhoMin;
   }

   /** @inheritDoc */
   @Override
   public double getRhoRateDefaultWeight()
   {
      return rhoRateDefaultWeight;
   }

   /** @inheritDoc */
   @Override
   public double getRhoRateHighWeight()
   {
      return rhoRateHighWeight;
   }

   /** @inheritDoc */
   @Override
   public Vector2D getCoPWeight()
   {
      return copWeight;
   }

   /** @inheritDoc */
   @Override
   public Vector2D getCoPRateDefaultWeight()
   {
      return copRateDefaultWeight;
   }

   /** @inheritDoc */
   @Override
   public Vector2D getCoPRateHighWeight()
   {
      return copRateHighWeight;
   }

   /** @inheritDoc */
   @Override
   public int getNumberOfBasisVectorsPerContactPoint()
   {
      return nBasisVectorsPerContactPoint;
   }

   /** @inheritDoc */
   @Override
   public int getNumberOfContactPointsPerContactableBody()
   {
      return nContactPointsPerContactableBody;
   }

   /** @inheritDoc */
   @Override
   public int getNumberOfContactableBodies()
   {
      return nContactableBodies;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/kinematicsPlanningToolboxAPI/KinematicsPlanningToolboxMessageFactory.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.kinematicsPlanningToolboxAPI;

import java.util.ArrayList;
import java.util.List;

import controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage;
import gnu.trove.list.array.TDoubleArrayList;
import us.ihmc.communication.packets.MessageTools;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public class KinematicsPlanningToolboxMessageFactory
{
   public static final double DEFAULT_POSITION_DISPLACEMENT = 0.01;
   public static final double DEFAULT_ORIENTATION_DISPLACEMENT = 0.01;

   public static void setDefaultAllowableDisplacement(KinematicsPlanningToolboxRigidBodyMessage message, int numberOfKeyFrames)
   {
      for (int i = 0; i < numberOfKeyFrames; i++)
      {
         message.getAllowablePositionDisplacement().add(DEFAULT_POSITION_DISPLACEMENT);
         message.getAllowableOrientationDisplacement().add(DEFAULT_ORIENTATION_DISPLACEMENT);
      }
   }

   public static KinematicsPlanningToolboxRigidBodyMessage holdRigidBodyCurrentPose(RigidBodyBasics rigidBody, TDoubleArrayList keyFrameTimes)
   {
      List<Pose3DReadOnly> currentPoses = new ArrayList<Pose3DReadOnly>();
      for (int i = 0; i < keyFrameTimes.size(); i++)
         currentPoses.add(new Pose3D(rigidBody.getBodyFixedFrame().getTransformToWorldFrame()));
      KinematicsPlanningToolboxRigidBodyMessage message = HumanoidMessageTools.createKinematicsPlanningToolboxRigidBodyMessage(rigidBody, keyFrameTimes,
                                                                                                                               currentPoses);

      message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(20.0));
      message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(20.0));

      return message;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/AfterJointReferenceFrameNameMap.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.LinkedHashMap;
import java.util.List;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public class AfterJointReferenceFrameNameMap
{
   private final LinkedHashMap<String, ReferenceFrame> afterJointReferenceFrames = new LinkedHashMap<String, ReferenceFrame>();
   
   public AfterJointReferenceFrameNameMap(RigidBodyBasics base)
   {
      for(JointBasics joint : base.childrenSubtreeIterable())
      {
         afterJointReferenceFrames.put(joint.getFrameAfterJoint().getName(), joint.getFrameAfterJoint());
      }
   }
   
   public AfterJointReferenceFrameNameMap(List<ReferenceFrame> frames)
   {
      for(ReferenceFrame frame : frames)
      {
         afterJointReferenceFrames.put(frame.getName(), frame);
      }
   }

   public ReferenceFrame getFrameByName(String name)
   {
      final ReferenceFrame referenceFrame = afterJointReferenceFrames.get(name);
      if(referenceFrame == null)
      {
         throw new RuntimeException("Unknown frame " + name);
      }
      return referenceFrame;
   }
}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/parameters/QuadrupedSitDownParameters.java<|end_filename|>
package us.ihmc.quadrupedRobotics.parameters;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WholeBodySetpointParameters;

public interface QuadrupedSitDownParameters
{
   WholeBodySetpointParameters getSitDownParameters();

   double getTimeToMoveForSittingDown();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HeightQuadTreeMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "HeightQuadTreeMessage" defined in "HeightQuadTreeMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from HeightQuadTreeMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit HeightQuadTreeMessage_.idl instead.
*
*/
public class HeightQuadTreeMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.HeightQuadTreeMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::HeightQuadTreeMessage_";

   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.HeightQuadTreeMessage 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.HeightQuadTreeMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 5000; ++i0)
      {
          current_alignment += controller_msgs.msg.dds.HeightQuadTreeLeafMessagePubSubType.getMaxCdrSerializedSize(current_alignment);}

      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.HeightQuadTreeMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.HeightQuadTreeMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getLeaves().size(); ++i0)
      {
          current_alignment += controller_msgs.msg.dds.HeightQuadTreeLeafMessagePubSubType.getCdrSerializedSize(data.getLeaves().get(i0), current_alignment);}


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.HeightQuadTreeMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_5(data.getDefaultHeight());

      cdr.write_type_5(data.getResolution());

      cdr.write_type_5(data.getSizeX());

      cdr.write_type_5(data.getSizeY());

      if(data.getLeaves().size() <= 5000)
      cdr.write_type_e(data.getLeaves());else
          throw new RuntimeException("leaves field exceeds the maximum length");

   }

   public static void read(controller_msgs.msg.dds.HeightQuadTreeMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setDefaultHeight(cdr.read_type_5());
      	
      data.setResolution(cdr.read_type_5());
      	
      data.setSizeX(cdr.read_type_5());
      	
      data.setSizeY(cdr.read_type_5());
      	
      cdr.read_type_e(data.getLeaves());	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.HeightQuadTreeMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_5("default_height", data.getDefaultHeight());
      ser.write_type_5("resolution", data.getResolution());
      ser.write_type_5("size_x", data.getSizeX());
      ser.write_type_5("size_y", data.getSizeY());
      ser.write_type_e("leaves", data.getLeaves());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.HeightQuadTreeMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setDefaultHeight(ser.read_type_5("default_height"));
      data.setResolution(ser.read_type_5("resolution"));
      data.setSizeX(ser.read_type_5("size_x"));
      data.setSizeY(ser.read_type_5("size_y"));
      ser.read_type_e("leaves", data.getLeaves());
   }

   public static void staticCopy(controller_msgs.msg.dds.HeightQuadTreeMessage src, controller_msgs.msg.dds.HeightQuadTreeMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.HeightQuadTreeMessage createData()
   {
      return new controller_msgs.msg.dds.HeightQuadTreeMessage();
   }
   @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.HeightQuadTreeMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.HeightQuadTreeMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.HeightQuadTreeMessage src, controller_msgs.msg.dds.HeightQuadTreeMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public HeightQuadTreeMessagePubSubType newInstance()
   {
      return new HeightQuadTreeMessagePubSubType();
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/WholeBodyTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.WholeBodyTrajectoryMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.WholeBodyTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class WholeBodyTrajectoryTask extends BehaviorAction
{
   private final WholeBodyTrajectoryMessage wholebodyTrajectoryMessage;
   private final WholeBodyTrajectoryBehavior wholebodyTrajectoryBehavior;

   public WholeBodyTrajectoryTask(WholeBodyTrajectoryMessage wholebodyTrajectoryMessage, WholeBodyTrajectoryBehavior wholebodyTrajectoryBehavior)
   {
      super(wholebodyTrajectoryBehavior);
      this.wholebodyTrajectoryBehavior = wholebodyTrajectoryBehavior;
      this.wholebodyTrajectoryMessage = wholebodyTrajectoryMessage;
   }

   @Override
   protected void setBehaviorInput()
   {
      wholebodyTrajectoryBehavior.setInput(wholebodyTrajectoryMessage);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/SliderBoardTransmitterInterface.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public interface SliderBoardTransmitterInterface
{
   public abstract void moveControl(MidiControl midiControl);
   public abstract void moveControl(MidiControl midiControl, int sliderValue);
   public abstract void closeAndDispose();
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/SimulationsChangedListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;


public interface SimulationsChangedListener
{
   public void simulationsChanged();
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/TaskRegion.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

public class TaskRegion
{
   private double[] upperLimit;
   private double[] lowerLimit;
   private boolean[] isEnable;
   
   private double trajectoryTime; 
   
   public TaskRegion(int size)
   {
      upperLimit = new double[size];
      lowerLimit = new double[size];
      isEnable = new boolean[size];
   }
   
   public void setRandomRegion(int index, double lowerValue, double upperValue)
   {
      if(index == 0)
      {
         upperLimit[index] = upperValue;
         trajectoryTime = upperValue;
      }
      else
      { 
         upperLimit[index] =  upperValue;
      }      
      
      lowerLimit[index] = lowerValue;
      
      if(upperValue == lowerValue)
      {
         isEnable[index] = false;
      }
      else
      {
         isEnable[index] = true;
      }
   }
   
   public boolean isEnable(int index)
   {
      return isEnable[index];
   }
   
   public double getUpperLimit(int index)
   {
      return upperLimit[index];
   }
   
   public double getLowerLimit(int index)
   {
      return lowerLimit[index];
   }
   
   public double sizeOfRegion(int index)
   {
      return Math.abs(getUpperLimit(index) - getLowerLimit(index));
   }
      
   public double getTrajectoryTime()
   {
      return trajectoryTime;
   }
}

<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/interfaces/Connectable.java<|end_filename|>
package us.ihmc.communication.interfaces;

public interface Connectable
{
   public boolean isConnected();
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/controller/LIDARFilterAnchorPaneController.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.controller;

import javafx.fxml.FXML;
import javafx.scene.control.Slider;
import javafx.scene.control.Spinner;
import javafx.scene.control.SpinnerValueFactory.DoubleSpinnerValueFactory;
import javafx.scene.control.ToggleButton;
import us.ihmc.javaFXToolkit.messager.MessageBidirectionalBinding.PropertyToMessageTypeConverter;
import us.ihmc.robotEnvironmentAwareness.communication.REAModuleAPI;
import us.ihmc.robotEnvironmentAwareness.ui.properties.BoundingBoxParametersProperty;

public class LIDARFilterAnchorPaneController extends REABasicUIController
{
   @FXML
   private ToggleButton enableBoundingBoxButton;
   @FXML
   private ToggleButton showBoundingBoxButton;
   @FXML
   private Spinner<Double> boundingBoxMinXSpinner;
   @FXML
   private Spinner<Double> boundingBoxMaxXSpinner;
   @FXML
   private Spinner<Double> boundingBoxMinYSpinner;
   @FXML
   private Spinner<Double> boundingBoxMaxYSpinner;
   @FXML
   private Spinner<Double> boundingBoxMinZSpinner;
   @FXML
   private Spinner<Double> boundingBoxMaxZSpinner;

   private final BoundingBoxParametersProperty boundingBoxParametersProperty = new BoundingBoxParametersProperty(this, "boundingBoxParameters");

   @FXML
   private Slider lidarMinRange;
   @FXML
   private Slider lidarMaxRange;

   private final PropertyToMessageTypeConverter<Double, Number> converter = new PropertyToMessageTypeConverter<Double, Number>()
   {

      @Override
      public Double convert(Number propertyValue)
      {
         return propertyValue.doubleValue();
      }

      @Override
      public Number interpret(Double messageContent)
      {
         return messageContent.doubleValue();
      }
   };

   public LIDARFilterAnchorPaneController()
   {
   }

   private void setupControls()
   {
      boundingBoxMinXSpinner.setValueFactory(createBoundingBoxValueFactory(0.0));
      boundingBoxMaxXSpinner.setValueFactory(createBoundingBoxValueFactory(5.0));
      boundingBoxMinYSpinner.setValueFactory(createBoundingBoxValueFactory(-2.0));
      boundingBoxMaxYSpinner.setValueFactory(createBoundingBoxValueFactory(2.0));
      boundingBoxMinZSpinner.setValueFactory(createBoundingBoxValueFactory(-1.0));
      boundingBoxMaxZSpinner.setValueFactory(createBoundingBoxValueFactory(1.0));
   }

   @Override
   public void bindControls()
   {
      setupControls();

      uiMessager.bindBidirectionalGlobal(REAModuleAPI.OcTreeBoundingBoxEnable, enableBoundingBoxButton.selectedProperty());
      uiMessager.bindBidirectionalGlobal(REAModuleAPI.LidarMinRange, lidarMinRange.valueProperty(), converter);
      uiMessager.bindBidirectionalGlobal(REAModuleAPI.LidarMaxRange, lidarMaxRange.valueProperty(), converter);

      boundingBoxParametersProperty.binBidirectionalMinX(boundingBoxMinXSpinner.getValueFactory().valueProperty());
      boundingBoxParametersProperty.binBidirectionalMinY(boundingBoxMinYSpinner.getValueFactory().valueProperty());
      boundingBoxParametersProperty.binBidirectionalMinZ(boundingBoxMinZSpinner.getValueFactory().valueProperty());
      boundingBoxParametersProperty.binBidirectionalMaxX(boundingBoxMaxXSpinner.getValueFactory().valueProperty());
      boundingBoxParametersProperty.binBidirectionalMaxY(boundingBoxMaxYSpinner.getValueFactory().valueProperty());
      boundingBoxParametersProperty.binBidirectionalMaxZ(boundingBoxMaxZSpinner.getValueFactory().valueProperty());
      uiMessager.bindBidirectionalGlobal(REAModuleAPI.OcTreeBoundingBoxParameters, boundingBoxParametersProperty);

      load();
      uiMessager.bindBidirectionalInternal(REAModuleAPI.UIOcTreeBoundingBoxShow, showBoundingBoxButton.selectedProperty(), true);
   }

   @FXML
   public void save()
   {
      uiMessager.submitStateRequestToModule(REAModuleAPI.SaveMainUpdaterConfiguration);
      saveUIControlProperty(REAModuleAPI.UIOcTreeBoundingBoxShow, showBoundingBoxButton);
   }

   public void load()
   {
      loadUIControlProperty(REAModuleAPI.UIOcTreeBoundingBoxShow, showBoundingBoxButton);
   }

   private DoubleSpinnerValueFactory createBoundingBoxValueFactory(double initialValue)
   {
      double min = -100.0;
      double max = 100.0;
      double amountToStepBy = 0.1;
      return new DoubleSpinnerValueFactory(min, max, initialValue, amountToStepBy);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BoundingBoxesPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "BoundingBoxesPacket" defined in "BoundingBoxesPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from BoundingBoxesPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit BoundingBoxesPacket_.idl instead.
*
*/
public class BoundingBoxesPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.BoundingBoxesPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::BoundingBoxesPacket_";

   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.BoundingBoxesPacket 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.BoundingBoxesPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0)
      {
        current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1;
      }

      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.BoundingBoxesPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.BoundingBoxesPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getBoundingBoxesXCoordinates().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getBoundingBoxesYCoordinates().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getBoundingBoxesWidths().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getBoundingBoxesHeights().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getLabels().size(); ++i0)
      {
          current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getLabels().get(i0).length() + 1;
      }

      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.BoundingBoxesPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getBoundingBoxesXCoordinates().size() <= 100)
      cdr.write_type_e(data.getBoundingBoxesXCoordinates());else
          throw new RuntimeException("bounding_boxes_x_coordinates field exceeds the maximum length");

      if(data.getBoundingBoxesYCoordinates().size() <= 100)
      cdr.write_type_e(data.getBoundingBoxesYCoordinates());else
          throw new RuntimeException("bounding_boxes_y_coordinates field exceeds the maximum length");

      if(data.getBoundingBoxesWidths().size() <= 100)
      cdr.write_type_e(data.getBoundingBoxesWidths());else
          throw new RuntimeException("bounding_boxes_widths field exceeds the maximum length");

      if(data.getBoundingBoxesHeights().size() <= 100)
      cdr.write_type_e(data.getBoundingBoxesHeights());else
          throw new RuntimeException("bounding_boxes_heights field exceeds the maximum length");

      if(data.getLabels().size() <= 100)
      cdr.write_type_e(data.getLabels());else
          throw new RuntimeException("labels field exceeds the maximum length");

   }

   public static void read(controller_msgs.msg.dds.BoundingBoxesPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_e(data.getBoundingBoxesXCoordinates());	
      cdr.read_type_e(data.getBoundingBoxesYCoordinates());	
      cdr.read_type_e(data.getBoundingBoxesWidths());	
      cdr.read_type_e(data.getBoundingBoxesHeights());	
      cdr.read_type_e(data.getLabels());	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.BoundingBoxesPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_e("bounding_boxes_x_coordinates", data.getBoundingBoxesXCoordinates());
      ser.write_type_e("bounding_boxes_y_coordinates", data.getBoundingBoxesYCoordinates());
      ser.write_type_e("bounding_boxes_widths", data.getBoundingBoxesWidths());
      ser.write_type_e("bounding_boxes_heights", data.getBoundingBoxesHeights());
      ser.write_type_e("labels", data.getLabels());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.BoundingBoxesPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_e("bounding_boxes_x_coordinates", data.getBoundingBoxesXCoordinates());
      ser.read_type_e("bounding_boxes_y_coordinates", data.getBoundingBoxesYCoordinates());
      ser.read_type_e("bounding_boxes_widths", data.getBoundingBoxesWidths());
      ser.read_type_e("bounding_boxes_heights", data.getBoundingBoxesHeights());
      ser.read_type_e("labels", data.getLabels());
   }

   public static void staticCopy(controller_msgs.msg.dds.BoundingBoxesPacket src, controller_msgs.msg.dds.BoundingBoxesPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.BoundingBoxesPacket createData()
   {
      return new controller_msgs.msg.dds.BoundingBoxesPacket();
   }
   @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.BoundingBoxesPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.BoundingBoxesPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.BoundingBoxesPacket src, controller_msgs.msg.dds.BoundingBoxesPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public BoundingBoxesPacketPubSubType newInstance()
   {
      return new BoundingBoxesPacketPubSubType();
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTTestProject.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.util.ArrayList;

import javax.swing.JFrame;
import javax.swing.JPanel;

import us.ihmc.commons.PrintTools;

public class RRTTestProject extends JPanel
{
   double xUpper;
   double xLower;

   double yUpper;
   double yLower;

   double scale;

   int sizeU;
   int sizeV;

   double[] allPoints;
   int numOfPoints;

   double[] pathPoints;
   double[] boxInfo = new double[4];

   RRTPlanner info;

   public RRTTestProject(int scaleF, double[] pointInfo, RRTPlanner info)
   {
      this.scale = scaleF;
      xUpper = info.getRRTTree().upperBoundNode.getNodeData(0);
      xLower = info.getRRTTree().lowerBoundNode.getNodeData(0);
      yUpper = info.getRRTTree().upperBoundNode.getNodeData(1);
      yLower = info.getRRTTree().lowerBoundNode.getNodeData(1);

      sizeU = (int) Math.round((-yLower + yUpper) * scale);
      sizeV = (int) Math.round((-xLower + xUpper) * scale);

      allPoints = pointInfo;
      this.numOfPoints = allPoints.length / 4;

      //pathPoints = pointInfoOfPath;
      boxInfo[0] = RRTNode2D.boxCenterX;
      boxInfo[1] = RRTNode2D.boxCenterY;
      boxInfo[2] = RRTNode2D.boxSizeX;
      boxInfo[3] = RRTNode2D.boxSizeY;
      this.info = info;
   }

   public void paint(Graphics g)
   {
      super.paint(g);
      g.setColor(Color.red);
      g.drawLine(y2u(0.0), x2v(0.0), y2u(0.5), x2v(0.0));
      g.setColor(Color.green);
      g.drawLine(y2u(0.0), x2v(0.0), y2u(0.0), x2v(0.5));

      g.setColor(Color.GRAY);

      // all node and branch line
      for (int i = 0; i < numOfPoints; i++)
      {
         branchFill(g, allPoints[i * 4 + 0], allPoints[i * 4 + 1], allPoints[i * 4 + 2], allPoints[i * 4 + 3], 4);
      }

      // root node
      point(g, 0.0, 0.0, 4);

      // path
      g.setColor(Color.MAGENTA);
      ArrayList<RRTNode> infoPathNode = info.getRRTTree().pathNode;
      for (int i =1;i<infoPathNode.size();i++)
      {
         branch(g, infoPathNode.get(i-1).getNodeData(0), infoPathNode.get(i-1).getNodeData(1), infoPathNode.get(i).getNodeData(0), infoPathNode.get(i).getNodeData(1), 6);
      }

      // goal node
      g.setColor(Color.blue);
      point(g, info.getGoalNode().getNodeData(0), info.getGoalNode().getNodeData(1), 4);

      //      g.setColor(Color.red);
      // box
      branch(g, boxInfo[0] + boxInfo[2] * 0.5, boxInfo[1] + boxInfo[3] * 0.5, boxInfo[0] + boxInfo[2] * 0.5, boxInfo[1] - boxInfo[3] * 0.5, 2);
      branch(g, boxInfo[0] + boxInfo[2] * 0.5, boxInfo[1] + boxInfo[3] * 0.5, boxInfo[0] - boxInfo[2] * 0.5, boxInfo[1] + boxInfo[3] * 0.5, 2);
      branch(g, boxInfo[0] - boxInfo[2] * 0.5, boxInfo[1] - boxInfo[3] * 0.5, boxInfo[0] - boxInfo[2] * 0.5, boxInfo[1] + boxInfo[3] * 0.5, 2);
      branch(g, boxInfo[0] - boxInfo[2] * 0.5, boxInfo[1] - boxInfo[3] * 0.5, boxInfo[0] + boxInfo[2] * 0.5, boxInfo[1] - boxInfo[3] * 0.5, 2);

      // piecewise
      g.setColor(Color.green);
      ArrayList<RRTNode> piecewisePath = info.getPiecewisePath().getPiecewisePath();
      for (int i =0;i< piecewisePath.size()-1;i++)
      {
         double p1x = piecewisePath.get(i).getNodeData(0);
         double p1y = piecewisePath.get(i).getNodeData(1);
         double p2x = piecewisePath.get(i+1).getNodeData(0);
         double p2y = piecewisePath.get(i+1).getNodeData(1);
         branch(g, p1x, p1y, p2x, p2y, 4);
      }

      // shortcut
      g.setColor(Color.black);
      ArrayList<RRTNode> optimalPath = info.getOptimalPath();
      for (int i =0;i< optimalPath.size()-1;i++)
      {
         double p1x = optimalPath.get(i).getNodeData(0);
         double p1y = optimalPath.get(i).getNodeData(1);
         double p2x = optimalPath.get(i+1).getNodeData(0);
         double p2y = optimalPath.get(i+1).getNodeData(1);
         branchFill(g, p1x, p1y, p2x, p2y, 6);
      }
   }

   public int x2v(double px)
   {
      return (int) Math.round(((-px) + xUpper) * scale);
   }

   public int y2u(double py)
   {
      return (int) Math.round(((-py) + yUpper) * scale);
   }

   public void branch(Graphics g, double p1x, double p1y, double p2x, double p2y, int size)
   {
      point(g, p1x, p1y, size);
      point(g, p2x, p2y, size);

      g.drawLine(y2u(p1y), x2v(p1x), y2u(p2y), x2v(p2x));
   }

   public void branchFill(Graphics g, double p1x, double p1y, double p2x, double p2y, int size)
   {
      pointFill(g, p1x, p1y, size);
      pointFill(g, p2x, p2y, size);

      g.drawLine(y2u(p1y), x2v(p1x), y2u(p2y), x2v(p2x));
   }

   public void point(Graphics g, double px, double py, int size)
   {
      int diameter = size;
      g.drawOval(y2u(py) - diameter / 2, x2v(px) - diameter / 2, diameter, diameter);
   }

   public void pointFill(Graphics g, double px, double py, int size)
   {
      int diameter = size;
      g.fillOval(y2u(py) - diameter / 2, x2v(px) - diameter / 2, diameter, diameter);
   }

   public static void main(String[] args)
   {
      PrintTools.info("Start!!! ");

      RRTNode startNode = new RRTNode2D(0.0, 0.0);
      RRTNode goalNode = new RRTNode2D(3.0, 2.0);
      RRTPlanner2D rrtPlanner = new RRTPlanner2D(startNode, goalNode, 0.6);

      RRTNode upperBoundNode = new RRTNode2D(5.0, 4.0);
      RRTNode lowerBoundNode = new RRTNode2D(-5.0, -4.0);
      rrtPlanner.getRRTTree().setUpperBound(upperBoundNode);
      rrtPlanner.getRRTTree().setLowerBound(lowerBoundNode);

      int maxNumberOfExpanding = 1500;
      double[] rrtPointInfo;
      rrtPointInfo = new double[maxNumberOfExpanding * 4];
      double[] rrtNewPointInfo;
      rrtNewPointInfo = new double[4];

      for (int i = 0; i < maxNumberOfExpanding; i++)
      {  
         for (int j = 0; j < 4; j++)
         {
            rrtPointInfo[i * 4 + j] = rrtNewPointInfo[j];
         }

         if (rrtPlanner.expandTreeGoal(rrtNewPointInfo) == true)
         {
            i++;
            for (int j = 0; j < 4; j++)
            {
               rrtPointInfo[i * 4 + j] = rrtNewPointInfo[j];
            }
            
            i++;
            for (int j = 0; j < 2; j++)
            {               
               rrtPointInfo[i * 4 + j] = rrtPointInfo[(i-1) * 4 + j];
               rrtPointInfo[i * 4 + 2+j] = rrtPlanner.getGoalNode().getNodeData(j);
            }
            break;
         }
      }

      JFrame frame;
      Dimension dim;
      
//      String newAmount = new Scanner(System.in).nextLine();
      
      rrtPlanner.updateOptimalPath(101, 100);

      frame = new JFrame("RRTTest");
      RRTTestProject drawPanel = new RRTTestProject(100, rrtPointInfo, rrtPlanner);
      dim = new Dimension(drawPanel.sizeU, drawPanel.sizeV);
      frame.setPreferredSize(dim);
      frame.setLocation(200, 100);

      frame.add(drawPanel);
      frame.pack();
      frame.setVisible(true);
      
      


   }
}




<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/FootstepUtils.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep;

import java.util.List;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

/**
 * @author Sylvain
 */
@Deprecated
public class FootstepUtils
{
   private final static boolean DEBUG = false;

   static
   {
      if (DEBUG)
         System.out.println("FootstepUtils: DEBUG flag is true.");
   }

   public static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

   public static Footstep generateStandingFootstep(RobotSide side, SideDependentList<? extends ContactablePlaneBody> bipedFeet)
   {
      ContactablePlaneBody endEffector = bipedFeet.get(side);

      FramePose3D footFramePose = new FramePose3D(endEffector.getSoleFrame());
      footFramePose.changeFrame(worldFrame);
      boolean trustHeight = false;

      Footstep footstep = new Footstep(side, footFramePose, trustHeight);

      return footstep;
   }

   public static Footstep generateStandingFootstep(RobotSide side, RigidBodyBasics foot, ReferenceFrame soleFrame)
   {
      FramePose3D footFramePose = new FramePose3D(soleFrame);
      footFramePose.changeFrame(worldFrame);
      boolean trustHeight = false;

      Footstep footstep = new Footstep(side, footFramePose, trustHeight);

      return footstep;
   }

   //used once
   public static FramePoint3D getCenterOfFootstep(Footstep footstep)
   {
      return getCenterOfFootstep(footstep, RobotSide.LEFT, 0.0, 0.0);
   }

   public static FramePoint3D getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
   {
      FramePoint3D stanceCenter = new FramePoint3D(stance.getSoleReferenceFrame());
      stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame());
      stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter);
      stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
      stanceCenter.changeFrame(worldFrame);

      return stanceCenter;
   }

   public static FramePoint3D getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
   {
      Point2D footstepCenter;

      List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints();
      if (predictedContactPoints != null)
      {
         ConvexPolygon2D contactPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(predictedContactPoints));
         footstepCenter = new Point2D(contactPolygon.getCentroid());
      }
      else
      {
         footstepCenter = new Point2D();
      }

      footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter);
      footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));

      FramePoint3D footstepCenter3d = new FramePoint3D(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0);
      footstepCenter3d.changeFrame(worldFrame);

      return footstepCenter3d;
   }

   //used in 1 other class
   public static RobotSide getFrontFootRobotSideBasedOnSoleCenters(SideDependentList<? extends ContactablePlaneBody> bipedFeet, FramePoint3D destination)
   {
      FramePoint3D destinationInWorld = new FramePoint3D(destination);
      destinationInWorld.changeFrame(worldFrame);
      SideDependentList<Double> distances = new SideDependentList<Double>();
      FramePoint3D footCenter = new FramePoint3D();
      for (RobotSide side : RobotSide.values)
      {
         footCenter.setToZero(bipedFeet.get(side).getSoleFrame());
         footCenter.changeFrame(worldFrame);
         distances.set(side, footCenter.distanceSquared(destinationInWorld));
      }

      RobotSide frontSide = (distances.get(RobotSide.LEFT) <= distances.get(RobotSide.RIGHT)) ? RobotSide.LEFT : RobotSide.RIGHT;

      return frontSide;
   }

   //used once
   public static RobotSide getFrontFootRobotSideFromFootsteps(SideDependentList<Footstep> footSteps, FramePoint3D destination)
   {
      FramePoint3D destinationInWorld = new FramePoint3D(destination);
      destinationInWorld.changeFrame(worldFrame);
      SideDependentList<Double> distances = new SideDependentList<Double>();
      for (RobotSide side : RobotSide.values)
      {
         FramePoint3D footstepPosition = new FramePoint3D();
         footSteps.get(side).getPosition(footstepPosition);
         footstepPosition.changeFrame(worldFrame);
         distances.set(side, new Double(footstepPosition.distanceSquared(destinationInWorld)));
      }

      RobotSide frontSide = (distances.get(RobotSide.LEFT) <= distances.get(RobotSide.RIGHT)) ? RobotSide.LEFT : RobotSide.RIGHT;

      return frontSide;
   }

   //used once
   public static double getHeading(ReferenceFrame ankleFrame)
   {
      RigidBodyTransform transform = ankleFrame.getTransformToDesiredFrame(worldFrame);
      Vector3D xVector = new Vector3D(1.0, 0.0, 0.0);
      transform.transform(xVector);
      xVector.sub(new Vector3D(0.0, 0.0, -xVector.getZ()));
      xVector.normalize();

      if (Double.isNaN(xVector.getX()))
      {
         throw new RuntimeException("FootstepUtils: Footsteps cannot face directly upwards");
      }

      double cosTheta = xVector.dot(new Vector3D(1.0, 0.0, 0.0));
      double sinTheta = xVector.dot(new Vector3D(0.0, 1.0, 0.0));
      double theta = Math.atan2(sinTheta, cosTheta);

      return theta;
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/setterUpper/GUISetterUpper.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.setterUpper;

import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public interface GUISetterUpper
{
   public abstract void setupGUI(SimulationConstructionSet scs);
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTNode.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

import java.util.ArrayList;

/**\
 * RRT Node is a member node of the rapidly randomized tree (RRT).
 * The RRT is the one of the sampling based motion planning tool. 
 * 
 * Every node has node data which of the dimension is larger than 0.
 * 
 * Except root node of the tree, every node has parent node.
 * For the parent node of the 'A' node, 'A' node is a child node of the parent node.
 * This structure excludes extra computing time consuming when <RRTTree> gets a path from root node to goal node.
 *
 * All class that inherits this class <RRTNode> must override two methods, isValid() and createNode().
 * 
 * @param nodeData
 * @param childRRTNode and ParentRRTNode
 * 
 * @author InhoLee 170217
 *
 */
public abstract class RRTNode implements RRTInterface 
{
   private NodeData nodeData;
   private ArrayList<RRTNode> childRRTNode;
   private RRTNode parentRRTNode;

   public RRTNode()
   {

   }

   public RRTNode(RRTNode node)
   {
      this.nodeData = node.nodeData;
      this.childRRTNode = node.childRRTNode;
      this.parentRRTNode = node.parentRRTNode;
   }

   public RRTNode(double[] rootData)
   {
      this.nodeData = new NodeData(rootData.length);
      this.nodeData.q = rootData;
      this.childRRTNode = new ArrayList<RRTNode>();
   }

   public RRTNode(int dimensionOfData)
   {
      this.nodeData = new NodeData(dimensionOfData);
      this.childRRTNode = new ArrayList<RRTNode>();
   }

   public final int getDimensionOfNodeData()
   {
      return nodeData.getDimension();
   }

   public final double getNodeData(int index)
   {
      return nodeData.getQ(index);
   }

   public final double getDistance(RRTNode targetNode)
   {
      return nodeData.distance(targetNode.nodeData);
   }

   public final void setNodeData(int index, double data)
   {
      nodeData.setQ(index, data);
   }
   
   public final void setNodeData(RRTNode copyNode)
   {
      for(int i=0;i<copyNode.getDimensionOfNodeData();i++)
      {
         nodeData.setQ(i, copyNode.getNodeData(i));
      }
   }

   public final void addChildNode(RRTNode childNode)
   {
      childRRTNode.add(childNode);
      childNode.setParentNode(this);
   }

   public final void setChildNode(int indexOfChild, RRTNode childNode)
   {
      childRRTNode.set(indexOfChild, childNode);
   }

   public final RRTNode getChildNode(int indexOfChild)
   {
      return childRRTNode.get(indexOfChild);
   }

   public final int getNumberOfChild()
   {
      return childRRTNode.size();
   }

   public final void setParentNode(RRTNode parentNode)
   {
      parentRRTNode = parentNode;
   }

   public final RRTNode getParentNode()
   {
      return parentRRTNode;
   }

   public class NodeData
   {
      private double[] q;
      private int dimension;

      private NodeData(int dimension)
      {
         this.q = new double[dimension];
         this.dimension = dimension;
      }

      private NodeData(NodeData nodeData)
      {
         this.q = new double[nodeData.getDimension()];
         for (int i = 0; i < nodeData.getDimension(); i++)
         {
            this.q[i] = nodeData.getQ(i);
         }
         this.dimension = nodeData.getDimension();
      }

      private final int getDimension()
      {
         return dimension;
      }

      private final double getQ(int index)
      {
         return q[index];
      }

      private final void setQ(int index, double value)
      {
         this.q[index] = value;
      }

      private final double distance(NodeData nodeData)
      {
         double ret = 0;
         for (int i = 0; i < dimension; i++)
         {
            ret = ret + (nodeData.getQ(i) - this.getQ(i)) * (nodeData.getQ(i) - this.getQ(i));
         }
         ret = Math.sqrt(ret);

         return ret;
      }
   }
   
   public abstract void setRandomNodeData();

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/globalParameters/DoubleGlobalParameter.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.globalParameters;

import us.ihmc.yoVariables.variable.YoDouble;

public class DoubleGlobalParameter extends GlobalParameter
{
   public DoubleGlobalParameter(String name, String description, double value, GlobalParameterChangedListener listener)
   {
      super(listener);

      yoVariable = new YoDouble(name, description, registry);
      ((YoDouble)yoVariable).set(value);

      if (changedListener != null)
         changedListener.globalParameterCreated(this);
   }

   protected DoubleGlobalParameter(String name, String description, GlobalParameter[] parents, GlobalParameterChangedListener listener)
   {
      super(parents, listener);

      yoVariable = new YoDouble(name, description,registry);

      if (changedListener != null)
         changedListener.globalParameterCreated(this);
   }

   public double getValue()
   {
      return  ((YoDouble)yoVariable).getDoubleValue();
   }

   protected int getNumberOfCharactersForDisplay()
   {
      return 9;
   }

   public void setOnlyIfChange(double value)
   {
      setOnlyIfChange(value, "");
   }

   public void setOnlyIfChange(double value, String comment)
   {
      verifyNoParents();

      // check if the value is the same as the current value
      if (value ==  ((YoDouble)yoVariable).getDoubleValue())
         return;
      else
         setDoubleValue(value, comment);
   }

   public void set(double value)
   {
      set(value, "");
   }

   public void set(double value, String comment)
   {
      verifyNoParents();
      setDoubleValue(value, comment);
   }

   @Override
   public String getValueInStringFormat()
   {
      String ret = Double.toString( ((YoDouble)yoVariable).getDoubleValue());

      return padWithSpaces(ret, numberOfCharactersForDisplay);
   }



   protected void setDoubleValue(double newValue, String comment)
   {
//    checkIfThereAreParents();

      double previousValue =  ((YoDouble)yoVariable).getDoubleValue();
      ((YoDouble)yoVariable).set(newValue);

      if (changedListener != null)
      {
         changedListener.doubleValueChanged(this, comment, previousValue, newValue);
      }

      updateChildren(comment);
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/ArmTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.ArmTrajectoryMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.ArmTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class ArmTrajectoryTask extends BehaviorAction
{
   private final ArmTrajectoryMessage armTrajectoryMessage;
   private final ArmTrajectoryBehavior armTrajectoryBehavior;

   public ArmTrajectoryTask(ArmTrajectoryMessage armTrajectoryMessage, ArmTrajectoryBehavior armTrajectoryBehavior)
   {
      super(armTrajectoryBehavior);
      this.armTrajectoryBehavior = armTrajectoryBehavior;
      this.armTrajectoryMessage = armTrajectoryMessage;
   }

   @Override
   protected void setBehaviorInput()
   {
      armTrajectoryBehavior.setInput(armTrajectoryMessage);
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/LocalObjectCommunicator.java<|end_filename|>
package us.ihmc.communication.net;

import java.io.IOException;
import java.util.ArrayList;
import java.util.LinkedHashMap;

public class LocalObjectCommunicator implements ObjectCommunicator
{
   private final LinkedHashMap<Class<?>, ArrayList<ObjectConsumer<?>>> listeners = new LinkedHashMap<Class<?>, ArrayList<ObjectConsumer<?>>>();
   
   private final ArrayList<GlobalObjectConsumer> globalListeners = new ArrayList<GlobalObjectConsumer>();
   
   @Override
   @SuppressWarnings("unchecked")
   public synchronized void consumeObject(Object object)
   {
      
      final ArrayList<ObjectConsumer<?>> classListeners = listeners.get(object.getClass());
      if(classListeners != null)
      {
         for(@SuppressWarnings("rawtypes") ObjectConsumer consumer : classListeners)
         {
            consumer.consumeObject(object);
         }
      }
   }

   @Override
   public void attachStateListener(ConnectionStateListener stateListener)
   {
      stateListener.connected();
   }

   @Override
   public <T> void attachListener(Class<T> clazz, ObjectConsumer<T> listener)
   {
      if(!listeners.containsKey(clazz))
      {
         listeners.put(clazz, new ArrayList<ObjectConsumer<?>>());
      }
      listeners.get(clazz).add(listener);
   }
   
   @Override
   public <T> void detachListener(Class<T> clazz, ObjectConsumer<T> listener)
   {
      if(listeners.containsKey(clazz))
      {
         ArrayList<ObjectConsumer<?>> listenerList = listeners.get(clazz);
         if (listenerList.contains(listener))
         {
            listenerList.remove(listener);
         }
      }
   }


   @Override
   public boolean isConnected()
   {
      return true;
   }

   @Override
   public void disconnect()
   {
   }

   @Override
   public void connect() throws IOException
   {
   }

	@Override
   public void attachGlobalListener(GlobalObjectConsumer listener)
	{
		globalListeners.add(listener);
	}
	
	@Override
   public void detachGlobalListener(GlobalObjectConsumer listener)
	{
	   if (globalListeners.contains(listener))
	   {
	      globalListeners.remove(listener);
	   }
	}
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullQuadrupedRobotModelFromDescription.java<|end_filename|>
package us.ihmc.robotModels;

import java.util.ArrayList;
import java.util.EnumMap;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import org.apache.commons.lang3.tuple.ImmutablePair;

import com.google.common.collect.BiMap;
import com.google.common.collect.HashBiMap;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.kinematics.JointLimit;
import us.ihmc.robotics.kinematics.JointLimitData;
import us.ihmc.robotics.partNames.JointRole;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.partNames.LimbName;
import us.ihmc.robotics.partNames.QuadrupedJointName;
import us.ihmc.robotics.partNames.QuadrupedJointNameMap;
import us.ihmc.robotics.robotDescription.JointDescription;
import us.ihmc.robotics.robotDescription.RobotDescription;
import us.ihmc.robotics.robotSide.QuadrantDependentList;
import us.ihmc.robotics.robotSide.RobotQuadrant;
import us.ihmc.robotics.robotSide.SegmentDependentList;

public class FullQuadrupedRobotModelFromDescription extends FullRobotModelFromDescription implements FullQuadrupedRobotModel
{
   private final BiMap<QuadrupedJointName, OneDoFJointBasics> jointNameOneDoFJointBiMap = HashBiMap.create();
   @Deprecated
   private final Map<QuadrupedJointName, JointLimit> jointLimits = new EnumMap<>(QuadrupedJointName.class);
   private final Map<OneDoFJointBasics, JointLimitData> jointLimitData = new HashMap<>();
   private final QuadrantDependentList<MovingReferenceFrame> soleFrames = new QuadrantDependentList<>();

   private QuadrantDependentList<EnumMap<LegJointName, OneDoFJointBasics>> legJointMaps;
   private QuadrantDependentList<ArrayList<OneDoFJointBasics>> legJointLists;

   private QuadrantDependentList<RigidBodyBasics> feet;
   private boolean initialized = false;

   private final RobotQuadrant[] robotQuadrants;

   public FullQuadrupedRobotModelFromDescription(RobotDescription description, QuadrupedJointNameMap sdfJointNameMap, String[] sensorLinksToTrack,
                                                 Map<QuadrupedJointName, JointLimit> jointLimits)
   {
      this(RobotQuadrant.values, description, sdfJointNameMap, sensorLinksToTrack);

      this.jointLimits.putAll(jointLimits);
   }

   public FullQuadrupedRobotModelFromDescription(RobotDescription description, QuadrupedJointNameMap sdfJointNameMap, String[] sensorLinksToTrack)
   {
      this(RobotQuadrant.values, description, sdfJointNameMap, sensorLinksToTrack);
   }

   public FullQuadrupedRobotModelFromDescription(RobotQuadrant[] robotQuadrants, RobotDescription description, QuadrupedJointNameMap sdfJointNameMap,
                                                 String[] sensorLinksToTrack)
   {
      super(description, sdfJointNameMap, sensorLinksToTrack);

      this.robotQuadrants = robotQuadrants;

      for (OneDoFJointBasics oneDoFJoint : getOneDoFJoints())
      {
         QuadrupedJointName quadrupedJointName = sdfJointNameMap.getJointNameForSDFName(oneDoFJoint.getName());
         jointNameOneDoFJointBiMap.put(quadrupedJointName, oneDoFJoint);

         // Assign default joint limits
         JointLimit jointLimit = new JointLimit(oneDoFJoint);
         jointLimits.put(quadrupedJointName, jointLimit);

         jointLimitData.put(oneDoFJoint, new JointLimitData(oneDoFJoint));
      }

      for (RobotQuadrant robotQuadrant : robotQuadrants)
      {
         RigidBodyTransform soleToParentTransform = sdfJointNameMap.getSoleToParentFrameTransform(robotQuadrant);
         MovingReferenceFrame soleFrame = MovingReferenceFrame
               .constructFrameFixedInParent(robotQuadrant.toString() + "SoleFrame", getEndEffectorFrame(robotQuadrant, LimbName.LEG), soleToParentTransform);
         soleFrames.put(robotQuadrant, soleFrame);
      }
   }

   private boolean hasQuadrant(RobotQuadrant quadrant)
   {
      for (RobotQuadrant robotQuadrant : robotQuadrants)
      {
         if (robotQuadrant == quadrant)
            return true;
      }

      return false;
   }

   @Override
   protected void mapRigidBody(JointDescription joint, OneDoFJointBasics inverseDynamicsJoint, RigidBodyBasics rigidBody)
   {
      initializeLists();

      super.mapRigidBody(joint, inverseDynamicsJoint, rigidBody);

      QuadrupedJointNameMap jointMap = (QuadrupedJointNameMap) sdfJointNameMap;
      for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
      {
         String jointBeforeFootName = jointMap.getJointBeforeFootName(robotQuadrant);

         if (jointBeforeFootName != null && jointBeforeFootName.equals(joint.getName()))
         {
            feet.set(robotQuadrant, rigidBody);
         }
      }

      JointRole jointRole = sdfJointNameMap.getJointRole(joint.getName());
      if (jointRole != null)
      {
         switch (jointRole)
         {
         case LEG:
            ImmutablePair<RobotQuadrant, LegJointName> legJointName = jointMap.getLegJointName(joint.getName());
            legJointMaps.get(legJointName.getLeft()).put(legJointName.getRight(), inverseDynamicsJoint);
            legJointLists.get(legJointName.getLeft()).add(inverseDynamicsJoint);
            break;
         }
      }
   }

   private void initializeLists()
   {
      if (!initialized)
      {
         legJointMaps = QuadrantDependentList.createListOfEnumMaps(LegJointName.class);
         legJointLists = QuadrantDependentList.createListOfArrayLists();

         feet = new QuadrantDependentList<>();
         initialized = true;
      }
   }

   @Override
   public MovingReferenceFrame getFrameAfterLegJoint(RobotQuadrant robotQuadrant, LegJointName legJointName)
   {
      if (hasQuadrant(robotQuadrant))
         return getLegJoint(robotQuadrant, legJointName).getFrameAfterJoint();
      else
         return null;
   }

   @Override
   public OneDoFJointBasics getLegJoint(RobotQuadrant robotQuadrant, LegJointName legJointName)
   {
      if (hasQuadrant(robotQuadrant))
         return legJointMaps.get(robotQuadrant).get(legJointName);
      else
         return null;
   }

   /* (non-Javadoc)
       * @see us.ihmc.modelFileLoaders.SdfLoader.FullQuadrupedRobotModel#getFoot(us.ihmc.robotics.robotSide.RobotQuadrant)
       */
   @Override
   public RigidBodyBasics getFoot(RobotQuadrant robotQuadrant)
   {
      return feet.get(robotQuadrant);
   }

   @Override
   public RigidBodyBasics getEndEffector(RobotQuadrant robotQuadrant, LimbName limbName)
   {
      switch (limbName)
      {
      case LEG:
         return feet.get(robotQuadrant);
      default:
         throw new RuntimeException("Unknown end effector");
      }
   }

   @Override
   public RigidBodyBasics getBody()
   {
      return getRootBody();
   }

   @Override
   public MovingReferenceFrame getEndEffectorFrame(RobotQuadrant robotQuadrant, LimbName limbName)
   {
      if (hasQuadrant(robotQuadrant))
         return getEndEffector(robotQuadrant, limbName).getParentJoint().getFrameAfterJoint();
      else
         return null;
   }

   @Override
   public MovingReferenceFrame getSoleFrame(RobotQuadrant robotQuadrant)
   {
      return soleFrames.get(robotQuadrant);
   }

   @Override
   public SegmentDependentList<RobotQuadrant, MovingReferenceFrame> getSoleFrames()
   {
      return soleFrames;
   }

   /* (non-Javadoc)
    * @see us.ihmc.modelFileLoaders.SdfLoader.FullQuadrupedRobotModel#getNameForOneDoFJoint(us.ihmc.robotics.screwTheory.OneDoFJoint)
    */
   @Override
   public QuadrupedJointName getNameForOneDoFJoint(OneDoFJointBasics oneDoFJoint)
   {
      return jointNameOneDoFJointBiMap.inverse().get(oneDoFJoint);
   }

   /* (non-Javadoc)
    * @see us.ihmc.modelFileLoaders.SdfLoader.FullQuadrupedRobotModel#getJointLimit(us.ihmc.modelFileLoaders.SdfLoader.partNames.QuadrupedJointName)
    */
   @Deprecated
   @Override
   public JointLimit getJointLimit(QuadrupedJointName jointName)
   {
      return jointLimits.get(jointName);
   }

   @Override
   public JointLimitData getJointLimitData(OneDoFJointBasics joint)
   {
      return jointLimitData.get(joint);
   }

   @Override
   public List<OneDoFJointBasics> getLegJointsList(RobotQuadrant robotQuadrant)
   {
      return legJointLists.get(robotQuadrant);
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/pointClouds/shape/ExpectationMaximizationFitter.java<|end_filename|>
package us.ihmc.sensorProcessing.pointClouds.shape;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import org.apache.commons.math3.distribution.NormalDistribution;

import georegression.geometry.UtilPlane3D_F64;
import georegression.metric.Distance3D_F64;
import georegression.struct.plane.PlaneGeneral3D_F64;
import georegression.struct.plane.PlaneNormal3D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;

public class ExpectationMaximizationFitter
{
   public static List<PlaneGeneral3D_F64> fit(int numPlanes, List<Point3D_F64> allPoints, ScoringFunction<PlaneGeneral3D_F64, Point3D_F64> scorer, int iterations)
   {
      return fit(numPlanes, new Random(), allPoints, scorer, iterations);
   }

   public static List<PlaneGeneral3D_F64> fit(int numPlanes, Random rand, List<Point3D_F64> allPoints, ScoringFunction<PlaneGeneral3D_F64, Point3D_F64> scorer, int iterations)
   {
      List<PlaneGeneral3D_F64> planes = new ArrayList<PlaneGeneral3D_F64>();
      for (int i = 0; i < numPlanes; i++)
      {
         planes.add(new PlaneGeneral3D_F64(rand.nextDouble(), rand.nextDouble(), rand.nextDouble(), rand.nextDouble()));
      }
      System.out.println(planes.size());
      return fit(planes, allPoints, scorer, iterations);
   }

   public static List<PlaneGeneral3D_F64> fit(List<PlaneGeneral3D_F64> planes, List<Point3D_F64> allPoints, ScoringFunction<PlaneGeneral3D_F64, Point3D_F64> scorer, int iterations)
   {
      FitPlaneWeighted3D_F64 fitter = new FitPlaneWeighted3D_F64();
      Point3D_F64 fitOrigin = new Point3D_F64();
      Vector3D_F64 fitNormal = new Vector3D_F64();

      double[][] weights = new double[planes.size()][allPoints.size()];

      for (; iterations > 0; iterations--)
      {
         getWeights(weights, planes, allPoints, scorer);

         planes.clear();
         for (int i = 0; i < weights.length; i++)
         {
            fitter.svd(allPoints, weights[i], fitOrigin, fitNormal);
            planes.add(UtilPlane3D_F64.convert(new PlaneNormal3D_F64(fitOrigin, fitNormal), null));

         }
      }

      return planes;
   }

   public static double[][] getWeights(double[][] weights, List<PlaneGeneral3D_F64> planes, List<Point3D_F64> allPoints, ScoringFunction<PlaneGeneral3D_F64, Point3D_F64> scorer)
   {
      if(weights == null)
         weights = new double[planes.size()][allPoints.size()];
      
      double[] probabilities = new double[planes.size()];
      double sumProbability;

      for (int i = 0; i < allPoints.size(); i++)
      {

         sumProbability = 0;
         for (int j = 0; j < planes.size(); j++)
         {
            probabilities[j] = scorer.score(planes.get(j), allPoints.get(i));
            sumProbability += probabilities[j];
         }

         for (int j = 0; j < probabilities.length; j++)
         {
            if (sumProbability != 0)
            {
               probabilities[j] /= sumProbability;
               weights[j][i] = probabilities[j];
            }
            else
            {
               weights[j][i] = 0;
            }
         }
      }
      
      return weights;
   }

   public static interface ScoringFunction <T,E>
   {
      public double score(T model, E sample);
   }

   public static ScoringFunction<PlaneGeneral3D_F64, Point3D_F64> getGaussianSqauresMixedError(double sigma)
   {
      final NormalDistribution d = new NormalDistribution(0, sigma);
      final double distAmplitude = d.density(0);

      return new ScoringFunction<PlaneGeneral3D_F64, Point3D_F64>()
      {
         public double score(PlaneGeneral3D_F64 plane, Point3D_F64 point)
         {
            double e = Distance3D_F64.distance(plane, point);
            // gaussian best for fitting, small 1/(e*e) component for large errors 
            return d.density(e) + distAmplitude * 1e-12 / (1e-10 + (e * e));
         }
      };
   }
   
   public static ScoringFunction<PlaneNormal3D_F64, Point3D_F64> getNormalSingularityError()
   {
      return new ScoringFunction<PlaneNormal3D_F64, Point3D_F64>()
      {
         public double score(PlaneNormal3D_F64 plane, Point3D_F64 diff)
         {
            double e = plane.n.x * diff.x + plane.n.y * diff.y + plane.n.z * diff.z;
            e /= diff.norm();
            return 1 / (1e-2 + (e));
         }
      };
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/externalcontroller/SensorInterface.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.externalcontroller;

interface SensorInterface
{
   public String getYoVariableOrder();

   public int getNumberOfVariables();

   public double[] getMessageValues();

   public void setTau(double tau);



}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/posePlayback/ValkyrieWarmupPoseSequencePacket.java<|end_filename|>
package us.ihmc.valkyrie.posePlayback;

import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;

import us.ihmc.avatar.posePlayback.PlaybackPose;
import us.ihmc.avatar.posePlayback.PlaybackPoseSequence;
import us.ihmc.avatar.posePlayback.PlaybackPoseSequenceReader;
import us.ihmc.avatar.posePlayback.PosePlaybackPacket;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.valkyrie.configuration.YamlWithIncludesLoader;

public class ValkyrieWarmupPoseSequencePacket implements PosePlaybackPacket
{
   private final double trajectoryTime = 2.0;
   private final double delayTime = 0.25;
   private final double initialGainScaling;
   
   private final LinkedHashMap<OneDoFJointBasics, Double> kps = new LinkedHashMap<>();
   private final LinkedHashMap<OneDoFJointBasics, Double> kds = new LinkedHashMap<>();
   private final PlaybackPoseSequence playbackPoseSequence;

   public ValkyrieWarmupPoseSequencePacket(FullHumanoidRobotModel fullRobotModel, double initialGainScaling)
   {
      @SuppressWarnings("unchecked")
      Map<String, Map<String, Double>> gainMap = (Map<String, Map<String, Double>>) YamlWithIncludesLoader.load("standPrep", "gains.yaml");

      this.initialGainScaling = initialGainScaling;
      playbackPoseSequence = new PlaybackPoseSequence(fullRobotModel);
      
      for (OneDoFJointBasics joint : fullRobotModel.getOneDoFJoints())
      {
         Map<String, Double> jointGains = gainMap.get(joint.getName());
         
         kps.put(joint, jointGains.get("kp"));
         kds.put(joint, jointGains.get("kd"));
      }

      List<Double> listOfHipAdductorPoses = new ArrayList<>();
      listOfHipAdductorPoses.add(0.0);
      listOfHipAdductorPoses.add(-0.4);
      listOfHipAdductorPoses.add(0.0);
      listOfHipAdductorPoses.add(-0.4);
      listOfHipAdductorPoses.add(0.0);

      for (Double angle : listOfHipAdductorPoses)
      {
         LinkedHashMap<OneDoFJointBasics, Double> map = new LinkedHashMap<>();
         for (RobotSide robotSide : RobotSide.values)
         {
            map.put(fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_ROLL), angle);
         }
         
         PlaybackPose playbackPose = new PlaybackPose(map);
         playbackPose.setPlaybackDelayBeforePose(delayTime);
         playbackPose.setPlayBackDuration(trajectoryTime);
         
         playbackPoseSequence.addPose(playbackPose);
      }
   }
   
   
   public ValkyrieWarmupPoseSequencePacket(String filename, FullRobotModel fullRobotModel, double initialGainScaling)
   {
      @SuppressWarnings("unchecked")
      Map<String, Map<String, Double>> gainMap = (Map<String, Map<String, Double>>) YamlWithIncludesLoader.load("standPrep", "gains.yaml");

      this.initialGainScaling = initialGainScaling;
//      playbackPoseSequence = PlaybackPoseSequenceReader.readFromFile(fullRobotModel, filename);
      playbackPoseSequence = PlaybackPoseSequenceReader.readFromInputStream(fullRobotModel, getClass().getClassLoader().getResourceAsStream(filename));
      for (OneDoFJointBasics joint : fullRobotModel.getOneDoFJoints())
      {
         Map<String, Double> jointGains = gainMap.get(joint.getName());
         
         System.out.println(joint.getName());
         
         kps.put(joint, jointGains.get("kp"));
         kds.put(joint, jointGains.get("kd"));
      }
   }

   @Override
   public Map<OneDoFJointBasics, Double> getJointKps()
   {
      return kps;
   }

   @Override
   public Map<OneDoFJointBasics, Double> getJointKds()
   {
      return kds;
   }

   @Override
   public PlaybackPoseSequence getPlaybackPoseSequence()
   {
      return playbackPoseSequence;
   }

   @Override
   public double getInitialGainScaling()
   {
      return initialGainScaling;
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/streamingData/HumanoidGlobalDataProducer.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.streamingData;

import controller_msgs.msg.dds.CapturabilityBasedStatus;
import controller_msgs.msg.dds.HandJointAnglePacket;
import controller_msgs.msg.dds.RobotConfigurationData;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.streamingData.GlobalDataProducer;

public class HumanoidGlobalDataProducer extends GlobalDataProducer
{
   public HumanoidGlobalDataProducer(PacketCommunicator communicator)
   {
      super(communicator);
   }

   /**
    * Special method to directly send RobotConfigurationData, skipping the queue
    * @param robotConfigData
    */
   public void send(RobotConfigurationData robotConfigData)
   {
      communicator.send(robotConfigData);
   }

   /**
    * Special method to directly send HandJointAnglePacket, skipping the queue
    * @param handJointAnglePacket
    */
   public void send(HandJointAnglePacket handJointAnglePacket)
   {
      communicator.send(handJointAnglePacket);
   }


   /**
    * Special method to directly send CapturabilityBasedStatus, skipping the queue
    *
    * @param status
    */
   public void send(CapturabilityBasedStatus status)
   {
      communicator.send(status);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/SimulationToDispatch.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;

import us.ihmc.simulationconstructionset.Simulation;
import us.ihmc.simulationconstructionset.SimulationConstructor;

public class SimulationToDispatch
{
   private final Simulation simulation;
   private final String description;
   private String resultsString;

   private final SimulationConstructor constructor;

   private final String[] structuralParameterNames;
   private final double[] structuralParameterValues;
   private final String[] inputStateVariableNames, outputStateVariableNames;
   private double[] inputStateVals;

   private double[] finalState;
   private boolean simulationFinished = false;

   private final DispatchDoneListener dispatchDoneListener;

   public SimulationToDispatch(Simulation simulation, String description, SimulationConstructor constructor, String[] structuralParameterNames,
                               double[] structuralParameterValues, String[] inputStateVars, double[] newVals, String[] outputStateVars,
                               DispatchDoneListener dispatchDoneListener)
   {
      this.simulation = simulation;
      this.description = description;
      this.constructor = constructor;

      this.structuralParameterNames = structuralParameterNames;
      this.structuralParameterValues = structuralParameterValues;

      this.inputStateVariableNames = inputStateVars;
      this.outputStateVariableNames = outputStateVars;

      this.finalState = new double[outputStateVars.length];

      setStateVals(newVals);
      this.dispatchDoneListener = dispatchDoneListener;
   }

   public DispatchDoneListener getDispatchDoneListener()
   {
      return dispatchDoneListener;
   }

   public Simulation getSimulation()
   {
      return simulation;
   }

   public SimulationConstructor getConstructor()
   {
      return constructor;
   }

   public String[] getStructuralParameterNames()
   {
      return structuralParameterNames;
   }

   public double[] getStructuralParameterValues()
   {
      return structuralParameterValues;
   }

   public double[] getFinalState()
   {
      return finalState;
   }

   public void setStateVals(double[] newVals)
   {
      this.inputStateVals = new double[newVals.length];

      for (int i = 0; i < newVals.length; i++)
      {
         inputStateVals[i] = newVals[i];
      }
   }

   public void setResultsString(String resultsString)
   {
      this.resultsString = resultsString;
   }

   public String getResultsString()
   {
      return resultsString;
   }

   public String getDescription()
   {
      return description;
   }

   public void setFinalState(double[] finalState)
   {
      this.finalState = finalState;
   }

   public double[] getInputState()
   {
      return inputStateVals;
   }

   public String[] getInputStateVariableNames()
   {
      return inputStateVariableNames;
   }

   public String[] getOutputStateVariableNames()
   {
      return outputStateVariableNames;
   }

   public void setSimulationFinished()
   {
      this.simulationFinished = true;
   }

   public boolean isSimulationFinished()
   {
      return simulationFinished;
   }

}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/subscriber/RosImuSubscriber.java<|end_filename|>
package us.ihmc.utilities.ros.subscriber;

import java.net.URI;
import java.net.URISyntaxException;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.RosTools;

public abstract class  RosImuSubscriber extends AbstractRosTopicSubscriber<sensor_msgs.Imu> 
{
   /*
    * header: 
        seq: 676
        stamp: 
          secs: 1424311090
          nsecs: 671066000
        frame_id: /multisense/accel
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0
      orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
      angular_velocity: 
        x: -0.00580321995013
        y: -0.00687223413779
        z: 0.00488692192639
      angular_velocity_covariance: [8.79376936e-06, -3.56007627e-07, 2.22611968e-07, -3.56007627e-07, 8.78939245e-06, -8.08367486e-07, 1.33981577e-05, 2.22611968e-07, -8.08367486e-07]
      linear_acceleration: 
        x: -9.45361105359
        y: -0.588399023381
        z: -3.76575363273
      linear_acceleration_covariance: [0.000698179077, 2.46789341e-06, -2.50549745e-06, 2.46789341e-06, 0.000502177646, 5.26265558e-05, -2.50549745e-06, 5.26265558e-05, 0.000922796937]
    */
   private long timeStamp = 0;
   private int seq_id;
   private String frameId;

   private final us.ihmc.euclid.tuple4D.Quaternion orientationEstimate = new us.ihmc.euclid.tuple4D.Quaternion();
   private final Vector3D angularVelocity = new Vector3D();
   private final Vector3D linearAcceleration = new Vector3D();
   
   public RosImuSubscriber()
   {
      super(sensor_msgs.Imu._TYPE);
   }

   @Override
   public synchronized void onNewMessage(sensor_msgs.Imu message)
   {
      this.timeStamp = message.getHeader().getStamp().totalNsecs();
      this.seq_id = message.getHeader().getSeq();
      this.frameId = message.getHeader().getFrameId();
      RosTools.packRosQuaternionToEuclidQuaternion(message.getOrientation(), this.orientationEstimate);
      RosTools.packRosVector3ToEuclidTuple3D(message.getAngularVelocity(), this.angularVelocity);
      RosTools.packRosVector3ToEuclidTuple3D(message.getLinearAcceleration(), this.linearAcceleration);
      onNewMessage(timeStamp, seq_id, orientationEstimate, angularVelocity, linearAcceleration);
   }
   
   public String getFrameId()
   {
      return frameId;
   }
   

   protected abstract void onNewMessage(long timeStamp, int seqId, Quaternion orientation, Vector3D angularVelocity, Vector3D linearAcceleration);
   
   public static void main(String[] arg) throws URISyntaxException
   {
      RosMainNode mainNode = new RosMainNode(new URI("http://localhost:11311"), "testImu");
      mainNode.attachSubscriber("/multisense/imu/imu_data", new RosImuSubscriber()
      {
         
         @Override
         protected void onNewMessage(long timeStamp, int seqId, Quaternion orientation, Vector3D angularVelocity, Vector3D linearAcceleration)
         {
            System.out.println("Gravity:"+ linearAcceleration);
         }
      });
      mainNode.execute();
   }

}




<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/whiteBoard/DoSomeWorker.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.whiteBoard;

public interface DoSomeWorker
{
   public abstract void doSomeWork();
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/genericQuadruped/model/GenericQuadrupedSDFParameters.java<|end_filename|>
package us.ihmc.exampleSimulations.genericQuadruped.model;

import us.ihmc.modelFileLoaders.SdfLoader.SDFParameters;
import us.ihmc.tools.io.resources.ResourceTools;

import java.io.InputStream;
import java.nio.file.Path;
import java.nio.file.Paths;

public class GenericQuadrupedSDFParameters implements SDFParameters
{
   private static final String SDF_MODEL_NAME = "genericQuadruped";
   private static final String RESOURCE_DIRECTORY = "models/";
   private static final String[] RESOURCE_DIRECTORIES = new String[] {RESOURCE_DIRECTORY};
   private static final Path SDF_MODEL_PATH = Paths.get("models", "genericQuadruped", "sdf", SDF_MODEL_NAME + ".sdf");
   
   @Override
   public Path getSdfModelPath()
   {
      return SDF_MODEL_PATH;
   }

   @Override
   public String getSdfFilePath()
   {
      return SDF_MODEL_PATH.toString();
   }

   @Override
   public String getSdfModelName()
   {
      return SDF_MODEL_NAME;
   }

   @Override
   public String[] getResourceDirectories()
   {
      return RESOURCE_DIRECTORIES;
   }

   @Override
   public String getResourceDirectory()
   {
      return RESOURCE_DIRECTORY;
   }

   @Override
   public InputStream getSdfAsInputStream()
   {
      return ResourceTools.openStreamSystem(SDF_MODEL_PATH);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/planarRegionEnvironments/VaryingHeightTiledGroundEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.planarRegionEnvironments;

import us.ihmc.euclid.tools.EuclidCoreRandomTools;
import us.ihmc.graphicsDescription.appearance.YoAppearance;

import java.util.Random;

public class VaryingHeightTiledGroundEnvironment extends PlanarRegionEnvironmentInterface
{
   private final Random random = new Random(2390L);

   public VaryingHeightTiledGroundEnvironment(double tileWidth, int courseLengthInNumberOfTiles, int courseWidthInNumberOfTiles, double nominalGroundHeight, double maxHeightVariation)
   {
      generator.identity();
      double startCoordinateX = - 0.5 * tileWidth * (courseLengthInNumberOfTiles - 1);
      double startCoordinateY = - 0.5 * tileWidth * (courseWidthInNumberOfTiles- 1);

      for (int i = 0; i < courseLengthInNumberOfTiles; i++)
      {
         for (int j = 0; j < courseWidthInNumberOfTiles; j++)
         {
            double tilePositionX = startCoordinateX + tileWidth * i;
            double tilePositionY = startCoordinateY + tileWidth * j;
            double tilePositionZ = nominalGroundHeight + EuclidCoreRandomTools.nextDouble(random, 0.5 * maxHeightVariation);

            generator.translate(tilePositionX, tilePositionY, tilePositionZ);
            generator.addRectangle(tileWidth, tileWidth);
            generator.identity();
         }
      }

      addPlanarRegionsToTerrain(YoAppearance.Grey());
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/initialSetup/SquaredUpDRCDemo01OutsidePen.java<|end_filename|>
package us.ihmc.atlas.initialSetup;

import us.ihmc.avatar.initialSetup.OffsetAndYawRobotInitialSetup;
import us.ihmc.euclid.tuple3D.Vector3D;

public class SquaredUpDRCDemo01OutsidePen extends OffsetAndYawRobotInitialSetup
{
   private static final Vector3D additionalOffset = new Vector3D(3.0, 12.0, 0.0);
   private static final double yaw = Math.PI/2.0;
   
   public SquaredUpDRCDemo01OutsidePen(double groundZ)
   {
      super(groundZ, additionalOffset, yaw);
   }
}



<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTTree.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

import java.util.ArrayList;
import java.util.Random;

import us.ihmc.commons.PrintTools;

/**
 * Every tree has a root node.
 * When the initiate this RRTTree class, user must define the following parameters for a tree. * 
 * @param root node.
 * @param searching bound for the random exploring expand.
 * @param step length.
 * 
 * This RRT class provides expanding and find a path from selected node to the root node of this tree.
 * 
 * The basic expanding process is 
 * 1. get random node in boundary.
 * 2. from the random node, find near node among the all nodes in tree.
 * 3. create a new node between the random node and the near node. If they are close as much as the step length, the random node become new node.
 * 4. add the new node to tree
 * 
 * <expandTree> find random node with uniform random value in boundary.
 * Voronoi diagram will be added in near future.
 * 
 * @author InhoLee 170217
 *
 */
public class RRTTree
{
   protected RRTNode rootNode;
   protected RRTNode nearNode;
   protected RRTNode newNode;
   protected double stepLength;

   protected RRTNode upperBoundNode;
   protected RRTNode lowerBoundNode;
   protected ArrayList<RRTNode> pathNode = new ArrayList<RRTNode>();
   
   protected ArrayList<RRTNode> wholeNodes = new ArrayList<RRTNode>();
   public ArrayList<RRTNode> failNodes = new ArrayList<RRTNode>();

   protected RRTNode nodeCreator;
   
   // numberOfNodes, ArrayList<RRTNode> nodes. every node of the nodes has its parent node.

   public RRTTree(RRTNode rootNode)
   {
      this.rootNode = rootNode;
      nodeCreator = rootNode.createNode();
      wholeNodes.add(this.rootNode);
   }

   public void setStepLength(double length)
   {
      this.stepLength = length;
   }

   public double getStepLength()
   {
      return this.stepLength;
   }

   public void setUpperBound(RRTNode upperBoundNode)
   {
      this.upperBoundNode = upperBoundNode;
   }

   public void setLowerBound(RRTNode lowerBoundNode)
   {
      this.lowerBoundNode = lowerBoundNode;
   }

   // User can override
   public double getMatric(RRTNode nodeOne, RRTNode nodeTwo)
   {
      return nodeOne.getDistance(nodeTwo);
   }

   public RRTNode getRandomNode()
   {
      warningMessage();
      RRTNode randomNode = nodeCreator.createNode();
      Random randomManager = new Random();
      for (int i = 0; i < randomNode.getDimensionOfNodeData(); i++)
      {
         double randonValue = randomManager.nextDouble() * (this.upperBoundNode.getNodeData(i) - this.lowerBoundNode.getNodeData(i))
               + this.lowerBoundNode.getNodeData(i);
         randomNode.setNodeData(i, randonValue);
      }
      return randomNode;
   }

   public boolean expandTree()
   {
      RRTNode node = getRandomNode();
      updateNearNodeForTargetNode(node);
      this.newNode = getNewNode(node);

      return addNewNode();
   }

   public boolean expandTree(RRTNode node)
   {
      updateNearNodeForTargetNode(node);
      this.newNode = getNewNode(node);
      return addNewNode();
   }

   public void updateNearNodeForTargetNode(RRTNode targetNode)
   {
      RRTNode optNode = this.wholeNodes.get(0);
      RRTNode curNode;

      double optMatric = Double.MAX_VALUE;
      double curMatric;
      
      for(int i=0;i<this.wholeNodes.size();i++)
      {
         curNode = this.wholeNodes.get(i);
         curMatric = getMatric(curNode, targetNode);
         if (curMatric < optMatric)
         {
            optMatric = curMatric;
            optNode = curNode;
         }         
      }
      
      this.nearNode = optNode;
   }
   

   public RRTNode getNewNode(RRTNode targetNode)
   {
      RRTNode newNode = nodeCreator.createNode();
      if (targetNode.getDistance(nearNode) < getStepLength())
      {
         newNode = targetNode;
      }
      else
      {
         for (int i = 0; i < targetNode.getDimensionOfNodeData(); i++)
         {
            double normalizedDistance = (targetNode.getNodeData(i) - nearNode.getNodeData(i)) / nearNode.getDistance(targetNode);
            newNode.setNodeData(i, nearNode.getNodeData(i) + getStepLength() * normalizedDistance);
         }
      }
      return newNode;
   }

   public boolean addNewNode()
   {
      if (this.newNode.isValidNode() == true)
      {
         RRTValidConnection rrtValidConnection = new RRTValidConnection(this.nearNode, this.newNode);
         if (rrtValidConnection.isValidConnection())
         {
            nearNode.addChildNode(this.newNode);
            wholeNodes.add(newNode);
            return true;
         }
      }
      else
      {
         // PrintTools.info("The newly created node is unvalid");
      }
      return false;
   }

   public void updatePathNode(RRTNode endNodeOfPath)
   {
      this.pathNode.clear();

      ArrayList<RRTNode> pathNodeOne = new ArrayList<RRTNode>();
      RRTNode singleNode = endNodeOfPath;
      while (true)
      {
         pathNodeOne.add(singleNode);
         if (singleNode == rootNode)
         {
            PrintTools.info("node Path is completely built");
            break;
         }
         singleNode = singleNode.getParentNode();
      }

      int pathSize = pathNodeOne.size();
      for (int i = 0; i < pathSize; i++)
      {
         pathNode.add(pathNodeOne.get(pathSize - i - 1));
      }
   }

   private void warningMessage()
   {
      if (upperBoundNode == null || lowerBoundNode == null)
      {
         PrintTools.info("Searching boundary should be defined (Use methods setUpperBound and setLowerBound)");
      }
      if (stepLength == 0)
      {
         PrintTools.info("Step length should be defined (Use methods setStepLength)");
      }
   }

   public RRTNode getRootNode()
   {
      return rootNode;
   }

   public RRTNode getNearNode()
   {
      return nearNode;
   }

   public RRTNode getNewNode()
   {
      return newNode;
   }

   public ArrayList<RRTNode> getPathNode()
   {
      return pathNode;
   }
   
   public ArrayList<RRTNode> getWholeNode()
   {
      return wholeNodes;
   }
   
   
   
   
   /*
    * will be deleted.
    */

   public void updateNearNodeForTargetNodeOld(RRTNode targetNode)
   {
      RRTNode[] nodeOnCurrentLevel;
      RRTNode[] nodeOnFutureLevel;
      int currentLevel = 0;
      int numberOfCurrentLevel = 0;
      int numberOfFutureLevel = 0;
      int numberOfSearch = 0;

      RRTNode optNode = this.rootNode;
      RRTNode curNode = this.rootNode;

      double optMatric = Double.MAX_VALUE;
      double curMatric = getMatric(targetNode, curNode);

      if (curMatric < optMatric)
      {
         optMatric = curMatric;
         optNode = curNode;
      }

      numberOfCurrentLevel++;
      nodeOnCurrentLevel = new RRTNode[numberOfCurrentLevel];
      nodeOnCurrentLevel[currentLevel] = this.rootNode;

      while (true)
      {
         numberOfFutureLevel = 0;
         for (int i = 0; i < numberOfCurrentLevel; i++)
         {
            numberOfFutureLevel = numberOfFutureLevel + nodeOnCurrentLevel[i].getNumberOfChild();
         }

         if (numberOfFutureLevel == 0)
         {
            break;
         }

         int tempNumber = 0;
         nodeOnFutureLevel = new RRTNode[numberOfFutureLevel];
         for (int i = 0; i < numberOfCurrentLevel; i++)
         {
            for (int j = 0; j < nodeOnCurrentLevel[i].getNumberOfChild(); j++)
            {
               nodeOnFutureLevel[tempNumber + j] = nodeOnCurrentLevel[i].getChildNode(j);
               curNode = nodeOnFutureLevel[tempNumber + j];

               curMatric = getMatric(targetNode, curNode);
               if (curMatric < optMatric)
               {
                  optMatric = curMatric;
                  optNode = curNode;
               }
               numberOfSearch++;
            }
            tempNumber = tempNumber + nodeOnCurrentLevel[i].getNumberOfChild();
         }

         numberOfCurrentLevel = numberOfFutureLevel;
         nodeOnCurrentLevel = new RRTNode[numberOfCurrentLevel];
         for (int i = 0; i < numberOfCurrentLevel; i++)
         {
            nodeOnCurrentLevel[i] = nodeOnFutureLevel[i];
         }

         currentLevel++;
      }
      this.nearNode = optNode;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/SliderboardTools.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

import javax.sound.midi.MidiDevice;
import javax.sound.midi.MidiUnavailableException;
import javax.sound.midi.Receiver;
import javax.sound.midi.Transmitter;

import us.ihmc.commons.MathTools;

public class SliderboardTools
{
   /**
    * Indicates that the MIDI command starting with this byte is a control command.
    */
   public static final byte CONTROL_BYTE = 0b1011;

   /**
    * Indicates the MIDI channel for a MIDI message.
    */
   public static final byte CHANNEL_ZERO = 0b0000;

   /**
    * MIDI message status for a control command on MIDI channel 0.
    */
   public static final short STATUS = (CONTROL_BYTE << 4) | CHANNEL_ZERO;

   /**
    * Maximum value for a valid data byte to be sent as part of a MIDI message.
    */
   public static final int MAX_VALUE = 0b01111111;

   public static double toSliderPercent(int dataByte)
   {
      return MathTools.clamp(((double) dataByte) / MAX_VALUE, 0.0, 1.0);
   }

   public static boolean toButtonStatus(int dataByte)
   {
      return dataByte == MAX_VALUE;
   }

   public static int toDataByte(double sliderPercent)
   {
      return (int) (MAX_VALUE * MathTools.clamp(sliderPercent, 0.0, 1.0));
   }

   public static int toDataByte(boolean status)
   {
      return status ? MAX_VALUE : 0;
   }

   public static boolean infoMatchesSliderboard(MidiDevice.Info info, String sliderBoardName)
   {
      String name = info.getName();
      String description = info.getDescription();
      return name.contains(sliderBoardName) || description.contains(sliderBoardName);
   }

   public static Transmitter getTransmitter(MidiDevice current)
   {
      if (current.getMaxTransmitters() != 0)
      {
         try
         {
            if (!current.isOpen())
            {
               current.open();
            }
         }
         catch (MidiUnavailableException e)
         {
            return null;
         }

         try
         {
            return current.getTransmitter();
         }
         catch (MidiUnavailableException e)
         {
            current.close();
         }
      }

      return null;
   }

   public static Receiver getReceiver(MidiDevice current)
   {
      if (current.getMaxReceivers() != 0)
      {
         try
         {
            if (!current.isOpen())
            {
               current.open();
            }
         }
         catch (MidiUnavailableException e)
         {
            return null;
         }

         try
         {
            return current.getReceiver();
         }
         catch (MidiUnavailableException e)
         {
            current.close();
         }
      }

      return null;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/TotalWrenchCalculator.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.Collection;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.spatial.Wrench;
import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly;

public class TotalWrenchCalculator
{
   private final Wrench temporaryWrench = new Wrench();

   public void computeTotalWrench(Wrench totalGroundReactionWrenchToPack, Collection<? extends WrenchReadOnly> wrenches, ReferenceFrame referenceFrame)
   {
      totalGroundReactionWrenchToPack.setToZero(referenceFrame, referenceFrame);

      for (WrenchReadOnly wrench : wrenches)
      {
         temporaryWrench.setIncludingFrame(wrench);
         temporaryWrench.changeFrame(referenceFrame);
         temporaryWrench.setBodyFrame(referenceFrame);
         totalGroundReactionWrenchToPack.add(temporaryWrench);
      }
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/construct/ConstructOctreeLeaf_I32.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.construct;

import java.util.ArrayList;
import java.util.List;

import georegression.metric.Intersection3D_I32;
import georegression.struct.point.Point3D_I32;
import georegression.struct.shapes.Box3D_I32;

/**
 * Constructs an octree by adding points to the smallest possible leaf which contains
 * the point.  The smallest possible leaf is 1x1x1 wide and will thus have the same coordinate
 * as the point which was added to it.
 *
 * When a new point is added it is only added to the leaf node and not any of the other nodes on the
 * path from the root to the leaf.  This constructor is geared towards maps, where all the information
 * of interest can be found in leafs which are the smallest possible size.
 *
 * @author <NAME>
 */
public class ConstructOctreeLeaf_I32 extends ConstructOctree_I32 {

	/**
	 * Adds the point to the smallest leaf at this point.
	 *
	 * NOTE: This function is of dubious value since all points at the leaf will have
	 * the same coordinate so in many situations there is no point in keeping a list.
	 * Instead the leaf itself could be used.  See {@link #addLeaf(georegression.struct.point.Point3D_I32)}.
	 */
	@Override
	public Octree_I32 addPoint(Point3D_I32 point, Object data) {

		// declare the structure which stores the point and data
		Octree.Info<Point3D_I32> info = storageInfo.grow();
		info.point = point;
		info.userData = data;

		Octree_I32 leaf = addLeaf(point);
		if( leaf != null )
			leaf.points.add(info);

		return leaf;
	}

	/**
	 * Finds the smallest leaf at this point.  If none exist then a new one is created.  New nodes in
	 * the graph are created as it searches.  A reference to the leaf is returned.
	 * @param point Point which is contained by the desired smallest leaf.
	 * @return Smallest possible leaf at that point.
	 */
	public Octree_I32 addLeaf( Point3D_I32 point ) {
		if( !tree.contained(point) )
			return null;

		Octree_I32 node = tree;

		while( true ) {
			if( node.isSmallest() ) {
				return node;
			} else {
				if( node.isLeaf() ) {
					node.children = getChildrenArray();
					computeDivider(node.space, node.divider);
				}
				// Traverse down to the next child
				int index = node.getChildIndex(point);
				node = checkAddChild(node, index);
			}
		}
	}

	/**
	 * Finds and creates all leaf nodes which intersect the provided region.  If desired,
	 * external storage can be provided to avoid declaring new memory.
	 *
	 * @param region The region which is being tested for intersection
	 * @param output (Optional) Storage for list containing leafs.  Is cleared.
	 * @param workspace (Optional) Storage for internal book keeping. Is cleared.
	 * @return List of all the leaves inside the region.
	 */
	public List<Octree_I32> addLeafsIntersect( Box3D_I32 region , List<Octree_I32> output ,
											   List<Octree_I32> workspace) {
		if( output == null )
			output = new ArrayList<Octree_I32>();
		if( workspace == null )
			workspace = new ArrayList<Octree_I32>();

		List<Octree_I32> open = workspace;
		open.clear();
		output.clear();

		open.add(tree);

		while( !open.isEmpty() ) {
			Octree_I32 node = open.remove(open.size()-1);

			if( node.isLeaf() ) {
				if( node.isSmallest() ) {
					output.add(node);
					continue;
				} else {
					node.children = getChildrenArray();
					computeDivider(node.space, node.divider);
				}
			}

			// add all children which are contained inside the region
			for (int i = 0; i < 8; i++) {
				Octree_I32 child = checkAddChild(node, i);
				if( child != null && Intersection3D_I32.intersect(region,child.space)) {
					open.add(child);
				}
			}
		}

		return output;
	}

	/**
	 * Searches for all leafs which are of the smallest size within the region.  The
	 * tree will not be modified by this operation and no new leafs are added.
	 *
	 * @param region The region which is being tested for intersection
	 * @param output (Optional) Storage for list containing leafs.  Is cleared.
	 * @param workspace (Optional) Storage for internal book keeping. Is cleared.
	 * @return List of all the leaves inside the region.
	 */
	public List<Octree_I32> findLeafsIntersect( Box3D_I32 region , List<Octree_I32> output ,
												List<Octree_I32> workspace) {
		if( output == null )
			output = new ArrayList<Octree_I32>();
		if( workspace == null )
			workspace = new ArrayList<Octree_I32>();

		List<Octree_I32> open = workspace;
		open.clear();
		output.clear();

		open.add(tree);

		while( !open.isEmpty() ) {
			Octree_I32 node = open.remove(open.size()-1);

			if( node.isLeaf() ) {
				if( node.isSmallest() ) {
					output.add(node);
				}
				continue;
			}

			// add all children which are contained inside the region
			for (int i = 0; i < 8; i++) {
				Octree_I32 child = node.children[i];
				if( child != null && Intersection3D_I32.intersect(region,child.space)) {
					open.add(child);
				}
			}
		}

		return output;
	}
}


<|start_filename|>websitedocs/website/node_modules/docusaurus/lib/copy-examples.js<|end_filename|>
#!/usr/bin/env node

/**
 * Copyright (c) 2017-present, Facebook, Inc.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

const chalk = require('chalk');
const commander = require('commander');
const fs = require('fs-extra');
const glob = require('glob');
const path = require('path');

const CWD = process.cwd();

let feature;

commander
  .arguments('[feature]')
  .action(feat => {
    feature = feat;
  })
  .parse(process.argv);

// add scripts to package.json file
if (fs.existsSync(CWD + '/package.json')) {
  const packageContent = JSON.parse(
    fs.readFileSync(CWD + '/package.json', 'utf8')
  );
  if (!packageContent.scripts) {
    packageContent.scripts = {};
  }
  packageContent.scripts['start'] = 'docusaurus-start';
  packageContent.scripts['build'] = 'docusaurus-build';
  packageContent.scripts['publish-gh-pages'] = 'docusaurus-publish';
  packageContent.scripts['examples'] = 'docusaurus-examples';
  packageContent.scripts['write-translations'] =
    'docusaurus-write-translations';
  packageContent.scripts['version'] = 'docusaurus-version';
  packageContent.scripts['rename-version'] = 'docusaurus-rename-version';
  fs.writeFileSync(
    CWD + '/package.json',
    JSON.stringify(packageContent, null, 2) + '\n'
  );
  console.log(
    `${chalk.green('Wrote docusaurus scripts to package.json file.')}\n`
  );
}

const outerFolder = path.basename(path.dirname(CWD));

let docsCreated = false;
let blogCreated = false;
let exampleSiteCreated = false;

// handles cases where feature is "translations", "versions" or neither/not present
if (feature === 'translations') {
  // copy files for translations
  const folder = path.join(__dirname, '..', 'examples', 'translations');
  if (fs.existsSync(CWD + '/../crowdin.yaml')) {
    console.log(
      `${chalk.yellow('crowdin.yaml already exists')} in ${chalk.yellow(
        outerFolder + '/'
      )}. Rename or remove the file to regenerate an example version.\n`
    );
  } else {
    fs.copySync(folder + '/crowdin.yaml', CWD + '/../crowdin.yaml');
    exampleSiteCreated = true;
  }
  let files = glob.sync(folder + '/**/*');
  files.forEach(file => {
    if (fs.lstatSync(file).isDirectory()) {
      return;
    }
    if (path.basename(file) === 'crowdin.yaml') {
      return;
    }
    const filePath = path.resolve(file).split(path.resolve(folder))[1];
    try {
      fs.copySync(file, CWD + filePath, {
        overwrite: false,
        errorOnExist: true,
      });
      exampleSiteCreated = true;
    } catch (e) {
      console.log(
        `${chalk.yellow(
          path.basename(filePath) + ' already exists'
        )} in ${chalk.yellow(
          'website' + filePath.split(path.basename(filePath))[0]
        )}. Rename or remove the file to regenerate an example version.\n`
      );
    }
  });
} else if (feature === 'versions') {
  // copy files for versions
  const folder = path.join(__dirname, '..', 'examples', 'versions');
  let files = glob.sync(folder + '/**/*');
  files.forEach(file => {
    if (fs.lstatSync(file).isDirectory()) {
      return;
    }
    const filePath = path.resolve(file).split(path.resolve(folder))[1];
    try {
      fs.copySync(file, CWD + filePath, {
        overwrite: false,
        errorOnExist: true,
      });
      exampleSiteCreated = true;
    } catch (e) {
      console.log(
        `${chalk.yellow(
          path.basename(filePath) + ' already exists'
        )} in ${chalk.yellow(
          'website' + filePath.split(path.basename(filePath))[0]
        )}. Rename or remove the file to regenerate an example version.\n`
      );
    }
  });
} else {
  const folder = path.join(__dirname, '..', 'examples', 'basics');
  // copy docs examples
  if (fs.existsSync(CWD + '/../docs-examples-from-docusaurus')) {
    console.log(
      `${chalk.yellow(
        'Example docs already exist!'
      )} Rename or remove ${chalk.yellow(
        outerFolder + '/docs-examples-from-docusaurus'
      )} to regenerate example docs.\n`
    );
  } else {
    fs.copySync(
      folder + '/docs-examples-from-docusaurus',
      CWD + '/../docs-examples-from-docusaurus'
    );
    exampleSiteCreated = true;
    docsCreated = true;
  }
  // copy blog examples
  if (fs.existsSync(CWD + '/blog-examples-from-docusaurus')) {
    console.log(
      `${chalk.yellow(
        'Example blog posts already exist!'
      )} Rename or remove ${chalk.yellow(
        outerFolder + '/website/blog-examples-from-docusaurus'
      )} to regenerate example blog posts.\n`
    );
  } else {
    fs.copySync(
      path.join(folder, 'blog-examples-from-docusaurus'),
      path.join(CWD, 'blog-examples-from-docusaurus')
    );
    exampleSiteCreated = true;
    blogCreated = true;
  }
  // copy .gitignore file
  let gitignoreName = '.gitignore';
  if (fs.existsSync(CWD + '/../.gitignore')) {
    gitignoreName = '.gitignore-example-from-docusaurus';
    console.log(
      `${chalk.yellow('.gitignore already exists')} in ${chalk.yellow(
        CWD
      )}. Creating an example gitignore file for you to copy from if desired.\n`
    );
  }
  fs.copySync(
    path.join(folder, 'gitignore'),
    path.join(CWD, '/../' + gitignoreName)
  );

  // copy other files
  let files = glob.sync(folder + '/**/*');
  files.forEach(file => {
    if (fs.lstatSync(file).isDirectory()) {
      return;
    }
    const containingFolder = path.basename(path.dirname(file));
    if (
      path.basename(file) === 'gitignore' ||
      containingFolder === 'blog-examples-from-docusaurus' ||
      containingFolder === 'docs-examples-from-docusaurus'
    ) {
      return;
    }
    const filePath = path.resolve(file).split(path.resolve(folder))[1];
    try {
      fs.copySync(file, CWD + filePath, {
        overwrite: false,
        errorOnExist: true,
      });
      exampleSiteCreated = true;
    } catch (e) {
      console.log(
        `${chalk.yellow(
          path.basename(filePath) + ' already exists'
        )} in ${chalk.yellow(
          'website' + filePath.split(path.basename(filePath))[0]
        )}. Rename or remove the file to regenerate an example version.\n`
      );
    }
  });

  if (exampleSiteCreated) {
    try {
      const tree = require('tree-node-cli');
      const dirString = tree(path.join(CWD, '..'), {
        exclude: [
          /node_modules/, // npm
          /vendor/, // composer
        ],
      });
      console.log(dirString);
    } catch (error) {
      console.warn(`Error printing directory: ${error}`);
    }
  }
}

if (docsCreated) {
  console.log(
    `Rename ${chalk.yellow(
      outerFolder + '/docs-examples-from-docusaurus'
    )} to ${chalk.yellow(
      outerFolder + '/docs'
    )} to see the example docs on your site.\n`
  );
}

if (blogCreated) {
  console.log(
    `Rename ${chalk.yellow(
      outerFolder + '/website/blog-examples-from-docusaurus'
    )} to ${chalk.yellow(
      outerFolder + '/website/blog'
    )} to see the example blog posts on your site.\n`
  );
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/genericQuadruped/parameters/GenericQuadrupedSitDownParameters.java<|end_filename|>
package us.ihmc.exampleSimulations.genericQuadruped.parameters;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WholeBodySetpointParameters;
import us.ihmc.quadrupedRobotics.parameters.QuadrupedSitDownParameters;

public class GenericQuadrupedSitDownParameters implements QuadrupedSitDownParameters
{
   private final GenericQuadrupedSitDownSetpoints sitDownSetpoints = new GenericQuadrupedSitDownSetpoints();

   @Override
   public WholeBodySetpointParameters getSitDownParameters()
   {
      return sitDownSetpoints;
   }

   @Override
   public double getTimeToMoveForSittingDown()
   {
      return 1.5;
   }
}


<|start_filename|>ihmc-interfaces/src/rosdocker/linux/Dockerfile<|end_filename|>
FROM osrf/ros2:ardent-ros1-bridge

RUN apt-get update && \
    apt-get install -y curl git python-catkin-pkg python3-vcstool ros-${ROS2_DISTRO}-ament-tools && \
    rm -rf /var/lib/apt/lists/*

RUN apt-get update && apt-get install -y ros-${ROS2_DISTRO}-ros1-bridge build-essential cppcheck cmake \
                                         libopencv-dev python-empy python3-catkin-pkg-modules \
                                         python3-dev python3-empy python3-nose \
                                         python3-pip python3-pyparsing python3-setuptools python3-vcstool \
                                         python3-pytest python3-yaml libtinyxml-dev libeigen3-dev \
                                         libasio-dev libtinyxml2-dev git curl

RUN mkdir -p /root/ihmc_ros1_bridge/src

WORKDIR /root/ihmc_ros1_bridge

# Add controller_msgs
COPY src/main/messages/ihmc_interfaces/controller_msgs src/controller_msgs

RUN /bin/bash -c "source /opt/ros/$ROS2_DISTRO/setup.bash \
    && ament build --only controller_msgs"

# Add ihmc demos (build separately to save time)
COPY src/ihmc_demo_py src/ihmc_demo_py

RUN /bin/bash -c "source /opt/ros/$ROS2_DISTRO/setup.bash \
    && ament build --only ihmc_demo_py"

RUN mkdir -p /root/ihmc_catkin_ws/src

WORKDIR /root/ihmc_catkin_ws/src

RUN /bin/bash -c "source /opt/ros/kinetic/setup.bash \
    && catkin_init_workspace"

WORKDIR /root/ihmc_catkin_ws

COPY src/main/messages/ros1/controller_msgs src/controller_msgs

RUN /bin/bash -c "source /opt/ros/kinetic/setup.bash \
    && catkin_make && catkin_make install"

WORKDIR /root/ihmc_ros1_bridge
# Add ros1_bridge
COPY src/rosdocker/linux/ihmc_ros1_bridge.repos .
RUN vcs import src < ihmc_ros1_bridge.repos

#Uncomment this if bridge fails to compile due to out-of-memory issues
#ENV MAKEFLAGS=-j1

RUN /bin/bash -c "source /opt/ros/$ROS2_DISTRO/setup.bash \
               && source /root/ihmc_ros1_bridge/install/local_setup.bash \
               && source /opt/ros/kinetic/setup.bash \
               && source /root/ihmc_catkin_ws/install/setup.bash \
               && ament build --only ros1_bridge --force-cmake-configure"

#COPY ./runtime_entrypoint.sh /

COPY ./source_ros1.sh /
COPY ./source_ros2.sh /
COPY ./source_ros1_then_ros2.sh /

RUN ["chmod", "+x", "/source_ros1.sh"]
RUN ["chmod", "+x", "/source_ros2.sh"]
RUN ["chmod", "+x", "/source_ros1_then_ros2.sh"]

#RUN /bin/bash -c "echo 'source /runtime_entrypoint.sh' >> /root/.bashrc"

CMD ["/bin/bash"]


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/ControllerFailureListener.java<|end_filename|>
package us.ihmc.robotics.controllers;

import us.ihmc.euclid.referenceFrame.FrameVector2D;

public interface ControllerFailureListener
{
   public void controllerFailed(FrameVector2D fallingDirection);
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/RequestLidarScanMessage.java<|end_filename|>
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;

/**
       * Message used to request a new LIDAR scan, and also enable some filters.
       */
public class RequestLidarScanMessage extends Packet<RequestLidarScanMessage> implements Settable<RequestLidarScanMessage>, EpsilonComparable<RequestLidarScanMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean remove_shadows_ = true;
   public boolean remove_self_collisions_ = true;

   public RequestLidarScanMessage()
   {
   }

   public RequestLidarScanMessage(RequestLidarScanMessage other)
   {
      this();
      set(other);
   }

   public void set(RequestLidarScanMessage other)
   {
      sequence_id_ = other.sequence_id_;

      remove_shadows_ = other.remove_shadows_;

      remove_self_collisions_ = other.remove_self_collisions_;

   }

   /**
            * 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 void setRemoveShadows(boolean remove_shadows)
   {
      remove_shadows_ = remove_shadows;
   }
   public boolean getRemoveShadows()
   {
      return remove_shadows_;
   }

   public void setRemoveSelfCollisions(boolean remove_self_collisions)
   {
      remove_self_collisions_ = remove_self_collisions;
   }
   public boolean getRemoveSelfCollisions()
   {
      return remove_self_collisions_;
   }


   public static Supplier<RequestLidarScanMessagePubSubType> getPubSubType()
   {
      return RequestLidarScanMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return RequestLidarScanMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(RequestLidarScanMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.remove_shadows_, other.remove_shadows_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.remove_self_collisions_, other.remove_self_collisions_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof RequestLidarScanMessage)) return false;

      RequestLidarScanMessage otherMyClass = (RequestLidarScanMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.remove_shadows_ != otherMyClass.remove_shadows_) return false;

      if(this.remove_self_collisions_ != otherMyClass.remove_self_collisions_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("RequestLidarScanMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("remove_shadows=");
      builder.append(this.remove_shadows_);      builder.append(", ");
      builder.append("remove_self_collisions=");
      builder.append(this.remove_self_collisions_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-java-toolkit/src/test/java/us/ihmc/tools/inputDevices/joystick/JoystickDemo.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick;

import net.java.games.input.Event;
import us.ihmc.commons.PrintTools;
import us.ihmc.tools.inputDevices.joystick.exceptions.JoystickNotFoundException;

public class JoystickDemo
{
   public JoystickDemo()
   {
      try
      {
         Joystick joystick = new Joystick(JoystickModel.XBOX_ONE, 0);
         joystick.setStandalone();
         joystick.printListOfConnectedJoysticks();
         joystick.addJoystickStatusListener(new JoystickStatusListener()
         {
            @Override
            public void updateConnectivity(boolean connected)
            {
               PrintTools.info("Connected: " + connected);
            }
         });
         joystick.addJoystickEventListener(new JoystickEventListener()
         {
            @Override
            public void processEvent(Event event)
            {
               PrintTools.info("Event: " + event.toString());
            }
         });
         joystick.setPollInterval(10);
      }
      catch (JoystickNotFoundException e)
      {
         e.printStackTrace();
      }
   }
   
   public static void main(String[] args)
   {
      new JoystickDemo();
   }
}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/controllers/StartGoalAnchorPaneController.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui.controllers;

import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.GoalEditModeEnabled;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.ShowGoalPosition;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.ShowStartPosition;
import static us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics.StartEditModeEnabled;

import javafx.fxml.FXML;
import javafx.scene.control.Spinner;
import javafx.scene.control.SpinnerValueFactory.DoubleSpinnerValueFactory;
import javafx.scene.control.ToggleButton;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.javaFXToolkit.messager.JavaFXMessager;
import us.ihmc.pathPlanning.visibilityGraphs.ui.messager.UIVisibilityGraphsTopics;
import us.ihmc.pathPlanning.visibilityGraphs.ui.properties.Point3DProperty;

public class StartGoalAnchorPaneController
{
   @FXML
   private ToggleButton showStartToggleButton;
   @FXML
   private ToggleButton showGoalToggleButton;
   @FXML
   private ToggleButton placeStartToggleButton;
   @FXML
   private ToggleButton placeGoalToggleButton;
   @FXML
   private Spinner<Double> startXSpinner;
   @FXML
   private Spinner<Double> startYSpinner;
   @FXML
   private Spinner<Double> startZSpinner;
   @FXML
   private Spinner<Double> goalXSpinner;
   @FXML
   private Spinner<Double> goalYSpinner;
   @FXML
   private Spinner<Double> goalZSpinner;

   private JavaFXMessager messager;
   private final Point3DProperty startPositionProperty = new Point3DProperty(this, "startPositionProperty", new Point3D());
   private final Point3DProperty goalPositionProperty = new Point3DProperty(this, "goalPositionProperty", new Point3D());

   public void attachMessager(JavaFXMessager messager)
   {
      this.messager = messager;
   }

   private void setupControls()
   {
      startXSpinner.setValueFactory(createStartGoalValueFactory());
      startYSpinner.setValueFactory(createStartGoalValueFactory());
      startZSpinner.setValueFactory(createStartGoalValueFactory());

      goalXSpinner.setValueFactory(createStartGoalValueFactory());
      goalYSpinner.setValueFactory(createStartGoalValueFactory());
      goalZSpinner.setValueFactory(createStartGoalValueFactory());
   }

   public void bindControls()
   {
      setupControls();

      messager.bindBidirectional(ShowStartPosition, showStartToggleButton.selectedProperty(), false);
      messager.bindBidirectional(ShowGoalPosition, showGoalToggleButton.selectedProperty(), false);
      
      messager.bindBidirectional(StartEditModeEnabled, placeStartToggleButton.selectedProperty(), false);
      messager.bindBidirectional(StartEditModeEnabled, placeGoalToggleButton.disableProperty(), false);

      messager.bindBidirectional(GoalEditModeEnabled, placeGoalToggleButton.selectedProperty(), false);
      messager.bindBidirectional(GoalEditModeEnabled, placeStartToggleButton.disableProperty(), false);


      startPositionProperty.bindBidirectionalX(startXSpinner.getValueFactory().valueProperty());
      startPositionProperty.bindBidirectionalY(startYSpinner.getValueFactory().valueProperty());
      startPositionProperty.bindBidirectionalZ(startZSpinner.getValueFactory().valueProperty());
      messager.bindBidirectional(UIVisibilityGraphsTopics.StartPosition, startPositionProperty, false);

      goalPositionProperty.bindBidirectionalX(goalXSpinner.getValueFactory().valueProperty());
      goalPositionProperty.bindBidirectionalY(goalYSpinner.getValueFactory().valueProperty());
      goalPositionProperty.bindBidirectionalZ(goalZSpinner.getValueFactory().valueProperty());
      messager.bindBidirectional(UIVisibilityGraphsTopics.GoalPosition, goalPositionProperty, false);
   }

   private DoubleSpinnerValueFactory createStartGoalValueFactory()
   {
      double min = -100.0;
      double max = 100.0;
      double amountToStepBy = 0.1;
      return new DoubleSpinnerValueFactory(min, max, 0.0, amountToStepBy);
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/main/ChangeCollector.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.main;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Queue;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ConcurrentLinkedQueue;

import us.ihmc.parameterTuner.guiElements.GuiParameter;
import us.ihmc.parameterTuner.guiElements.ParameterChangeListener;

public class ChangeCollector implements ParameterChangeListener
{
   private final Queue<GuiParameter> changedParameters = new ConcurrentLinkedQueue<>();
   private final Map<String, String> pendingChanges = new ConcurrentHashMap<>();

   private boolean recordingChange = true;

   @Override
   public void changed(GuiParameter parameter)
   {
      // this is called on parameter changes (they happen in the Plattform.runLater(Runnable) thread)
      if (recordingChange)
      {
         changedParameters.add(new GuiParameter(parameter));
      }
   }

   public List<GuiParameter> getChangedParametersAndClear()
   {
      // this is called by the animation timer
      GuiParameter userChangedParameter;
      Map<String, GuiParameter> parametersToUpdate = new HashMap<>();
      while ((userChangedParameter = changedParameters.poll()) != null)
      {
         parametersToUpdate.put(userChangedParameter.getUniqueName(), userChangedParameter);
      }

      for (GuiParameter changedParameter : parametersToUpdate.values())
      {
         String uniqueName = changedParameter.getUniqueName();
         pendingChanges.put(uniqueName, changedParameter.getCurrentValue());
      }

      return new ArrayList<>(parametersToUpdate.values());
   }

   public void stopRecording()
   {
      recordingChange = false;
   }

   public void startRecording()
   {
      recordingChange = true;
   }

   public boolean isPending(String uniqueName)
   {
      return pendingChanges.containsKey(uniqueName);
   }

   public void parameterWasUpdated(String uniqueName, String newValue)
   {
      String expectedValue = pendingChanges.get(uniqueName);
      if (newValue.equals(expectedValue))
      {
         pendingChanges.remove(uniqueName);
      }
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepSnapper/BasicFootstepMask.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepSnapper;

import java.util.List;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.geometry.InclusionFunction;

/**
 * Created by agrabertilton on 1/15/15.
 */
public class BasicFootstepMask implements InclusionFunction<Point3D>
{
   private Point2D position = new Point2D();
   private double yaw;
   private double safetyBuffer;
   private boolean initialized = false;
   private ConvexPolygon2D footShapeWithBufferPolygon = new ConvexPolygon2D();

   public BasicFootstepMask(ContactablePlaneBody foot, double maskBufferSize)
   {
      this.safetyBuffer = maskBufferSize;

      footShapeWithBufferPolygon.clear();

      for (FramePoint2D vertex : foot.getContactPoints2d())
      {
         footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY()));
      }
      footShapeWithBufferPolygon.update();
   }

   public BasicFootstepMask(List<Point2D> footstepShape, double maskBufferSize)
   {
      this.safetyBuffer = maskBufferSize;

      footShapeWithBufferPolygon.clear();

      for (Point2D vertex : footstepShape)
      {
         footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY()));
      }
      footShapeWithBufferPolygon.update();
   }

   public BasicFootstepMask(ConvexPolygon2D footstepShape, double maskBufferSize)
   {
      int numVertices = footstepShape.getNumberOfVertices();
      Point2DReadOnly vertex;
      for (int i = 0; i < numVertices; i++)
      {
         vertex = footstepShape.getVertex(i);
         footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY()));
      }
      footShapeWithBufferPolygon.update();
   }

   private double inflate(double dimension)
   {
      return dimension + Math.signum(dimension) * safetyBuffer;
   }

   public void setPositionAndYaw(Point2D position, double yaw)
   {
      this.position.set(position.getX(), position.getY());
      this.yaw = yaw;
      initialized = true;
   }

   public void setPositionAndYaw(double x, double y, double yaw)
   {
      position.set(x,y);
      this.yaw = yaw;
      initialized = true;
   }

   public void setSafetyBuffer(double safetyBuffer)
   {
      this.safetyBuffer = safetyBuffer;
   }

   @Override
   public boolean isIncluded(Point3D inputPoint)
   {
      if (!initialized)
         return false;
      double x = inputPoint.getX() - position.getX();
      double y = inputPoint.getY() - position.getY();
      double cos = Math.cos(yaw);
      double sin = Math.sin(yaw);
      double xInFoot = x * cos + y * sin;
      double yInFoot = y * cos - x * sin;
      boolean isInside = footShapeWithBufferPolygon.isPointInside(xInFoot, yInFoot);

      return isInside;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ChestTrajectoryMessage.java<|end_filename|>
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 commands the controller to move in taskspace the chest to the desired orientation while going through the specified trajectory points.
       * A Hermite based curve (third order) is used to interpolate the orientations.
       * To execute a simple trajectory to reach a desired chest orientation, set only one trajectory point with zero velocity and its time to be equal to the desired trajectory time.
       */
public class ChestTrajectoryMessage extends Packet<ChestTrajectoryMessage> implements Settable<ChestTrajectoryMessage>, EpsilonComparable<ChestTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The orientation trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage so3_trajectory_;

   public ChestTrajectoryMessage()
   {
      so3_trajectory_ = new controller_msgs.msg.dds.SO3TrajectoryMessage();
   }

   public ChestTrajectoryMessage(ChestTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(ChestTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.SO3TrajectoryMessagePubSubType.staticCopy(other.so3_trajectory_, so3_trajectory_);
   }

   /**
            * 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_;
   }


   /**
            * The orientation trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage getSo3Trajectory()
   {
      return so3_trajectory_;
   }


   public static Supplier<ChestTrajectoryMessagePubSubType> getPubSubType()
   {
      return ChestTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ChestTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ChestTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.so3_trajectory_.epsilonEquals(other.so3_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ChestTrajectoryMessage)) return false;

      ChestTrajectoryMessage otherMyClass = (ChestTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.so3_trajectory_.equals(otherMyClass.so3_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ChestTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("so3_trajectory=");
      builder.append(this.so3_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WaypointBasedTrajectoryMessage.java<|end_filename|>
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;

/**
       * Configure a constrained trajectory for a given end-effector.
       * Main usage is the IHMC WholeBodyTrajectoryToolbox.
       */
public class WaypointBasedTrajectoryMessage extends Packet<WaypointBasedTrajectoryMessage> implements Settable<WaypointBasedTrajectoryMessage>, EpsilonComparable<WaypointBasedTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int end_effector_hash_code_;
   public us.ihmc.idl.IDLSequence.Double  waypoint_times_;
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.geometry.Pose3D>  waypoints_;
   public controller_msgs.msg.dds.SelectionMatrix3DMessage angular_selection_matrix_;
   public controller_msgs.msg.dds.SelectionMatrix3DMessage linear_selection_matrix_;
   public us.ihmc.euclid.tuple3D.Point3D control_frame_position_in_end_effector_;
   public us.ihmc.euclid.tuple4D.Quaternion control_frame_orientation_in_end_effector_;
   public double weight_ = -1.0;

   public WaypointBasedTrajectoryMessage()
   {
      waypoint_times_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      waypoints_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.geometry.Pose3D> (100, new geometry_msgs.msg.dds.PosePubSubType());
      angular_selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      linear_selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      control_frame_position_in_end_effector_ = new us.ihmc.euclid.tuple3D.Point3D();
      control_frame_orientation_in_end_effector_ = new us.ihmc.euclid.tuple4D.Quaternion();

   }

   public WaypointBasedTrajectoryMessage(WaypointBasedTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(WaypointBasedTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      end_effector_hash_code_ = other.end_effector_hash_code_;

      waypoint_times_.set(other.waypoint_times_);
      waypoints_.set(other.waypoints_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.angular_selection_matrix_, angular_selection_matrix_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.linear_selection_matrix_, linear_selection_matrix_);
      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.control_frame_position_in_end_effector_, control_frame_position_in_end_effector_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.control_frame_orientation_in_end_effector_, control_frame_orientation_in_end_effector_);
      weight_ = other.weight_;

   }

   /**
            * 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 void setEndEffectorHashCode(int end_effector_hash_code)
   {
      end_effector_hash_code_ = end_effector_hash_code;
   }
   public int getEndEffectorHashCode()
   {
      return end_effector_hash_code_;
   }


   public us.ihmc.idl.IDLSequence.Double  getWaypointTimes()
   {
      return waypoint_times_;
   }


   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.geometry.Pose3D>  getWaypoints()
   {
      return waypoints_;
   }


   public controller_msgs.msg.dds.SelectionMatrix3DMessage getAngularSelectionMatrix()
   {
      return angular_selection_matrix_;
   }


   public controller_msgs.msg.dds.SelectionMatrix3DMessage getLinearSelectionMatrix()
   {
      return linear_selection_matrix_;
   }


   public us.ihmc.euclid.tuple3D.Point3D getControlFramePositionInEndEffector()
   {
      return control_frame_position_in_end_effector_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getControlFrameOrientationInEndEffector()
   {
      return control_frame_orientation_in_end_effector_;
   }

   public void setWeight(double weight)
   {
      weight_ = weight;
   }
   public double getWeight()
   {
      return weight_;
   }


   public static Supplier<WaypointBasedTrajectoryMessagePubSubType> getPubSubType()
   {
      return WaypointBasedTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WaypointBasedTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WaypointBasedTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.end_effector_hash_code_, other.end_effector_hash_code_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.waypoint_times_, other.waypoint_times_, epsilon)) return false;

      if (this.waypoints_.size() != other.waypoints_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.waypoints_.size(); i++)
         {  if (!this.waypoints_.get(i).epsilonEquals(other.waypoints_.get(i), epsilon)) return false; }
      }

      if (!this.angular_selection_matrix_.epsilonEquals(other.angular_selection_matrix_, epsilon)) return false;
      if (!this.linear_selection_matrix_.epsilonEquals(other.linear_selection_matrix_, epsilon)) return false;
      if (!this.control_frame_position_in_end_effector_.epsilonEquals(other.control_frame_position_in_end_effector_, epsilon)) return false;
      if (!this.control_frame_orientation_in_end_effector_.epsilonEquals(other.control_frame_orientation_in_end_effector_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.weight_, other.weight_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WaypointBasedTrajectoryMessage)) return false;

      WaypointBasedTrajectoryMessage otherMyClass = (WaypointBasedTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.end_effector_hash_code_ != otherMyClass.end_effector_hash_code_) return false;

      if (!this.waypoint_times_.equals(otherMyClass.waypoint_times_)) return false;
      if (!this.waypoints_.equals(otherMyClass.waypoints_)) return false;
      if (!this.angular_selection_matrix_.equals(otherMyClass.angular_selection_matrix_)) return false;
      if (!this.linear_selection_matrix_.equals(otherMyClass.linear_selection_matrix_)) return false;
      if (!this.control_frame_position_in_end_effector_.equals(otherMyClass.control_frame_position_in_end_effector_)) return false;
      if (!this.control_frame_orientation_in_end_effector_.equals(otherMyClass.control_frame_orientation_in_end_effector_)) return false;
      if(this.weight_ != otherMyClass.weight_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WaypointBasedTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("end_effector_hash_code=");
      builder.append(this.end_effector_hash_code_);      builder.append(", ");
      builder.append("waypoint_times=");
      builder.append(this.waypoint_times_);      builder.append(", ");
      builder.append("waypoints=");
      builder.append(this.waypoints_);      builder.append(", ");
      builder.append("angular_selection_matrix=");
      builder.append(this.angular_selection_matrix_);      builder.append(", ");
      builder.append("linear_selection_matrix=");
      builder.append(this.linear_selection_matrix_);      builder.append(", ");
      builder.append("control_frame_position_in_end_effector=");
      builder.append(this.control_frame_position_in_end_effector_);      builder.append(", ");
      builder.append("control_frame_orientation_in_end_effector=");
      builder.append(this.control_frame_orientation_in_end_effector_);      builder.append(", ");
      builder.append("weight=");
      builder.append(this.weight_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepSnapper/GenericFootstepSnappingParameters.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepSnapper;

import us.ihmc.euclid.geometry.ConvexPolygon2D;

/**
 * Created by agrabertilton on 1/20/15.
 */
public class GenericFootstepSnappingParameters extends QuadTreeFootstepSnappingParameters
{
   public GenericFootstepSnappingParameters(){
      super(null, null, 0.3, Math.PI/4, 0.0, 0.01, 0.025, Math.PI/24, 1);
      collisionPolygon = new ConvexPolygon2D();
      collisionPolygon.addVertex(0.1, 0.05);
      collisionPolygon.addVertex(0.1, -0.05);
      collisionPolygon.addVertex(-0.05,-0.05);
      collisionPolygon.addVertex(-0.05, 0.05);
      collisionPolygon.update();

      supportPolygon = collisionPolygon;
   }

   @Override
   public double getMinArea()
   {
      return supportPolygon.getArea() * 0.01;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/footstepPlanning/ConcurrentList.java<|end_filename|>
package us.ihmc.avatar.footstepPlanning;

import us.ihmc.concurrent.ConcurrentCopier;

import java.util.ArrayList;
import java.util.List;

class ConcurrentList<T> extends ConcurrentCopier<List<T>>
{
   public ConcurrentList()
   {
      super(ArrayList::new);
   }

   public boolean isEmpty()
   {
      List<T> readCopy = this.getCopyForReading();
      if (readCopy == null)
         return true;
      else
         return readCopy.isEmpty();
   }

   public int size()
   {
      List<T> currentList = getCopyForReading();
      if (currentList == null)
         return 0;
      else
         return currentList.size();
   }

   public void clear()
   {
      List<T> updatedList = getCopyForWriting();
      updatedList.clear();
      commit();
   }

   public void add(T objectToAdd)
   {
      List<T> existingList = getCopyForReading();
      List<T> updatedList = getCopyForWriting();
      updatedList.clear();
      if (existingList != null)
         updatedList.addAll(existingList);
      updatedList.add(objectToAdd);

      this.commit();
   }

   public T remove(int indexToRemove)
   {
      List<T> existingList = getCopyForReading();
      List<T> updatedList = getCopyForWriting();
      updatedList.clear();
      if (existingList != null)
         updatedList.addAll(existingList);
      T objectToReturn = updatedList.remove(indexToRemove);

      this.commit();

      return objectToReturn;
   }

   public Iterable<T> iterable()
   {
      return getCopyForReading();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/simpleArm/SimpleArmSimulation.java<|end_filename|>
package us.ihmc.exampleSimulations.simpleArm;

import us.ihmc.yoVariables.variable.YoDouble;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class SimpleArmSimulation
{
   private static final double dt = 0.001;
   private final SimulationConstructionSet scs;

   public SimpleArmSimulation()
   {
      SimpleArmRobot simpleArmRobot = new SimpleArmRobot(0.0);
      SimpleRobotInputOutputMap robotInputOutputMap = new SimpleRobotInputOutputMap(simpleArmRobot);
      YoDouble time = simpleArmRobot.getYoTime();
      RigidBodyBasics endEffectorBody = simpleArmRobot.getEndEffectorBody();
      SimpleArmController controller = new SimpleArmController(robotInputOutputMap, endEffectorBody, time);
      simpleArmRobot.setController(controller);

      SimpleArmEstimator estimator = new SimpleArmEstimator(robotInputOutputMap, dt);
      simpleArmRobot.setController(estimator);

      scs = new SimulationConstructionSet(simpleArmRobot);
      scs.setDT(dt, 1);
      scs.startOnAThread();
      scs.simulate(4.0);
   }

   public static void main(String[] args)
   {
      new SimpleArmSimulation();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/FrameVertex3DSupplierList.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex3DSupplier;

public class FrameVertex3DSupplierList<T extends FramePoint3DReadOnly> extends Vertex3DSupplierList<T> implements FrameVertex3DSupplier
{
   @Override
   public T getVertex(int index)
   {
      return super.getVertex(index);
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/simple3DWalker/SimpleWalkerICPPlanner.java<|end_filename|>
package us.ihmc.exampleSimulations.simple3DWalker;

import java.util.ArrayList;

public class SimpleWalkerICPPlanner
{
   ArrayList<Double> CoPplan = new ArrayList();
   ArrayList<Double> ICPplan = new ArrayList<>();
   ArrayList<Double> ICPPlanOrdered = new ArrayList<>();
   double omega;
   public SimpleWalkerICPPlanner(ArrayList<Double> footstepsXvalues, double stepTime, double omega)
   {
      this.omega=omega;
      CoPplan = footstepsXvalues;

      double ICPf = CoPplan.get(CoPplan.size()-1);
      ICPplan.add(ICPf);
      for(int i=0; i<(CoPplan.size()-1);i++)
      {
         double CoPx = CoPplan.get(CoPplan.size()-2-i);
         double ICPprev = CoPx + Math.exp(-omega*stepTime)*(ICPf - CoPx);
         ICPplan.add(ICPprev);
         ICPf = ICPprev;
      }
   }

   public ArrayList<Double> getICPKnotPoints()
   {
      for(int i=0;i<ICPplan.size();i++)
      {
         double ICPcurr = ICPplan.get(ICPplan.size()-1-i);
         ICPPlanOrdered.add(ICPcurr);
      }
      return ICPPlanOrdered;
   }

   public double getICPReference(int currentStep, double timeInstate)
   {
      double ICPinit = ICPplan.get(ICPplan.size()-1-currentStep);
      double CoPinit = CoPplan.get(currentStep);
      double ICPref = Math.exp(omega*timeInstate)*(ICPinit-CoPinit)+CoPinit;
      return  ICPref;
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasPhysicalProperties.java<|end_filename|>
package us.ihmc.atlas.parameters;

import us.ihmc.avatar.drcRobot.RobotPhysicalProperties;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.robotics.geometry.TransformTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class AtlasPhysicalProperties implements RobotPhysicalProperties
{
   private final static double ATLAS_HEIGHT = 2.0;
   private final static double ATLAS_WEIGHT = 155.944174;

   private final double modelScale;
   private double massScalePower = 3.0;

   private final double ankleHeight;
   private final double pelvisToFoot;
   private final double footWidthForControl;
   private final double toeWidthForControl;
   private final double footLengthForControl;
   private final double footBackForControl;
   private final double actualFootWidth;
   private final double actualFootLength;
   private final double footStartToetaperFromBack;
   private final double footForwardForControl; // 0.16;   //0.178;    // 0.18;
   private final double shinLength;
   private final double thighLength;

   private final SideDependentList<RigidBodyTransform> soleToAnkleFrameTransforms = new SideDependentList<>();
   private final SideDependentList<RigidBodyTransform> handAttachmentPlateToWristTransforms = new SideDependentList<RigidBodyTransform>();

   public AtlasPhysicalProperties()
   {
      this(1.0);
   }

   public AtlasPhysicalProperties(double height, double weight)
   {
      this(height / ATLAS_HEIGHT);

      double massFactor = Math.log(weight / ATLAS_WEIGHT) / Math.log(modelScale);
      setMassScalePower(massFactor);
   }

   public AtlasPhysicalProperties(double modelScale)
   {
      this.modelScale = modelScale;

      double footWidthScale = modelScale;
      //      if (modelScale < 1.0)
      //      {
      //         footWidthScale = modelScale + 0.5 * (1.0 - modelScale);
      //      }

      ankleHeight = modelScale * 0.084;
      pelvisToFoot = modelScale * 0.887;
      footWidthForControl = footWidthScale * 0.11; //0.12; // 0.08;   //0.124887;
      toeWidthForControl = footWidthScale * 0.085; //0.095; // 0.07;   //0.05;   //
      footLengthForControl = modelScale * 0.22; //0.255;
      footBackForControl = modelScale * 0.085; // 0.09; // 0.06;   //0.082;    // 0.07;
      actualFootWidth = footWidthScale * 0.138;
      actualFootLength = modelScale * 0.26;
      footStartToetaperFromBack = modelScale * 0.195;
      footForwardForControl = footLengthForControl - footBackForControl; // 0.16;   //0.178;    // 0.18;
      shinLength = modelScale * 0.374;
      thighLength = modelScale * 0.422;

      // TODO Auto-generated constructor stub
      for (RobotSide robotSide : RobotSide.values)
      {
         RigidBodyTransform soleToAnkleFrame = TransformTools.createTranslationTransform(footLengthForControl / 2.0 - footBackForControl, 0.0, -ankleHeight);
         soleToAnkleFrameTransforms.put(robotSide, soleToAnkleFrame);

         double y = robotSide.negateIfRightSide(0.1);
         double yaw = robotSide.negateIfRightSide(Math.PI / 2.0);
         RigidBodyTransform handControlFrameToWristTransform = TransformTools.createTransformFromTranslationAndEulerAngles(0.0, y, 0.0, 0.0, 0.0, yaw);
         handAttachmentPlateToWristTransforms.put(robotSide, handControlFrameToWristTransform);
      }
   }

   @Override
   public double getFootWidthForControl()
   {
      return footWidthForControl;
   }

   @Override
   public double getToeWidthForControl()
   {
      return toeWidthForControl;
   }

   @Override
   public double getFootLengthForControl()
   {
      return footLengthForControl;
   }

   @Override
   public double getFootBackForControl()
   {
      return footBackForControl;
   }

   @Override
   public double getActualFootWidth()
   {
      return actualFootWidth;
   }

   @Override
   public double getActualFootLength()
   {
      return actualFootLength;
   }

   @Override
   public double getFootForwardForControl()
   {
      return footForwardForControl;
   }

   @Override
   public double getShinLength()
   {
      return shinLength;
   }

   @Override
   public double getThighLength()
   {
      return thighLength;
   }

   @Override
   public SideDependentList<RigidBodyTransform> getSoleToAnkleFrameTransforms()
   {
      return soleToAnkleFrameTransforms;
   }

   @Override
   public SideDependentList<RigidBodyTransform> getHandAttachmentPlateToWristTransforms()
   {
      return handAttachmentPlateToWristTransforms;
   }

   public double getModelScale()
   {
      return modelScale;
   }

   public double getMassScalePower()
   {
      return massScalePower;
   }

   public void setMassScalePower(double massScalePower)
   {
      this.massScalePower = massScalePower;
   }

}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/FootstepListTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.FootstepDataListMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.FootstepListBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class FootstepListTask extends BehaviorAction
{
   private final FootstepListBehavior footstepListBehavior;
   private final FootstepDataListMessage footStepList;

   public FootstepListTask(FootstepListBehavior footstepListBehavior, FootstepDataListMessage footStepList)
   {
      super(footstepListBehavior);
      this.footstepListBehavior = footstepListBehavior;
      this.footStepList = footStepList;
   }

   @Override
   protected void setBehaviorInput()
   {
      footstepListBehavior.set(footStepList);
   }
}


<|start_filename|>ihmc-ros-tools/generated-src/ihmc_msgs/WalkingStatusRosMessage.java<|end_filename|>
package ihmc_msgs;

public interface WalkingStatusRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_msgs/WalkingStatusRosMessage";
  static final java.lang.String _DEFINITION = "## WalkingStatusRosMessage\n# This class is used to report the status of walking.\n\n# Status of walking. Either STARTED, COMPLETED, or ABORT_REQUESTED.\nint8 walking_status\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  byte getWalkingStatus();
  void setWalkingStatus(byte value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/SleepTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.SleepBehavior;

public class SleepTask extends BehaviorAction
{
   private final SleepBehavior sleepBehavior;
   private final double sleepTime;

   public SleepTask(SleepBehavior sleepBehavior, double sleepTime)
   {
      super(sleepBehavior);
      this.sleepBehavior = sleepBehavior;
      this.sleepTime = sleepTime;
   }

   @Override
   protected void setBehaviorInput()
   {
      sleepBehavior.setSleepTime(sleepTime);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/JointspaceTrajectoryMessage.java<|end_filename|>
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.
       * General purpose message that holds onto information to execute a trajectory in jointspace.
       * A third order polynomial function is used to interpolate between trajectory points.
       * The joint_trajectory_messages can have different waypoint times and different number of waypoints.
       * If a joint trajectory message is empty, the controller will hold the last desired joint position while executing the other joint trajectories.
       */
public class JointspaceTrajectoryMessage extends Packet<JointspaceTrajectoryMessage> implements Settable<JointspaceTrajectoryMessage>, EpsilonComparable<JointspaceTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Trajectory for each joint.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.OneDoFJointTrajectoryMessage>  joint_trajectory_messages_;
   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage queueing_properties_;

   public JointspaceTrajectoryMessage()
   {
      joint_trajectory_messages_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.OneDoFJointTrajectoryMessage> (100, new controller_msgs.msg.dds.OneDoFJointTrajectoryMessagePubSubType());
      queueing_properties_ = new controller_msgs.msg.dds.QueueableMessage();

   }

   public JointspaceTrajectoryMessage(JointspaceTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(JointspaceTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      joint_trajectory_messages_.set(other.joint_trajectory_messages_);
      controller_msgs.msg.dds.QueueableMessagePubSubType.staticCopy(other.queueing_properties_, queueing_properties_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * Trajectory for each joint.
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.OneDoFJointTrajectoryMessage>  getJointTrajectoryMessages()
   {
      return joint_trajectory_messages_;
   }


   /**
            * Properties for queueing trajectories.
            */
   public controller_msgs.msg.dds.QueueableMessage getQueueingProperties()
   {
      return queueing_properties_;
   }


   public static Supplier<JointspaceTrajectoryMessagePubSubType> getPubSubType()
   {
      return JointspaceTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return JointspaceTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(JointspaceTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.joint_trajectory_messages_.size() != other.joint_trajectory_messages_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.joint_trajectory_messages_.size(); i++)
         {  if (!this.joint_trajectory_messages_.get(i).epsilonEquals(other.joint_trajectory_messages_.get(i), epsilon)) return false; }
      }

      if (!this.queueing_properties_.epsilonEquals(other.queueing_properties_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof JointspaceTrajectoryMessage)) return false;

      JointspaceTrajectoryMessage otherMyClass = (JointspaceTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.joint_trajectory_messages_.equals(otherMyClass.joint_trajectory_messages_)) return false;
      if (!this.queueing_properties_.equals(otherMyClass.queueing_properties_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("JointspaceTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("joint_trajectory_messages=");
      builder.append(this.joint_trajectory_messages_);      builder.append(", ");
      builder.append("queueing_properties=");
      builder.append(this.queueing_properties_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ObjectDetectorResultPacket.java<|end_filename|>
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 behavior module
       */
public class ObjectDetectorResultPacket extends Packet<ObjectDetectorResultPacket> implements Settable<ObjectDetectorResultPacket>, EpsilonComparable<ObjectDetectorResultPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public controller_msgs.msg.dds.HeatMapPacket heat_map_;
   public controller_msgs.msg.dds.BoundingBoxesPacket bounding_boxes_;

   public ObjectDetectorResultPacket()
   {
      heat_map_ = new controller_msgs.msg.dds.HeatMapPacket();
      bounding_boxes_ = new controller_msgs.msg.dds.BoundingBoxesPacket();
   }

   public ObjectDetectorResultPacket(ObjectDetectorResultPacket other)
   {
      this();
      set(other);
   }

   public void set(ObjectDetectorResultPacket other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.HeatMapPacketPubSubType.staticCopy(other.heat_map_, heat_map_);
      controller_msgs.msg.dds.BoundingBoxesPacketPubSubType.staticCopy(other.bounding_boxes_, bounding_boxes_);
   }

   /**
            * 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 controller_msgs.msg.dds.HeatMapPacket getHeatMap()
   {
      return heat_map_;
   }


   public controller_msgs.msg.dds.BoundingBoxesPacket getBoundingBoxes()
   {
      return bounding_boxes_;
   }


   public static Supplier<ObjectDetectorResultPacketPubSubType> getPubSubType()
   {
      return ObjectDetectorResultPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ObjectDetectorResultPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ObjectDetectorResultPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.heat_map_.epsilonEquals(other.heat_map_, epsilon)) return false;
      if (!this.bounding_boxes_.epsilonEquals(other.bounding_boxes_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ObjectDetectorResultPacket)) return false;

      ObjectDetectorResultPacket otherMyClass = (ObjectDetectorResultPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.heat_map_.equals(otherMyClass.heat_map_)) return false;
      if (!this.bounding_boxes_.equals(otherMyClass.bounding_boxes_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ObjectDetectorResultPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("heat_map=");
      builder.append(this.heat_map_);      builder.append(", ");
      builder.append("bounding_boxes=");
      builder.append(this.bounding_boxes_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/microphone/DrillDetectionAlgorithm.java<|end_filename|>
package us.ihmc.avatar.sensors.microphone;

import java.io.InputStream;

import javax.sound.sampled.AudioFormat;

import us.ihmc.commons.Conversions;
import us.ihmc.simulationconstructionset.gui.BodePlotConstructor;

public abstract class DrillDetectionAlgorithm
{
   private static final float sampleRate = 16000; //Hz
   private static final int sampleSizeInBits = 16;
   private static final int channels = 1;
   private static final boolean signed = true;
   private static final boolean bigEndian = false;
   private static final AudioFormat format = new AudioFormat(sampleRate, sampleSizeInBits, channels, signed, bigEndian);
   private static final int frameSizeInBytes = format.getFrameSize();
   private static final int bufferLengthInFrames = 16384 / 8;
   private static final int bufferLengthInBytes = bufferLengthInFrames * frameSizeInBytes;

   public float getSampleRate()
   {
      return format.getSampleRate();
   }

   public double[][] getBodeData(double[] time, double[] data)
   {
      double[][] freqMagPhase = BodePlotConstructor.computeFreqMagPhase(time, data);

      double[] frequency = freqMagPhase[0];
      double[] magnitude = new double[freqMagPhase[1].length];
      for (int i = 0; i < freqMagPhase[1].length; i++)
      {
         magnitude[i] = Conversions.amplitudeToDecibels(freqMagPhase[1][i]);
      }
      double[] phase = new double[freqMagPhase[2].length];
      for (int i = 0; i < freqMagPhase[2].length; i++)
      {
         phase[i] = Math.toDegrees(freqMagPhase[2][i]);
      }

      double[][] bodeData = new double[][] { frequency, magnitude, phase };
      return bodeData;
   }

   public DrillDetectionResult isDrillOn(InputStream inputStream)
   {
      if (format.getSampleSizeInBits() != 16)
      {
         System.out.println("Can't detect: bad sample size");
         return null;
      }

      try
      {
         byte[] data = new byte[bufferLengthInBytes];
         int numBytesRead = inputStream.read(data, 0, bufferLengthInBytes);
         return isDrillOn(data, numBytesRead);
      }
      catch (Exception ignored)
      {
         System.out.println("Failed to read from stream...");
         return null;
      }
   }

   public abstract DrillDetectionResult isDrillOn(byte[] audioBytes, int size);
   public abstract int getNumReturnedBands();
}

<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTNode2D.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

public class RRTNode2D extends RRTNode
{
   static double boxCenterX = 2.0;
   static double boxCenterY = 1.0;
   static double boxSizeX = 1.0;
   static double boxSizeY = 1.0;
   
   public RRTNode2D()
   {
      super(2);
   }
   
   public RRTNode2D(double px, double py)
   {
      super(2);
      super.setNodeData(0, px);
      super.setNodeData(1, py);
   }
   
   @Override
   public boolean isValidNode()
   {
      double px = this.getNodeData(0);
      double py = this.getNodeData(1);
      if ((px < (boxCenterX + 0.5*boxSizeX) && px > (boxCenterX - 0.5*boxSizeX)) && (py < (boxCenterY + 0.5*boxSizeY)) && (py > (boxCenterY - 0.5*boxSizeY)))
      {
         return false;
      }
      
      return true;
   }

   @Override
   public RRTNode createNode()
   {
      return new RRTNode2D();
   }

   @Override
   public void setRandomNodeData()
   {
      
   }

}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/BoxMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Point3D32;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.Vector3D32;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.euclid.tuple4D.Quaternion32;

public class BoxMessage extends Packet<BoxMessage>
{
   public boolean isEmpty;
   public Vector3D32 size;
   public Point3D32 center;
   public Quaternion32 orientation;

   public BoxMessage()
   {
   }

   public static BoxMessage emptyBox()
   {
      BoxMessage empty = new BoxMessage();
      empty.isEmpty = true;
      empty.size = null;
      empty.center = null;
      empty.orientation = null;
      return empty;
   }

   @Override
   public void set(BoxMessage other)
   {
      setPacketInformation(other);
      isEmpty = other.isEmpty;
      size = new Vector3D32(other.size);
      center = new Point3D32(center);
      orientation = new Quaternion32(other.orientation);
   }

   public boolean isEmpty()
   {
      return isEmpty;
   }

   public void setSize(Vector3D size)
   {
      isEmpty = false;
      this.size = new Vector3D32(size);
   }

   public void setSize(Vector3D32 size)
   {
      isEmpty = false;
      this.size = size;
   }

   public void setCenter(Point3D center)
   {
      isEmpty = false;
      this.center = new Point3D32(center);
   }

   public void setCenter(Point3D32 center)
   {
      isEmpty = false;
      this.center = center;
   }

   public void setOrientation(Quaternion orientation)
   {
      isEmpty = false;
      this.orientation = new Quaternion32(orientation);
   }

   public void setOrientation(Quaternion32 orientation)
   {
      isEmpty = false;
      this.orientation = orientation;
   }

   public Vector3D32 getSize()
   {
      return size;
   }

   public Point3D32 getCenter()
   {
      return center;
   }

   public Quaternion32 getOrientation()
   {
      return orientation;
   }

   @Override
   public boolean epsilonEquals(BoxMessage other, double epsilon)
   {
      if (!size.epsilonEquals(other.size, (float) epsilon))
         return false;
      if (!center.epsilonEquals(other.center, (float) epsilon))
         return false;
      if (!orientation.epsilonEquals(other.orientation, (float) epsilon))
         return false;
      return true;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/DetectedFacesPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "DetectedFacesPacket" defined in "DetectedFacesPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from DetectedFacesPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit DetectedFacesPacket_.idl instead.
*
*/
public class DetectedFacesPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.DetectedFacesPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::DetectedFacesPacket_";

   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.DetectedFacesPacket 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.DetectedFacesPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0)
      {
        current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1;
      }
      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);}

      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.DetectedFacesPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.DetectedFacesPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getIds().size(); ++i0)
      {
          current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getIds().get(i0).length() + 1;
      }
      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getPositions().size(); ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getPositions().get(i0), current_alignment);}


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.DetectedFacesPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getIds().size() <= 100)
      cdr.write_type_e(data.getIds());else
          throw new RuntimeException("ids field exceeds the maximum length");

      if(data.getPositions().size() <= 100)
      cdr.write_type_e(data.getPositions());else
          throw new RuntimeException("positions field exceeds the maximum length");

   }

   public static void read(controller_msgs.msg.dds.DetectedFacesPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_e(data.getIds());	
      cdr.read_type_e(data.getPositions());	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.DetectedFacesPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_e("ids", data.getIds());
      ser.write_type_e("positions", data.getPositions());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.DetectedFacesPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_e("ids", data.getIds());
      ser.read_type_e("positions", data.getPositions());
   }

   public static void staticCopy(controller_msgs.msg.dds.DetectedFacesPacket src, controller_msgs.msg.dds.DetectedFacesPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.DetectedFacesPacket createData()
   {
      return new controller_msgs.msg.dds.DetectedFacesPacket();
   }
   @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.DetectedFacesPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.DetectedFacesPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.DetectedFacesPacket src, controller_msgs.msg.dds.DetectedFacesPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public DetectedFacesPacketPubSubType newInstance()
   {
      return new DetectedFacesPacketPubSubType();
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/packetCommunicator/PacketCommunicator.java<|end_filename|>
package us.ihmc.communication.packetCommunicator;

import java.io.IOException;
import java.util.HashMap;

import us.ihmc.commons.PrintTools;
import us.ihmc.communication.interfaces.Connectable;
import us.ihmc.communication.net.ConnectionStateListener;
import us.ihmc.communication.net.GlobalObjectConsumer;
import us.ihmc.communication.net.KryoObjectClient;
import us.ihmc.communication.net.KryoObjectServer;
import us.ihmc.communication.net.NetClassList;
import us.ihmc.communication.net.NetClassList.PacketTrimmer;
import us.ihmc.communication.net.NetworkedObjectCommunicator;
import us.ihmc.communication.net.ObjectConsumer;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.net.TcpNetStateListener;
import us.ihmc.communication.net.local.IntraprocessObjectCommunicator;
import us.ihmc.communication.packetCommunicator.interfaces.GlobalPacketConsumer;
import us.ihmc.communication.packets.Packet;
import us.ihmc.communication.util.NetworkPorts;

public class PacketCommunicator implements Connectable
{
   public static final int BUFFER_SIZE = 2097152 * 20;

   private final NetworkedObjectCommunicator communicator;
   private final HashMap<Class<?>, HashMap<PacketConsumer<?>, ObjectConsumer<?>>> consumers = new HashMap<>();
   private final HashMap<GlobalPacketConsumer, GlobalObjectConsumer> globalConsumers = new HashMap<>();

   private final NetClassList netClassList;

   private final String description;

   private static boolean freezeCommunication = false;

   public static PacketCommunicator createTCPPacketCommunicatorClient(String host, NetworkPorts port, NetClassList netClassList)
   {
      return createTCPPacketCommunicatorClient(host, port, netClassList, true);
   }

   public static void freezeCommunication(boolean freeze)
   {
      freezeCommunication = freeze;
   }

   public static PacketCommunicator createTCPPacketCommunicatorClient(String host, NetworkPorts port, NetClassList netClassList, boolean reconnectAutomatically)
   {
      PrintTools.info(PacketCommunicator.class, "Creating Kryo TCP client on port: " + port.getName());
      KryoObjectClient objectCommunicator = new KryoObjectClient(KryoObjectClient.getByName(host), port.getPort(), netClassList, BUFFER_SIZE, BUFFER_SIZE);
      objectCommunicator.setReconnectAutomatically(reconnectAutomatically);
      return new PacketCommunicator("TCPClient[host=" + host + ",port=" + port + "]", objectCommunicator, netClassList);
   }

   public static PacketCommunicator createTCPPacketCommunicatorServer(NetworkPorts port, NetClassList netClassList)
   {
      return createTCPPacketCommunicatorServer(port, BUFFER_SIZE, BUFFER_SIZE, netClassList);
   }

   public static PacketCommunicator createTCPPacketCommunicatorServer(NetworkPorts port, int writeBufferSize, int receiveBufferSize, NetClassList netClassList,
                                                                      int maximumObjectSize)
   {
      PrintTools.info(PacketCommunicator.class, "Creating Kryo TCP server on port: " + port.getName());
      KryoObjectServer server = new KryoObjectServer(port.getPort(), netClassList, writeBufferSize, receiveBufferSize);
      server.setMaximumObjectSize(maximumObjectSize);
      return new PacketCommunicator("TCPServer[port=" + port + "]", server, netClassList);
   }

   public static PacketCommunicator createTCPPacketCommunicatorServer(NetworkPorts port, int writeBufferSize, int receiveBufferSize, NetClassList netClassList)
   {
      return createTCPPacketCommunicatorServer(port, writeBufferSize, receiveBufferSize, netClassList, 0); //infinite
   }

   public static PacketCommunicator createIntraprocessPacketCommunicator(NetworkPorts port, NetClassList netClassList)
   {
      PrintTools.info(PacketCommunicator.class, "Creating Kryo intraprocess on port: " + port.getName());
      return new PacketCommunicator("IntraProcess[port=" + port + "]", new IntraprocessObjectCommunicator(port.getPort(), netClassList), netClassList);
   }

   public static PacketCommunicator createCustomPacketCommunicator(NetworkedObjectCommunicator objectCommunicator, NetClassList netClassList)
   {
      PrintTools.info(PacketCommunicator.class, "Creating custom");
      return new PacketCommunicator("Custom[class=" + objectCommunicator.getClass().getSimpleName() + "]", objectCommunicator, netClassList);
   }

   private PacketCommunicator(String description, NetworkedObjectCommunicator communicator, NetClassList netClassList)
   {
      this.description = description;
      this.communicator = communicator;
      this.netClassList = netClassList;
   }

   public void attachStateListener(ConnectionStateListener stateListener)
   {
      communicator.attachStateListener(stateListener);
   }

   public void attachStateListener(TcpNetStateListener stateListener)
   {
      communicator.attachStateListener(stateListener);
   }

   public <T extends Packet<?>> void attachListener(Class<T> clazz, PacketConsumer<T> listener)
   {
      PacketObjectConsumer<T> objectConsumer = new PacketObjectConsumer<>(listener);
      HashMap<PacketConsumer<?>, ObjectConsumer<?>> clazzConsumers = consumers.get(clazz);
      if (clazzConsumers == null)
      {
         clazzConsumers = new HashMap<>();
         consumers.put(clazz, clazzConsumers);
      }

      clazzConsumers.put(listener, objectConsumer);
      communicator.attachListener(clazz, objectConsumer);
   }

   @SuppressWarnings({"unchecked", "rawtypes"})
   public <T extends Packet> void detachListener(Class<T> clazz, PacketConsumer<T> listener)
   {
      HashMap<PacketConsumer<?>, ObjectConsumer<?>> clazzConsumers = consumers.get(clazz);
      if (clazzConsumers == null)
      {
         return;
      }
      ObjectConsumer consumer = clazzConsumers.get(listener);
      if (consumer != null)
      {
         communicator.detachListener(clazz, consumer);
         consumers.remove(listener);
      }
   }

   public void attachGlobalListener(GlobalPacketConsumer listener)
   {
      GlobalPacketObjectConsumer consumer = new GlobalPacketObjectConsumer(listener);
      globalConsumers.put(listener, consumer);
      communicator.attachGlobalListener(consumer);
   }

   public void detachGlobalListener(GlobalPacketConsumer listener)
   {
      GlobalObjectConsumer consumer = globalConsumers.get(listener);
      if (consumer != null)
      {
         communicator.detachGlobalListener(consumer);
         globalConsumers.remove(listener);
      }
   }

   @Override
   public boolean isConnected()
   {
      return communicator.isConnected();
   }

   public void closeConnection()
   {
      communicator.closeConnection();
   }

   public void disconnect()
   {
      try
      {
         communicator.disconnect();
      }
      catch (IOException e)
      {
         // handle silently for now
      }
   }

   public void connect() throws IOException
   {
      communicator.connect();
   }

   /**
    * @param packet Send a packet to connected receivers. Does not call listeners
    * @return
    */
   @SuppressWarnings({"unchecked", "rawtypes"})
   public int send(Packet<?> packet)
   {
      if (!freezeCommunication)
      {
         PacketTrimmer packetTrimmer = netClassList.getPacketTrimmer(packet.getClass());
         if (packetTrimmer != null)
            return communicator.send(packetTrimmer.trim(packet));
         else
            return communicator.send(packet);
      }
      else
      {
         return -1;
      }
   }

   private static class GlobalPacketObjectConsumer implements GlobalObjectConsumer
   {
      private final GlobalPacketConsumer globalPacketConsumer;

      private GlobalPacketObjectConsumer(GlobalPacketConsumer globalPacketConsumer)
      {
         this.globalPacketConsumer = globalPacketConsumer;
      }

      @Override
      public void consumeObject(Object object)
      {
         if (object instanceof Packet)
         {
            if (!freezeCommunication)
               globalPacketConsumer.receivedPacket((Packet<?>) object);
         }
      }
   }

   private static class PacketObjectConsumer<T extends Packet<?>> implements ObjectConsumer<T>
   {
      private final PacketConsumer<T> packetConsumer;

      private PacketObjectConsumer(PacketConsumer<T> packetConsumer)
      {
         this.packetConsumer = packetConsumer;
      }

      @Override
      public void consumeObject(T object)
      {
         if (!freezeCommunication)
            packetConsumer.receivedPacket(object);
      }
   }

   @Override
   public String toString()
   {
      return description;
   }

}


<|start_filename|>websitedocs/snippeterrorchecking/snippetErrors.js<|end_filename|>


//For this to work, npm install isomorphic-fetch, jsdom, and jquery

fetch = require('isomorphic-fetch');
var jsdom = require('jsdom');
const { JSDOM } = jsdom;
const { window } = new JSDOM();
const { document } = (new JSDOM('')).window;
global.document = document;
var $ = jQuery = require('jquery')(window);
var matchIndex, allCodeBlocks;

function checkSnippet() {
	var script = document.getElementById("snippetscript");
	var sources = eval(script.getAttribute('sources'));
	var numberOfSources = sources.length;

	// Getting the attributes from all the code blocks
	allCodeBlocks = Array.from(document.getElementsByTagName('code'));
	var urls = []; // array of data from each source

	// Throws error on invalid url
	function notValidURL(response) {
		if (!response.ok) {
			process.exitCode = 1;
			throw Error(response.statusText);
		}
		return response;
	}

	// Get and operate on data from source files
	for (var i = 0; i < numberOfSources; i++) {
		urls[i] = fetch(sources[i]).then(notValidURL).then(function(response) {
			return response.text()
		});
	}

	Promise
			.all(urls)
			.then(
					function(values) {
						for (var urlIndex = 0; urlIndex < numberOfSources; urlIndex++) {
							! function(urlIndex) {
							var dataFromSource = values[urlIndex];
							matchIndex = allCodeBlocks.findIndex(function(
									element) {
								return element.getAttribute('data-url-index') == urlIndex
										.toString();
							});

							// For each URL, find all the code blocks with matching
							// data-url-index
							while (matchIndex != -1) {

								// Change inner HTML of code block
								var codeBlock = allCodeBlocks[matchIndex];
								var typeOfSnippet = codeBlock
										.getAttribute('data-snippet');
								var startIndex, endindex;

								 if (typeOfSnippet == "portion") {
									startIndex = dataFromSource.indexOf(codeBlock
											.getAttribute('data-start'));
									if(startIndex < 0) {
										console.log("Start string not found at element id: " + codeBlock.id);
										process.exitCode = 1;
                    allCodeBlocks.splice(matchIndex, 1);
                    matchIndex = allCodeBlocks
                      .findIndex(function(element) {
                        return element
                          .getAttribute('data-url-index') == urlIndex
                          .toString();
                      });
                    continue;
									}

									// Substring from index to the rest of file
									if (codeBlock.getAttribute('data-end') != null) {
										endIndex = dataFromSource.indexOf(codeBlock
												.getAttribute('data-end'),
												startIndex);
										if(endIndex < 0) {
											console.log("End string not found at element id: " + codeBlock.id);
											process.exitCode = 1;
                      allCodeBlocks.splice(matchIndex, 1);
                      matchIndex = allCodeBlocks
                        .findIndex(function(element) {
                          return element
                            .getAttribute('data-url-index') == urlIndex
                            .toString();
                        });
                      continue;
									}
								}
								} else if (typeOfSnippet == "multipleportions")
								{
									var portions = eval(codeBlock
											.getAttribute('data-portions'));
									for (j = 0; j < portions.length; j++) {
										startIndex = dataFromSource
												.indexOf(portions[j][0]);
										if(startIndex < 0) {
											console.log("Start string not found at element id: " + codeBlock.id);
											process.exitCode = 1;
                      continue;
										}
										// Substring with start index to rest of
										// file
										if (portions[j].length > 1) {
											endIndex = dataFromSource.indexOf(
													portions[j][1], startIndex);
											if(endIndex < 0) {
												console.log("End string not found at element id: " + codeBlock.id);
												process.exitCode = 1;
                        continue;
											}
										}
									}
								}
								allCodeBlocks.splice(matchIndex, 1);

								matchIndex = allCodeBlocks  
										.findIndex(function(element) {
											return element
													.getAttribute('data-url-index') == urlIndex
													.toString();
										});
							}
					}(urlIndex);
				}
			});
}
		//using shell script
		var webpageUrl = process.argv[2];
		$.get(webpageUrl, function(data) {
			rawHTML = $.parseHTML(data, document, true);
			document.body.appendChild(rawHTML[rawHTML.length-1]);
			checkSnippet();
		});


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/planarRegion/PlanarRegionSegmentationParameters.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.planarRegion;

import java.util.Scanner;

import us.ihmc.jOctoMap.tools.ScannerTools;

public class PlanarRegionSegmentationParameters
{
   private static final double DEFAULT_SEARCH_RADIUS = 0.05;
   private static final double DEFAULT_MAX_DISTANCE_FROM_PLANE = 0.05;
   private static final double DEFAULT_MAX_ANGLE_FROM_PLANE = Math.toRadians(10.0);
   private static final double DEFAULT_MIN_NORMAL_QUALITY = 0.005;
   private static final int DEFAULT_MIN_REGION_SIZE = 50;
   private static final double DEFAULT_MAX_STANDARD_DEVIATION = 0.015;
   private static final double DEFAULT_VOLUMIC_DENSITY = 0.10 * 1.0e6; // cm^3 to m^3

   private double searchRadius;
   private double maxDistanceFromPlane;
   private double maxAngleFromPlane;
   private double minNormalQuality;
   private int minRegionSize;

   private double maxStandardDeviation;
   private double minVolumicDensity;

   public PlanarRegionSegmentationParameters()
   {
      setDefaultParameters();
   }

   public PlanarRegionSegmentationParameters(PlanarRegionSegmentationParameters other)
   {
      set(other);
   }

   public void setDefaultParameters()
   {
      searchRadius = DEFAULT_SEARCH_RADIUS;
      maxDistanceFromPlane = DEFAULT_MAX_DISTANCE_FROM_PLANE;
      maxAngleFromPlane = DEFAULT_MAX_ANGLE_FROM_PLANE;
      minNormalQuality = DEFAULT_MIN_NORMAL_QUALITY;
      minRegionSize = DEFAULT_MIN_REGION_SIZE;

      maxStandardDeviation = DEFAULT_MAX_STANDARD_DEVIATION;
      minVolumicDensity = DEFAULT_VOLUMIC_DENSITY;
   }

   public void set(PlanarRegionSegmentationParameters other)
   {
      searchRadius = other.searchRadius;
      maxDistanceFromPlane = other.maxDistanceFromPlane;
      maxAngleFromPlane = other.maxAngleFromPlane;
      minNormalQuality = other.minNormalQuality;
      minRegionSize = other.minRegionSize;
      maxStandardDeviation = other.maxStandardDeviation;
      minVolumicDensity = other.minVolumicDensity;
   }

   public void setSearchRadius(double searchRadius)
   {
      this.searchRadius = searchRadius;
   }

   public void setMaxDistanceFromPlane(double maxDistanceFromPlane)
   {
      this.maxDistanceFromPlane = maxDistanceFromPlane;
   }

   public void setMaxAngleFromPlane(double maxAngleFromPlane)
   {
      this.maxAngleFromPlane = maxAngleFromPlane;
   }

   public void setMinNormalQuality(double minNormalQuality)
   {
      this.minNormalQuality = minNormalQuality;
   }

   public void setMinRegionSize(int minRegionSize)
   {
      this.minRegionSize = minRegionSize;
   }

   public void setMaxStandardDeviation(double maxStandardDeviation)
   {
      this.maxStandardDeviation = maxStandardDeviation;
   }

   public void setMinVolumicDensity(double minVolumicDensity)
   {
      this.minVolumicDensity = minVolumicDensity;
   }

   public double getSearchRadius()
   {
      return searchRadius;
   }

   public double getMaxDistanceFromPlane()
   {
      return maxDistanceFromPlane;
   }

   public double getMaxAngleFromPlane()
   {
      return maxAngleFromPlane;
   }

   public double getMinNormalQuality()
   {
      return minNormalQuality;
   }

   public int getMinRegionSize()
   {
      return minRegionSize;
   }

   public double getMaxStandardDeviation()
   {
      return maxStandardDeviation;
   }

   public double getMinVolumicDensity()
   {
      return minVolumicDensity;
   }

   @Override
   public String toString()
   {
      return "search radius: " + searchRadius + ", max distance from plane: " + maxDistanceFromPlane
            + ", maxAngleFromPlane: " + maxAngleFromPlane + ", minNormalQuality: " + minNormalQuality + ", min region size: " + minRegionSize
            + ", max standard deviation: " + maxStandardDeviation + ", min volumic density: " + minVolumicDensity;
   }

   public static PlanarRegionSegmentationParameters parse(String parametersAsString)
   {
      parametersAsString = parametersAsString.replace(",", "");
      Scanner scanner = new Scanner(parametersAsString);
      PlanarRegionSegmentationParameters parameters = new PlanarRegionSegmentationParameters();
      parameters.setSearchRadius(ScannerTools.readNextDouble(scanner, parameters.getSearchRadius()));
      parameters.setMaxDistanceFromPlane(ScannerTools.readNextDouble(scanner, parameters.getMaxDistanceFromPlane()));
      parameters.setMaxAngleFromPlane(ScannerTools.readNextDouble(scanner, parameters.getMaxAngleFromPlane()));
      parameters.setMinNormalQuality(ScannerTools.readNextDouble(scanner, parameters.getMinNormalQuality()));
      parameters.setMinRegionSize(ScannerTools.readNextInt(scanner, parameters.getMinRegionSize()));
      parameters.setMaxStandardDeviation(ScannerTools.readNextDouble(scanner, parameters.getMaxStandardDeviation()));
      parameters.setMinVolumicDensity(ScannerTools.readNextDouble(scanner, parameters.getMinVolumicDensity()));
      scanner.close();
      return parameters;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/PelvisPoseCorrectionCommunicatorInterface.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import controller_msgs.msg.dds.LocalizationPacket;
import controller_msgs.msg.dds.PelvisPoseErrorPacket;
import controller_msgs.msg.dds.StampedPosePacket;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.pubsub.subscriber.Subscriber;
import us.ihmc.ros2.NewMessageListener;

public interface PelvisPoseCorrectionCommunicatorInterface extends NewMessageListener<StampedPosePacket>, PacketConsumer<StampedPosePacket>
{
   public boolean hasNewPose();

   public StampedPosePacket getNewExternalPose();
   
   public void sendPelvisPoseErrorPacket(PelvisPoseErrorPacket pelvisPoseErrorPacket);
   
   public void sendLocalizationResetRequest(LocalizationPacket localizationPacket);

   @Override
   default void onNewDataMessage(Subscriber<StampedPosePacket> subscriber)
   {
      receivedPacket(subscriber.takeNextData());
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/feedbackController/FeedbackControllerSettings.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.momentumBasedController.feedbackController;

import java.util.List;

import us.ihmc.commonWalkingControlModules.configurations.GroupParameter;
import us.ihmc.commonWalkingControlModules.controllerCore.FeedbackControllerToolbox;

public interface FeedbackControllerSettings
{
   /**
    * Whether to setup the feedback controllers for computing a feedback term proportional to the
    * position/orientation error integrated. IMPORTANT: This cannot be changed at runtime.
    * 
    * @return {@code true} if the variables to create the integral term are to be created,
    *         {@code false} otherwise.
    */
   default boolean enableIntegralTerm()
   {
      return true;
   }

   /**
    * Gets a list from end-effector and/or joint name to break frequency that is to be used to setup
    * a low-pass filter on the velocity error computed for that end-effector or joint and how to
    * regroup the parameters.
    * <p>
    * If there is no break frequency for a end-effector or a joint, no filter will be setup and this
    * cannot be changed later on. If the returned map is {@code null}, no filters at all will be
    * setup, this cannot be changed later on.
    * </p>
    * <p>
    * Note: for the center of mass feedback controller, use the following {@code String} for the
    * key: {@link FeedbackControllerToolbox#centerOfMassName}.
    * </p>
    * 
    * @return the list of grouped parameters to use for setting up break frequency providers.
    */
   default List<GroupParameter<Double>> getErrorVelocityFilterBreakFrequencies()
   {
      return null;
   }

   /**
    * Creates an instance of {@code FeedbackControllerSettings} for a default setup of the feedback
    * controllers.
    * 
    * @return the default settings.
    */
   public static FeedbackControllerSettings getDefault()
   {
      return new FeedbackControllerSettings()
      {
      };
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/DesiredFootstepCalculatorTools.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotics.contactable.ContactablePlaneBody;

public class DesiredFootstepCalculatorTools
{
   public static double computeMinZPointWithRespectToAnkleInWorldFrame(RotationMatrix footToWorldRotation, ContactablePlaneBody contactableBody)
   {
      List<FramePoint3D> footPoints = contactableBody.getContactPointsCopy();
      double minZ = Double.POSITIVE_INFINITY;
      FramePoint3D tempFramePoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      Vector3D tempVector = new Vector3D();
      for (FramePoint3D footPoint : footPoints)
      {
         tempFramePoint.setIncludingFrame(footPoint);
         tempFramePoint.changeFrame(contactableBody.getFrameAfterParentJoint());
         tempVector.set(tempFramePoint);
         footToWorldRotation.transform(tempVector);
         if (tempVector.getZ() < minZ)
            minZ = tempVector.getZ();
      }

      return minZ;
   }

   public static double computeMinZPointWithRespectToSoleInWorldFrame(RotationMatrix footToWorldRotation, ContactablePlaneBody contactableBody)
   {
      List<FramePoint3D> footPoints = contactableBody.getContactPointsCopy();
      double minZ = Double.POSITIVE_INFINITY;
      FramePoint3D tempFramePoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      Vector3D tempVector = new Vector3D();
      for (FramePoint3D footPoint : footPoints)
      {
         tempFramePoint.setIncludingFrame(footPoint);
         tempFramePoint.changeFrame(contactableBody.getSoleFrame());
         tempVector.set(tempFramePoint);
         footToWorldRotation.transform(tempVector);
         if (tempVector.getZ() < minZ)
            minZ = tempVector.getZ();
      }

      return minZ;
   }

   public static FramePoint3D computeMinZWithRespectToAnkleInWorldFramePoint(RotationMatrix footToWorldRotation, ContactablePlaneBody contactableBody)
   {
      List<FramePoint3D> footPoints = contactableBody.getContactPointsCopy();
      double minZ = Double.POSITIVE_INFINITY;
      FramePoint3D tempFramePoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      FramePoint3D minZPoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      Vector3D tempVector = new Vector3D();
      for (FramePoint3D footPoint : footPoints)
      {
         tempFramePoint.setIncludingFrame(footPoint);
         tempFramePoint.changeFrame(contactableBody.getFrameAfterParentJoint());
         tempVector.set(tempFramePoint);
         footToWorldRotation.transform(tempVector);

         if (tempVector.getZ() < minZ)
         {
            minZPoint.setIncludingFrame(tempFramePoint);
            minZ = tempVector.getZ();
         }
      }

      return minZPoint;
   }

   public static double computeMaxXWithRespectToAnkleInFrame(RotationMatrix footToWorldRotation, ContactablePlaneBody contactableBody, ReferenceFrame frame)
   {
      RigidBodyTransform worldToDesiredHeadingFrame = frame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
      List<FramePoint3D> footPoints = contactableBody.getContactPointsCopy();
      double maxX = Double.NEGATIVE_INFINITY;
      FramePoint3D tempFramePoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      Vector3D tempVector = new Vector3D();
      for (FramePoint3D footPoint : footPoints)
      {
         tempFramePoint.setIncludingFrame(footPoint);
         tempFramePoint.changeFrame(contactableBody.getFrameAfterParentJoint());
         tempVector.set(tempFramePoint); // foot point w.r.t. ankle in foot frame
         footToWorldRotation.transform(tempVector); // foot point w.r.t. ankle in world frame
         worldToDesiredHeadingFrame.transform(tempVector); // foot point w.r.t. ankle in desired heading frame
         if (tempVector.getX() > maxX)
            maxX = tempVector.getX();
      }

      return maxX;
   }

   public static FramePoint3D computeMinZPointInFrame(RigidBodyTransform footToWorldTransform, ContactablePlaneBody contactableBody, ReferenceFrame frame)
   {
      List<FramePoint3D> footPoints = contactableBody.getContactPointsCopy();

      ReferenceFrame bodyFrame = contactableBody.getFrameAfterParentJoint();

      return computeMinZPointInFrame(footToWorldTransform, footPoints, bodyFrame, frame);
   }

   public static FramePoint3D computeMinZPointInFrame(RigidBodyTransform footToWorldTransform, List<FramePoint3D> footPoints, ReferenceFrame bodyFrame,
         ReferenceFrame frame)
   {
      FramePoint3D minFramePoint = new FramePoint3D(frame);
      minFramePoint.setZ(Double.POSITIVE_INFINITY);
      FramePoint3D tempFramePoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      boolean pointFound = false;
      for (FramePoint3D footPoint : footPoints)
      {
         tempFramePoint.setIncludingFrame(footPoint);
         tempFramePoint.changeFrame(bodyFrame);
         tempFramePoint.applyTransform(footToWorldTransform);
         tempFramePoint.setIncludingFrame(ReferenceFrame.getWorldFrame(), tempFramePoint);
         tempFramePoint.changeFrame(frame);

         if (tempFramePoint.getZ() < minFramePoint.getZ())
         {
            minFramePoint.set(tempFramePoint);
            pointFound = true;
         }
      }

      if (!pointFound)
         throw new RuntimeException();

      return minFramePoint;
   }

   public static FramePoint3D computeMaxXPointInFrame(RigidBodyTransform footToWorldTransform, ContactablePlaneBody contactableBody, ReferenceFrame frame)
   {
      List<FramePoint3D> footPoints = contactableBody.getContactPointsCopy();
      FramePoint3D maxFramePoint = new FramePoint3D(frame);
      maxFramePoint.setX(Double.NEGATIVE_INFINITY);
      FramePoint3D tempFramePoint = new FramePoint3D(ReferenceFrame.getWorldFrame());
      boolean pointFound = false;
      for (FramePoint3D footPoint : footPoints)
      {
         tempFramePoint.setIncludingFrame(footPoint);
         tempFramePoint.changeFrame(contactableBody.getFrameAfterParentJoint());
         tempFramePoint.applyTransform(footToWorldTransform);
         tempFramePoint.setIncludingFrame(ReferenceFrame.getWorldFrame(), tempFramePoint);
         tempFramePoint.changeFrame(frame);

         if (tempFramePoint.getX() > maxFramePoint.getX())
         {
            maxFramePoint.set(tempFramePoint);
            pointFound = true;
         }
      }

      if (!pointFound)
         throw new RuntimeException();

      return maxFramePoint;
   }

   public static List<FramePoint3D> computeMaximumPointsInDirection(List<FramePoint3D> framePoints, FrameVector3D searchDirection, int nPoints)
   {
      if (framePoints.size() < nPoints)
         throw new RuntimeException("Not enough points");
      searchDirection.changeFrame(framePoints.get(0).getReferenceFrame());
      List<FramePoint3D> ret = new ArrayList<FramePoint3D>(framePoints);
      Collections.sort(ret, new SearchDirectionFramePointComparator(searchDirection));

      while (ret.size() > nPoints)
      {
         ret.remove(0);
      }

      return ret;
   }

   public static List<Point2D> computeMaximumPointsInDirection(List<Point2D> framePoints, Vector2D searchDirection, int nPoints)
   {
      if (framePoints.size() < nPoints)
         throw new RuntimeException("Not enough points");
      List<Point2D> ret = new ArrayList<Point2D>(framePoints);
      Collections.sort(ret, new SearchDirectionPoint2dComparator(searchDirection));

      while (ret.size() > nPoints)
      {
         ret.remove(0);
      }

      return ret;
   }

   public static int[] findMaximumPointIndexesInDirection(List<FramePoint3D> framePoints, FrameVector3D searchDirection, int nPoints)
   {
      List<FramePoint3D> maximumPoints = computeMaximumPointsInDirection(framePoints, searchDirection, nPoints);

      int[] indexes = new int[nPoints];

      for (int i = 0; i < nPoints; i++)
      {
         indexes[i] = framePoints.indexOf(maximumPoints.get(i));
      }

      return indexes;
   }

   private static class SearchDirectionFramePointComparator implements Comparator<FramePoint3D>
   {
      private final FrameVector3D searchDirection;
      private final FrameVector3D differenceVector = new FrameVector3D(ReferenceFrame.getWorldFrame());

      public SearchDirectionFramePointComparator(FrameVector3D searchDirection)
      {
         this.searchDirection = searchDirection;
      }

      public int compare(FramePoint3D o1, FramePoint3D o2)
      {
         differenceVector.setIncludingFrame(o1);
         differenceVector.sub(o2);
         double dotProduct = searchDirection.dot(differenceVector);

         return Double.compare(dotProduct, 0.0);
      }
   }

   private static class SearchDirectionPoint2dComparator implements Comparator<Point2D>
   {
      private final Vector2D searchDirection;
      private final Vector2D differenceVector = new Vector2D();

      public SearchDirectionPoint2dComparator(Vector2D searchDirection)
      {
         this.searchDirection = searchDirection;
      }

      public int compare(Point2D o1, Point2D o2)
      {
         differenceVector.set(o1);
         differenceVector.sub(o2);
         double dotProduct = searchDirection.dot(differenceVector);

         return Double.compare(dotProduct, 0.0);
      }
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/local/IntraprocessObjectCommunicator.java<|end_filename|>
package us.ihmc.communication.net.local;

import java.io.IOException;
import java.util.ArrayList;
import java.util.LinkedHashMap;

import com.esotericsoftware.kryo.Kryo;

import us.ihmc.communication.net.ConnectionStateListener;
import us.ihmc.communication.net.GlobalObjectConsumer;
import us.ihmc.communication.net.NetClassList;
import us.ihmc.communication.net.NetworkedObjectCommunicator;
import us.ihmc.communication.net.ObjectConsumer;
import us.ihmc.communication.net.TcpNetStateListener;

public class IntraprocessObjectCommunicator implements NetworkedObjectCommunicator
{
   private final Kryo kryo = new Kryo();
   private final LinkedHashMap<Class<?>, ArrayList<ObjectConsumer<?>>> listeners = new LinkedHashMap<Class<?>, ArrayList<ObjectConsumer<?>>>();
   private final ArrayList<GlobalObjectConsumer> globalListeners = new ArrayList<GlobalObjectConsumer>();
   private final ArrayList<ConnectionStateListener> stateListeners = new ArrayList<ConnectionStateListener>();

   private final int port;

   /**
    * Simple client for Interprocess Communication using objects. Will connect to a predefined port, mimicking TCP/UDP. Easily replaceable by an UDP/TCP implementation, but is traceable in debug.
    *  
    * @param port
    * @param classList for copying objects
    */
   public IntraprocessObjectCommunicator(int port, NetClassList classList)
   {
      this.port = port;
      classList.registerWithKryo(kryo);

      for (Class<?> clazz : classList.getPacketClassList())
      {
         listeners.put(clazz, new ArrayList<ObjectConsumer<?>>());
      }
   }

   @Override
   public void consumeObject(Object object)
   {
      send(object);
   }
   
   @Override
   public int send(Object object)
   {
      return IntraprocessCommunicationNetwork.sendObject(this, port, object);
   }
   
   @SuppressWarnings("unchecked")
   /* package-private */void receiveObject(Object object)
   {
      for (GlobalObjectConsumer listener : globalListeners)
      {
         listener.consumeObject(object);
      }

      ArrayList<ObjectConsumer<?>> objectListeners = listeners.get(object.getClass());
      if (objectListeners != null)
      {
         for (@SuppressWarnings("rawtypes")
         ObjectConsumer listener : objectListeners)
         {
            listener.consumeObject(object);
         }
      }
   }

   @Override
   public void attachStateListener(TcpNetStateListener stateListener)
   {
      throw new RuntimeException("TcpNetStateListeners are not allowed for intraprocess communication!");
   }

   @Override
   public void attachStateListener(ConnectionStateListener stateListener)
   {
      stateListeners.add(stateListener);
   }

   @Override
   public <T> void attachListener(Class<T> clazz, ObjectConsumer<T> listener)
   {
      if (listeners.containsKey(clazz))
      {
         listeners.get(clazz).add(listener);
      }
      else
      {
         throw new RuntimeException("Class " + clazz.getSimpleName() + " is not registered with ObjectCommunicator");
      }
   }

   @Override
   public <T> void detachListener(Class<T> clazz, ObjectConsumer<T> listener)
   {
      if (listeners.containsKey(clazz))
      {
         ArrayList<ObjectConsumer<?>> listenerList = listeners.get(clazz);
         if (listenerList.contains(listener))
         {
            listenerList.remove(listener);
         }
      }
   }

   @Override
   public void attachGlobalListener(GlobalObjectConsumer listener)
   {
      globalListeners.add(listener);
   }

   @Override
   public void detachGlobalListener(GlobalObjectConsumer listener)
   {
      if (globalListeners.contains(listener))
      {
         globalListeners.remove(listener);
      }
   }

   @Override
   public boolean isConnected()
   {
      return IntraprocessCommunicationNetwork.isConnected(this, port);
   }

   @Override
   public void closeConnection()
   {
      disconnect();
   }
   
   @Override
   public void disconnect()
   {
      IntraprocessCommunicationNetwork.disconnect(this, port);
   }

   @Override
   public void connect() throws IOException
   {
      IntraprocessCommunicationNetwork.connect(this, port);
   }

   /* package-private */void connected()
   {
      for (ConnectionStateListener stateListener : stateListeners)
      {
         stateListener.connected();
      }
   }

   /* package-private */void disconnected()
   {
      for (ConnectionStateListener stateListener : stateListeners)
      {
         stateListener.disconnected();
      }
   }

   @Override
   protected void finalize()
   {
      if(isConnected())
      {
         disconnect();         
      }
   }

   public Object copyPacket(Object object)
   {
      return kryo.copy(object);
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/genericQuadruped/parameters/GenericQuadrupedInitialPositionParameters.java<|end_filename|>
package us.ihmc.exampleSimulations.genericQuadruped.parameters;

import us.ihmc.quadrupedRobotics.model.QuadrupedInitialPositionParameters;
import us.ihmc.robotics.partNames.QuadrupedJointName;

public abstract class GenericQuadrupedInitialPositionParameters implements QuadrupedInitialPositionParameters
{
   private static final boolean INVERT_REAR_LEGS = true;

   @Override
   public double getInitialJointPosition(QuadrupedJointName joint)
   {
      double directionalMultiplier;
      if (INVERT_REAR_LEGS)
         directionalMultiplier = -1.0;
      else
         directionalMultiplier = 1.0;

      switch (joint)
      {
      case FRONT_LEFT_HIP_ROLL:
      case HIND_LEFT_HIP_ROLL:
         return getHipRollAngle();
      case FRONT_RIGHT_HIP_ROLL:
      case HIND_RIGHT_HIP_ROLL:
         return - getHipRollAngle();
      case FRONT_LEFT_HIP_PITCH:
      case FRONT_RIGHT_HIP_PITCH:
         return getHipPitchAngle();
      case HIND_LEFT_HIP_PITCH:
      case HIND_RIGHT_HIP_PITCH:
         return directionalMultiplier * getHipPitchAngle();
      case FRONT_LEFT_KNEE_PITCH:
      case FRONT_RIGHT_KNEE_PITCH:
         return getKneePitchAngle();
      case HIND_LEFT_KNEE_PITCH:
      case HIND_RIGHT_KNEE_PITCH:
         return directionalMultiplier * getKneePitchAngle();
      }

      throw new RuntimeException(joint + " not defined!");
   }

   abstract double getHipRollAngle();
   abstract double getHipPitchAngle();
   abstract double getKneePitchAngle();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/TimestampedPoint.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import us.ihmc.euclid.tuple3D.Point3D32;

public class TimestampedPoint extends Point3D32
{
   private static final long serialVersionUID = 2910113387832828116L;
   
   final long timestamp;

   public TimestampedPoint(float x, float y, float z, long t)
   {
      super(x, y, z);
      timestamp = t;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/Polygon2DMessage.java<|end_filename|>
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;

/**
       * General purpose message that contains vertices to represent a 2D polygon (z ignored).
       */
public class Polygon2DMessage extends Packet<Polygon2DMessage> implements Settable<Polygon2DMessage>, EpsilonComparable<Polygon2DMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  vertices_;

   public Polygon2DMessage()
   {
      vertices_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D> (50, new geometry_msgs.msg.dds.PointPubSubType());

   }

   public Polygon2DMessage(Polygon2DMessage other)
   {
      this();
      set(other);
   }

   public void set(Polygon2DMessage other)
   {
      sequence_id_ = other.sequence_id_;

      vertices_.set(other.vertices_);
   }

   /**
            * 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 us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  getVertices()
   {
      return vertices_;
   }


   public static Supplier<Polygon2DMessagePubSubType> getPubSubType()
   {
      return Polygon2DMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return Polygon2DMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(Polygon2DMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.vertices_.size() != other.vertices_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.vertices_.size(); i++)
         {  if (!this.vertices_.get(i).epsilonEquals(other.vertices_.get(i), epsilon)) return false; }
      }


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof Polygon2DMessage)) return false;

      Polygon2DMessage otherMyClass = (Polygon2DMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.vertices_.equals(otherMyClass.vertices_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("Polygon2DMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("vertices=");
      builder.append(this.vertices_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/MessagerStandaloneFootstepPlannerUI.java<|end_filename|>
package us.ihmc.footstepPlanning.ui;

import javafx.application.Application;
import javafx.stage.Stage;
import us.ihmc.footstepPlanning.communication.FootstepPlannerMessagerAPI;
import us.ihmc.footstepPlanning.ui.components.FootstepPathCalculatorModule;
import us.ihmc.javaFXToolkit.messager.SharedMemoryJavaFXMessager;

/**
 * This class provides a fully remote footstep planner. It includes the UI and the footstep planning modules, which are
 * available using JavaFX shared memory messages.
 */
public class MessagerStandaloneFootstepPlannerUI extends Application
{
   private SharedMemoryJavaFXMessager messager;
   private FootstepPathCalculatorModule module;

   private FootstepPlannerUI ui;

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      messager = new SharedMemoryJavaFXMessager(FootstepPlannerMessagerAPI.API);
      messager.startMessager();

      module = FootstepPathCalculatorModule.createMessagerModule(messager);
      module.start();

      ui = FootstepPlannerUI.createMessagerUI(primaryStage, messager);
      ui.show();

   }

   @Override
   public void stop() throws Exception
   {
      super.stop();

      messager.closeMessager();
      module.stop();
      ui.stop();
   }

   public static void main(String[] args)
   {
      launch();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/interfaces/RemoteIPRegistryInterface.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces;

import java.rmi.Remote;
import java.rmi.RemoteException;


public interface RemoteIPRegistryInterface extends Remote
{
   public abstract void registerMe(String hostname, String password) throws RemoteException;

   public abstract String[] getAllRegistered(String password) throws RemoteException;
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/microphone/SoundDetector.java<|end_filename|>
package us.ihmc.avatar.sensors.microphone;

import java.io.ByteArrayOutputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;

import javax.sound.sampled.AudioFormat;
import javax.sound.sampled.AudioInputStream;
import javax.sound.sampled.AudioSystem;
import javax.sound.sampled.DataLine;
import javax.sound.sampled.LineUnavailableException;
import javax.sound.sampled.Mixer;
import javax.sound.sampled.TargetDataLine;
import javax.swing.JLabel;

import org.jtransforms.fft.DoubleFFT_1D;

import controller_msgs.msg.dds.DrillDetectionPacket;
import us.ihmc.commons.Conversions;
import us.ihmc.communication.configuration.NetworkParameterKeys;
import us.ihmc.communication.configuration.NetworkParameters;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.util.NetworkPorts;
import us.ihmc.humanoidRobotics.kryo.IHMCCommunicationKryoNetClassList;

/**
 * 
 * <p>Title: SoundDetector</p>
 *
 * <p>Description: Detects a distinct sound by searching for a characteristic peak in FFT magnitude data of sound data from the Atlas Chest Webcam microphone around a given frequency</p>
 * 
 * 
 * @author Will
 *
 *
 */
public class SoundDetector implements Runnable
{
   private static final float sampleRate = 44100; //Hz
   private static final int sampleSizeInBits = 16;
   private static final int channels = 1;
   private static final boolean signed = true;
   private static final boolean bigEndian = false;
   private static final AudioFormat format = new AudioFormat(sampleRate, sampleSizeInBits, channels, signed, bigEndian);
   private static final int bufferSizeDesired = 16384 * 8;
   private static final int checkForRotozipFrequencyHz = 2; //Hz
   private JLabel statusIndicator = null;
   private final ArrayList<Double[]> peakCriteriaArrayList = new ArrayList<Double[]>();
   private static final String COMMA_DELIMITER = ",";
   private static final String NEW_LINE_SEPARATOR = "\n";
   public static AudioInputStream audioInputStream = null;
   Thread thread = null;
   TargetDataLine line = null;
   String errStr;
   double duration, seconds;
   private PacketCommunicator client = null;
   private boolean runningTheRealSchebang = false;

   public static void main(String[] args)
   {

      SoundDetector soundDetector = new SoundDetector();
      Thread processThread = new Thread(soundDetector);
      processThread.setName("main");
      processThread.start();

   }

   //Detection parameters
   private void setDetectionParams()
   {
      peakCriteriaArrayList.add(new Double[] { 420.0, 480.0, 250.0, 550.0, 9.0 });
      //      peakCriteriaArrayList.add(new Double[] { 2150.0, 2180.0, 2120.0, 2210.0, 5.0 });
      peakCriteriaArrayList.add(new Double[] { 5000.0, 6000.0, 4500.0, 6500.0, 7.0 });
   }

   /**
    * Meant for GUI Testing. Pass a JLabel for indcation that the drill is on
    * @param statusIndicator
    */
   public SoundDetector(JLabel statusIndicator)
   {
      setDetectionParams();
      runningTheRealSchebang = false; //We're probably just running the test GUI for tuning.
      this.statusIndicator = statusIndicator;
   }

   /**
    * Meant for background process to run on Atlas
    */
   public SoundDetector()
   {

      setDetectionParams();
      runningTheRealSchebang = true; //We're sending packets to the network processor. **** just got real.
      String networkManagerHost = NetworkParameters.getHost(NetworkParameterKeys.networkManager);
      client = PacketCommunicator.createTCPPacketCommunicatorClient(networkManagerHost, NetworkPorts.DRILL_DETECTOR,
            new IHMCCommunicationKryoNetClassList());
      try
      {
         System.out.println(getClass().getSimpleName() + ": Connecting to " + networkManagerHost);
         client.connect();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }

   }

   private void shutDown(String errorMessage)
   {
      System.err.println(getClass().getSimpleName() + errorMessage);
      thread = null;
   }

   public void run()
   {

      thread = new Thread("run");
      thread.start();

      DataLine.Info info = new DataLine.Info(TargetDataLine.class, format);

      if (!AudioSystem.isLineSupported(info))
      {
         shutDown(": Audio line not supported");
      }

      try
      {
         line = (TargetDataLine) AudioSystem.getLine(info);
         line.open(format, bufferSizeDesired); //line.getBufferSize() / 8;
      }
      catch (LineUnavailableException ex)
      {
         shutDown("Unable to open the line: " + ex);
         System.out.println("Mixers available:");
         for(Mixer.Info infos : AudioSystem.getMixerInfo()){
            System.out.println(infos.toString());
         }
         return;
      }
      catch (SecurityException ex)
      {
         shutDown(ex.toString());
         return;
      }
      catch (Exception ex)
      {
         shutDown(ex.toString());
         return;
      }

      line.start();
      int frameSizeInBytes = format.getFrameSize();
      int bufferLengthInFrames = bufferSizeDesired / 8; //line.getBufferSize() / 8;
      int bufferLengthInBytes = bufferLengthInFrames * frameSizeInBytes;
      byte[] data = new byte[bufferLengthInBytes];
      int numBytesRead;

      while (thread != null)
      {
         if (runningTheRealSchebang)
         {

            if (!client.isConnected())
            {
               System.out.println("SoundDetector not connected to NP!");
               continue;
            }

         }
         ByteArrayOutputStream out = new ByteArrayOutputStream();
         if ((numBytesRead = line.read(data, 0, bufferLengthInBytes)) == -1)
         {
            break;
         }
         out.write(data, 0, numBytesRead);
         try
         {
            out.flush();
         }
         catch (IOException e)
         {
            shutDown("ByteArrayOutputStream.flush IOException");
         }

         byte audioBytes[] = out.toByteArray();

         if (runningTheRealSchebang)
         {

            DrillDetectionPacket drillDetectionPacket = new DrillDetectionPacket();
            drillDetectionPacket.setIsDrillOn(detectDrillFrequency(audioBytes));
            if(drillDetectionPacket.getIsDrillOn()){
               System.out.println("isDrillOn = true");
            }
            client.send(drillDetectionPacket);
         }
         else
         {

            if (detectDrillFrequency(audioBytes) & statusIndicator != null)
            {
               statusIndicator.setText("ROTOZIP DETECTED!!!");
            }
            else if (statusIndicator != null)
            {
               statusIndicator.setText("Listening...");
            }

         }

         try
         {
            Thread.sleep(1000 / checkForRotozipFrequencyHz);
         }
         catch (InterruptedException e)
         {
            //            e.printStackTrace();
         }

         try
         {
            out.flush();
            out.close();
         }
         catch (IOException ex)
         {
            ex.printStackTrace();
         }
      }

      line.stop();
      line.close();
      line = null;

   }

   private boolean detectDrillFrequency(byte[] audioBytes)
   {
      boolean isDrillOn = false;
      boolean arePeaksThere = false;
      FileWriter fileWriter = null;
      FileWriter fileWriterBode = null;
      if (audioBytes != null)
      {
         int[] audioData = null;
         if (format.getSampleSizeInBits() == 16)
         {
            int nlengthInSamples = audioBytes.length / 2;
            audioData = new int[nlengthInSamples];

            //Little-Endian Code
            for (int i = 0; i < nlengthInSamples; i++)
            {
               /* First byte is LSB (low order) */
               int LSB = (int) audioBytes[2 * i];
               /* Second byte is MSB (high order) */
               int MSB = (int) audioBytes[2 * i + 1];
               audioData[i] = MSB << 8 | (255 & LSB);
            }

            if (!runningTheRealSchebang)
            {
               //Write buffer data to a CSV file
               //            /**
               try
               {
                  fileWriter = new FileWriter("buffercsv.csv");

                  for (int intToWrite : audioData)
                  {

                     fileWriter.append(intToWrite + COMMA_DELIMITER);
                     fileWriter.append(NEW_LINE_SEPARATOR);

                  }
                  fileWriter.flush();
                  fileWriter.close();
               }
               catch (IOException e)
               {
                  e.printStackTrace();
               }
            }
            //            **/

            double[] input = new double[audioData.length];
            double[] time = new double[audioData.length];
            int i = 0;
            for (int intToCast : audioData)
            {
               input[i] = (double) intToCast;
               time[i] = i * 1 / format.getSampleRate();
               i++;
            }

            double[][] fftData = computeFreqMagPhase(time, input);
            double[] frequency = fftData[0];
            double[] magnitude = new double[fftData[1].length];
            for (int j = 0; j < fftData[1].length; j++)
            {
               magnitude[j] = Conversions.amplitudeToDecibels(fftData[1][j]);
            }
            //            double[] phase = fftData[2];

            if (!runningTheRealSchebang)
            {
               //Write frequency and magnitude data to a CSV file
               try
               {
                  fileWriterBode = new FileWriter("freq_mag.csv");

                  for (int i1 = 0; i1 < frequency.length; i1++)
                  {

                     fileWriterBode.append(frequency[i1] + ", " + magnitude[i1] + ",");
                     fileWriterBode.append(NEW_LINE_SEPARATOR);

                  }
                  fileWriterBode.flush();
                  fileWriterBode.close();
               }
               catch (IOException e)
               {
                  e.printStackTrace();
               }
               
               AudioFFTPlotter plot = new AudioFFTPlotter(frequency, magnitude, "Magnitude Plot", "(Hz)", "(dB)");
               plot.packAndDisplayFrame(0, 0);
               
            }

            for (Double[] peakCriteria : peakCriteriaArrayList)
            {

               if (!scanBandForPeak(frequency, magnitude, peakCriteria))
               {
                  arePeaksThere = false;
                  isDrillOn = false;
                  break;
               }
               else
               {
                  arePeaksThere = true;
                  isDrillOn = true;
               }
            }

            if (isFourHundredPeakLoudEnough(frequency, magnitude, peakCriteriaArrayList) & arePeaksThere)
            {
               isDrillOn = true;
            }
            else
            {
               isDrillOn = false;
            }

         }
      }
      return isDrillOn;
   }

   public static double[][] computeFreqMagPhase(double[] time, double[] data)
   {
      int n = time.length;
      if (data.length != n)
         throw new RuntimeException("input.length != n");

      DoubleFFT_1D fft = new DoubleFFT_1D(n);

      double T = time[n - 1] - time[0];

      double fftInput[] = copyDoubleArray(data);

      fft.realForward(fftInput);

      double[] magnitude = extractMagnitude(fftInput);
      double[] phase = extractPhaseRadians(fftInput);
      double[] frequency = new double[n / 2];
      for (int i = 0; i < n / 2; i++)
      {
         frequency[i] = (i) / T;
      }

      return new double[][] { frequency, magnitude, phase };
   }

   private static double[] copyDoubleArray(double[] in)
   {
      double[] ret = new double[in.length];

      for (int i = 0; i < in.length; i++)
      {
         ret[i] = in[i];
      }

      return ret;
   }

   private static double[] extractMagnitude(double[] fftInput)
   {
      int n = fftInput.length;

      //    if (n % 2 != 0)
      //        throw new RuntimeException("Must be even length!");

      double[] magnitude = new double[n / 2];

      for (int k = 0; k < n / 2; k++)
      {
         double real = fftInput[2 * k];
         double imag = fftInput[2 * k + 1];

         if (k == 0)
            imag = 0; // This is due to storing a[1] = Re[n/2] when n is even and = Im[n/2-1] when n is odd, as per the fft specs. Just ignore that term for now...

         double mag = Math.sqrt(real * real + imag * imag);
         magnitude[k] = mag;

         //       magnitude[k] = 20.0 * Math.log10(mag);
      }

      return magnitude;
   }

   private static double[] extractPhaseRadians(double[] fftInput)
   {
      int n = fftInput.length;

      //    if (n % 2 != 0)
      //        throw new RuntimeException("Must be even length!");

      double[] phase = new double[n / 2];

      for (int k = 0; k < n / 2; k++)
      {
         double real = fftInput[2 * k];
         double imag = fftInput[2 * k + 1];

         if (k == 0)
            imag = 0; // This is due to storing a[1] = Re[n/2] when n is even and = Im[n/2-1] when n is odd, as per the fft specs. Just ignore that term for now...

         phase[k] = Math.atan2(imag, real);
      }

      return phase;
   }

   /**
    * 
    * A method to help avoid false positives from peaks that meet scanForPeak criteria but arent loud enough to be a rotozip.
    * 
    * @param frequency
    * @param magnitude
    * @return
    * 
    * A boolean that is true if the sound and at around 430Hz is loud enough. 430Hz hardcoded in here.
    */
   private boolean isFourHundredPeakLoudEnough(double[] frequency, double[] magnitude, ArrayList<Double[]> peakCriteriaArrayList)
   {
      boolean peakLoudEnough = false;
      double peakFourThirty = -100.0;
      double lowerBoundFreq = peakCriteriaArrayList.get(0)[0];
      double upperBoundFreq = peakCriteriaArrayList.get(0)[1];
      int lowerBoundIndex = 0;
      int upperBoundIndex = 0;
      for (int index = 0; index < frequency.length; index++)
      {
         if (frequency[index] < lowerBoundFreq)
         {
            lowerBoundIndex = index;
         }
         if (frequency[index] < upperBoundFreq)
         {
            upperBoundIndex = index;
         }
      }
      for (int index = lowerBoundIndex; index < upperBoundIndex; index++)
      {
         peakFourThirty = (magnitude[index] > peakFourThirty ? magnitude[index] : peakFourThirty);
      }

      if (peakFourThirty > 135.0)
      {
         peakLoudEnough = true;
      }

      return peakLoudEnough;
   }

   /**
    * 
    * Detects peaks in the given magnitude data at frequency ranges given in the peakCriteria (see below)
    * 
    * @param frequency
    * @param magnitude
    * @param peakCriteria[0] = dominantBandLowerBound
    * @param peakCriteria[1] = dominantBandUpperBound
    * @param peakCriteria[2] = relevantBandLowerBound
    * @param peakCriteria[3] = relevantBandUpperBand
    * @param peakCriteria[5] = decibelsToTrip
    * @param peakCriteria
    * 
    * The relevant frequency band is the range of frequencies that are used for comparison to distinguish a peak. 
    * The magnitude data is averaged across this range with the exception of the dominant frequency band magnitudes.
    * 
    * The dominant frequency band is the range of frequencies were the peak in magnitude could show up in. This band must be within the relevant band.
    * 
    * The decibelsToTrip is the minimum difference in magnitude the peak must have over the average magnitude to be considered a significant enough peak
    * to trip detection
    * 
    * @return
    * 
    * A boolean that is true if there is a peak that meets the peak criteria.
    */
   private boolean scanBandForPeak(double[] frequency, double[] magnitude, Double[] peakCriteria)
   {

      boolean peakDetected = false;
      
      int peakIndexSpread = 4;
      {
      
      //Peak Detection Math
      int dominantFrequencyBandLowerBoundIndex = 0;
      int dominantFrequencyBandUpperBoundIndex = 0;
      int relevantFrequencyBandLowerBoundIndex = 0;
      int relevantFrequencyBandUpperBoundIndex = 0;

      double dominantBandLowerBound = peakCriteria[0];
      double dominantBandUpperBound = peakCriteria[1];
      double relevantBandLowerBound = peakCriteria[2];
      double relevantBandUpperBand = peakCriteria[3];
      double decibelsToTrip = peakCriteria[4];

      for (int index = 0; frequency[index] < dominantBandLowerBound; index++)
      {
         dominantFrequencyBandLowerBoundIndex = index;
      }

      for (int index = 0; frequency[index] < dominantBandUpperBound; index++)
      {
         dominantFrequencyBandUpperBoundIndex = index;
      }

      for (int index = 0; frequency[index] < relevantBandLowerBound; index++)
      {
         relevantFrequencyBandLowerBoundIndex = index;
      }

      for (int index = 0; frequency[index] < relevantBandUpperBand; index++)
      {
         relevantFrequencyBandUpperBoundIndex = index;
      }

      double dominantBandPeakMag = 0;
      double relevantBandAverageMag = 0;
      int dominantBandPeakIndex = 0;

      for (int index = dominantFrequencyBandLowerBoundIndex; index < dominantFrequencyBandUpperBoundIndex; index++)
      {
         if (magnitude[index] > dominantBandPeakMag)
         {
            dominantBandPeakMag = magnitude[index];
            dominantBandPeakIndex = index;
         }
      }
      
      dominantFrequencyBandLowerBoundIndex = dominantBandPeakIndex - peakIndexSpread;
      dominantFrequencyBandUpperBoundIndex = dominantBandPeakIndex + peakIndexSpread;
      
      if(dominantFrequencyBandLowerBoundIndex<=relevantFrequencyBandLowerBoundIndex){
         dominantFrequencyBandLowerBoundIndex=relevantFrequencyBandLowerBoundIndex+1;
         System.out.println("WARNING: SoundDetector dominant band range may need to be extended lower for the "+dominantFrequencyBandLowerBoundIndex+" Hz peak detection.");
      }
      
      for (int index = relevantFrequencyBandLowerBoundIndex; index < dominantFrequencyBandLowerBoundIndex; index++)
      {
         relevantBandAverageMag += magnitude[index];
      }
      
      if(relevantFrequencyBandUpperBoundIndex<=dominantFrequencyBandUpperBoundIndex){
         dominantFrequencyBandUpperBoundIndex=relevantFrequencyBandUpperBoundIndex-1;
         System.out.println("WARNING: SoundDetector dominant band range may need to be extended higher for the "+dominantFrequencyBandUpperBoundIndex+" Hz peak detection.");
      }

      for (int index = dominantFrequencyBandUpperBoundIndex; index < relevantFrequencyBandUpperBoundIndex; index++)
      {
         relevantBandAverageMag += magnitude[index];
      }

      relevantBandAverageMag = relevantBandAverageMag
            / ((relevantFrequencyBandUpperBoundIndex - dominantFrequencyBandUpperBoundIndex) + (dominantFrequencyBandLowerBoundIndex - relevantFrequencyBandLowerBoundIndex));

      if ((dominantBandPeakMag - relevantBandAverageMag) > decibelsToTrip)
      {
         peakDetected = true;
      }
      return peakDetected;

      }
   }

   public void stop()
   {
      thread = null;
   }

}

<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/FunctionTrajectoryTools.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import us.ihmc.commons.MathTools;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
import us.ihmc.humanoidRobotics.communication.packets.manipulation.wholeBodyTrajectory.WholeBodyTrajectoryToolboxMessageTools.FunctionTrajectory;

public class FunctionTrajectoryTools
{
   /**
    * Returns an implementation of {@code FunctionTrajectory} that implements a circle trajectory.
    * <p>
    * Together {@code circleCenter} and {@code circleOrientation} describe the frame in which the
    * circle is drawn. In this local frame, the circle is drawn on the XY-plane.
    * </p>
    * 
    * @param circleCenter the center of the circle. Not modified.
    * @param circleOrientation the orientation of the circle. Not modified.
    * @param outputOrientation the constant orientation the this trajectory should output. Not
    *           modified.
    * @param radius the circle's radius.
    * @param angleStart the angle from the local x-axis that the circle should start at.
    * @param clockwise whether the circle is to be drawn in a clockwise or counter-clockwise manner.
    * @param t0 the start time of the trajectory.
    * @param tf the end time of the trajectory.
    * @return the function trajectory of a circle.
    */
   public static FunctionTrajectory circleTrajectory(Point3DReadOnly circleCenter, QuaternionReadOnly circleOrientation, QuaternionReadOnly outputOrientation,
                                                     double radius, double angleStart, boolean clockwise, double t0, double tf)
   {
      return new FunctionTrajectory()
      {
         @Override
         public Pose3D compute(double time)
         {
            RigidBodyTransform circlePose = new RigidBodyTransform(circleOrientation, circleCenter);

            time = MathTools.clamp(time, t0, tf);
            double angle = angleStart + 2.0 * Math.PI * (time - t0) / (tf - t0);

            if (clockwise)
               angle = -angle;

            double xLocal = radius * Math.cos(angle);
            double yLocal = radius * Math.sin(angle);

            Point3D point = new Point3D(xLocal, yLocal, 0.0);
            circlePose.transform(point);

            return new Pose3D(point, outputOrientation);
         }
      };
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/IntegerSlider.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

public class IntegerSlider extends NumericSlider<Integer>
{
   @Override
   public double toDouble(Integer number)
   {
      return number;
   }

   @Override
   public Integer toNumber(double doubleValue)
   {
      return (int) Math.round(doubleValue);
   }

   @Override
   public int getPrecision()
   {
      return 0;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/lowLevel/IntermediateDesiredJointDataHolder.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel;

import us.ihmc.sensorProcessing.outputData.JointDesiredOutput;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputList;

public class IntermediateDesiredJointDataHolder
{
   private final JointDesiredOutputList estimatorDataHolder;
   private final JointDesiredOutputList controllerDataHolder;

   private final JointDesiredOutput[] intermediateDesiredJointData;

   public IntermediateDesiredJointDataHolder(JointDesiredOutputList estimatorDataHolder, JointDesiredOutputList controllerDataHolder)
   {
      this.estimatorDataHolder = estimatorDataHolder;
      this.controllerDataHolder = controllerDataHolder;

      for(int i = 0; i < estimatorDataHolder.getNumberOfJointsWithDesiredOutput(); i++)
      {
         if(estimatorDataHolder.getOneDoFJoint(i).getName() != controllerDataHolder.getOneDoFJoint(i).getName())
         {
            throw new RuntimeException("Models do not match");
         }
      }
      intermediateDesiredJointData = new JointDesiredOutput[estimatorDataHolder.getNumberOfJointsWithDesiredOutput()];
      for(int i = 0; i < estimatorDataHolder.getNumberOfJointsWithDesiredOutput(); i++)
      {
         intermediateDesiredJointData[i] = new JointDesiredOutput();
      }
   }

   public void copyFromController()
   {
      for(int i = 0; i < controllerDataHolder.getNumberOfJointsWithDesiredOutput(); i++)
      {
         intermediateDesiredJointData[i].set(controllerDataHolder.getJointDesiredOutput(i));
      }

   }

   public void readIntoEstimator()
   {
      for(int i = 0; i < estimatorDataHolder.getNumberOfJointsWithDesiredOutput(); i++)
      {
         estimatorDataHolder.getJointDesiredOutput(i).set(intermediateDesiredJointData[i]);
      }
   }
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullQuadrupedRobotModelFactory.java<|end_filename|>
package us.ihmc.robotModels;

import us.ihmc.robotics.robotSide.RobotQuadrant;

public interface FullQuadrupedRobotModelFactory extends FullLeggedRobotModelFactory<RobotQuadrant>
{
   @Override
   FullQuadrupedRobotModel createFullRobotModel();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ArmDesiredAccelerationsMessage.java<|end_filename|>
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 gives the user the option to bypass IHMC feedback controllers for the arm joints by sending desired arm joint accelerations.
       * One needs experience in control when activating the bypass as it can result in unexpected behaviors for unreasonable accelerations.
       */
public class ArmDesiredAccelerationsMessage extends Packet<ArmDesiredAccelerationsMessage> implements Settable<ArmDesiredAccelerationsMessage>, EpsilonComparable<ArmDesiredAccelerationsMessage>
{
   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 command.
            */
   public byte robot_side_ = (byte) 255;
   /**
            * The desired joint acceleration information.
            */
   public controller_msgs.msg.dds.DesiredAccelerationsMessage desired_accelerations_;

   public ArmDesiredAccelerationsMessage()
   {
      desired_accelerations_ = new controller_msgs.msg.dds.DesiredAccelerationsMessage();
   }

   public ArmDesiredAccelerationsMessage(ArmDesiredAccelerationsMessage other)
   {
      this();
      set(other);
   }

   public void set(ArmDesiredAccelerationsMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      controller_msgs.msg.dds.DesiredAccelerationsMessagePubSubType.staticCopy(other.desired_accelerations_, desired_accelerations_);
   }

   /**
            * 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_;
   }

   /**
            * Specifies the side of the robot that will execute the command.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * Specifies the side of the robot that will execute the command.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }


   /**
            * The desired joint acceleration information.
            */
   public controller_msgs.msg.dds.DesiredAccelerationsMessage getDesiredAccelerations()
   {
      return desired_accelerations_;
   }


   public static Supplier<ArmDesiredAccelerationsMessagePubSubType> getPubSubType()
   {
      return ArmDesiredAccelerationsMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ArmDesiredAccelerationsMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ArmDesiredAccelerationsMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!this.desired_accelerations_.epsilonEquals(other.desired_accelerations_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ArmDesiredAccelerationsMessage)) return false;

      ArmDesiredAccelerationsMessage otherMyClass = (ArmDesiredAccelerationsMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if (!this.desired_accelerations_.equals(otherMyClass.desired_accelerations_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ArmDesiredAccelerationsMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("desired_accelerations=");
      builder.append(this.desired_accelerations_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/JointspaceTrajectoryMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "JointspaceTrajectoryMessage" defined in "JointspaceTrajectoryMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from JointspaceTrajectoryMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit JointspaceTrajectoryMessage_.idl instead.
*
*/
public class JointspaceTrajectoryMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.JointspaceTrajectoryMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::JointspaceTrajectoryMessage_";

   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.JointspaceTrajectoryMessage 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.JointspaceTrajectoryMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0)
      {
          current_alignment += controller_msgs.msg.dds.OneDoFJointTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += controller_msgs.msg.dds.QueueableMessagePubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.JointspaceTrajectoryMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.JointspaceTrajectoryMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getJointTrajectoryMessages().size(); ++i0)
      {
          current_alignment += controller_msgs.msg.dds.OneDoFJointTrajectoryMessagePubSubType.getCdrSerializedSize(data.getJointTrajectoryMessages().get(i0), current_alignment);}

      current_alignment += controller_msgs.msg.dds.QueueableMessagePubSubType.getCdrSerializedSize(data.getQueueingProperties(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.JointspaceTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getJointTrajectoryMessages().size() <= 100)
      cdr.write_type_e(data.getJointTrajectoryMessages());else
          throw new RuntimeException("joint_trajectory_messages field exceeds the maximum length");

      controller_msgs.msg.dds.QueueableMessagePubSubType.write(data.getQueueingProperties(), cdr);
   }

   public static void read(controller_msgs.msg.dds.JointspaceTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_e(data.getJointTrajectoryMessages());	
      controller_msgs.msg.dds.QueueableMessagePubSubType.read(data.getQueueingProperties(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.JointspaceTrajectoryMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_e("joint_trajectory_messages", data.getJointTrajectoryMessages());
      ser.write_type_a("queueing_properties", new controller_msgs.msg.dds.QueueableMessagePubSubType(), data.getQueueingProperties());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.JointspaceTrajectoryMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_e("joint_trajectory_messages", data.getJointTrajectoryMessages());
      ser.read_type_a("queueing_properties", new controller_msgs.msg.dds.QueueableMessagePubSubType(), data.getQueueingProperties());

   }

   public static void staticCopy(controller_msgs.msg.dds.JointspaceTrajectoryMessage src, controller_msgs.msg.dds.JointspaceTrajectoryMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage createData()
   {
      return new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
   }
   @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.JointspaceTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.JointspaceTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.JointspaceTrajectoryMessage src, controller_msgs.msg.dds.JointspaceTrajectoryMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public JointspaceTrajectoryMessagePubSubType newInstance()
   {
      return new JointspaceTrajectoryMessagePubSubType();
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/concurrent/IntermediateControllerStateHolder.java<|end_filename|>
package us.ihmc.wholeBodyController.concurrent;

import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.robotModels.FullRobotModelFactory;
import us.ihmc.robotics.screwTheory.InverseDynamicsJointDesiredAccelerationCopier;

public class IntermediateControllerStateHolder
{
   private final InverseDynamicsJointDesiredAccelerationCopier controllerToIntermediateCopier;
   private final InverseDynamicsJointDesiredAccelerationCopier intermediateToEstimatorCopier;


   public IntermediateControllerStateHolder(FullRobotModelFactory robotModelFactory, FullRobotModel estimatorModel,
         FullRobotModel controllerModel)
   {
      FullRobotModel intermediateModel = robotModelFactory.createFullRobotModel();

      controllerToIntermediateCopier = new InverseDynamicsJointDesiredAccelerationCopier(controllerModel.getElevator(), intermediateModel.getElevator());
      intermediateToEstimatorCopier = new InverseDynamicsJointDesiredAccelerationCopier(intermediateModel.getElevator(), estimatorModel.getElevator());

   }

   public void setFromController()
   {
      controllerToIntermediateCopier.copy();
   }

   public void getIntoEstimator()
   {
      intermediateToEstimatorCopier.copy();
   }

   public static class Builder implements us.ihmc.concurrent.Builder<IntermediateControllerStateHolder>
   {

      private final FullRobotModel estimatorModel;
      private final FullRobotModel controllerModel;
      private final FullRobotModelFactory robotModelFactory;

      public Builder(FullRobotModelFactory robotModelFactory, FullRobotModel estimatorModel, FullRobotModel controllerModel)
      {
         this.robotModelFactory = robotModelFactory;
         this.estimatorModel = estimatorModel;
         this.controllerModel = controllerModel;
      }

      @Override
      public IntermediateControllerStateHolder newInstance()
      {
         return new IntermediateControllerStateHolder(robotModelFactory, estimatorModel, controllerModel);
      }

   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/momentumBasedController/GeometricJacobianHolder.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.momentumBasedController;

import java.util.ArrayList;
import java.util.List;

import gnu.trove.map.hash.TIntObjectHashMap;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.screwTheory.GeometricJacobian;
import us.ihmc.robotics.screwTheory.ScrewTools;

/*
 * Change the strategy to be less expensive in terms of computation.
 * The method compute() should be replaced a method reset() which should flip a boolean somehow linked to each jacobian indicating that they need to be updated.
 * When a getter is called, call GeometricJacobian.compute() only if needed.
 * That should allow the user to create as many jacobian as needed and update only the ones that are actually being used.
 */
public class GeometricJacobianHolder
{
   public static final int NULL_JACOBIAN_ID = 0;

   private final TIntObjectHashMap<GeometricJacobian> hashCodeToJacobianMap = new TIntObjectHashMap<GeometricJacobian>();
   private final List<GeometricJacobian> geometricJacobians = new ArrayList<GeometricJacobian>();
   private final JointBasics[] temporaryToStoreJointPath = new JointBasics[30];

   public void compute()
   {
      for (int i = 0; i < geometricJacobians.size(); i++)
      {
         geometricJacobians.get(i).compute();
      }
   }

   /**
    * Find or create a Jacobian and register it in the {@link HighLevelHumanoidControllerToolbox}.
    * It returns an jacobianId with which it is possible to find the Jacobian later with the method getJacobian(int jacobianId).
    * @param ancestor
    * @param descendant
    * @param jacobianFrame
    * @return
    */
   public int getOrCreateGeometricJacobian(RigidBodyBasics ancestor, RigidBodyBasics descendant, ReferenceFrame jacobianFrame)
   {
      int numberOfJoints = ScrewTools.createJointPath(temporaryToStoreJointPath, ancestor, descendant);
      return getOrCreateGeometricJacobian(temporaryToStoreJointPath, numberOfJoints, jacobianFrame);
   }

   /**
    * Find or create a Jacobian and register it in the {@link HighLevelHumanoidControllerToolbox}.
    * It returns an jacobianId with which it is possible to find the Jacobian later with the method getJacobian(int jacobianId).
    * If the array of joints is empty, it returns -1.
    * @param joints
    * @param jacobianFrame
    * @return
    */
   public int getOrCreateGeometricJacobian(JointBasics[] joints, ReferenceFrame jacobianFrame)
   {
      return getOrCreateGeometricJacobian(joints, joints.length, jacobianFrame);
   }

   private int getOrCreateGeometricJacobian(JointBasics[] joints, int numberOfJointsToConsider, ReferenceFrame jacobianFrame)
   {
      if (joints == null || numberOfJointsToConsider == 0)
         return NULL_JACOBIAN_ID;
      
      // The mapping assumes the frame do not change.
      // On top of that, this class makes the different modules use the same instances of each Jacobian, so it would not be good if one module changes the frame of a Jacobian shared with another module. 
      boolean allowChangeFrame = false;

      int jacobianId = ScrewTools.computeGeometricJacobianHashCode(joints, 0, numberOfJointsToConsider - 1, jacobianFrame, allowChangeFrame);
      GeometricJacobian jacobian = getJacobian(jacobianId);

      if (jacobian == null)
      {
         if (joints.length == numberOfJointsToConsider)
         {
            jacobian = new GeometricJacobian(joints, jacobianFrame, allowChangeFrame);
         }
         else
         {
            JointBasics[] jointsForNewJacobian = new JointBasics[numberOfJointsToConsider];
            System.arraycopy(joints, 0, jointsForNewJacobian, 0, numberOfJointsToConsider);
            jacobian = new GeometricJacobian(jointsForNewJacobian, jacobianFrame, allowChangeFrame);
         }
         jacobian.compute(); // Compute in case you need it right away
         geometricJacobians.add(jacobian);
         hashCodeToJacobianMap.put(jacobian.hashCode(), jacobian);
      }

      return jacobian.hashCode();
   }

   /**
    * Return a jacobian previously created with the getOrCreate method using a jacobianId.
    * @param jacobianId
    * @return
    */
   public GeometricJacobian getJacobian(int jacobianId)
   {
      if (jacobianId == NULL_JACOBIAN_ID)
         return null;
      else
         return hashCodeToJacobianMap.get(jacobianId);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/PointToLineUnProjector.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;

public class PointToLineUnProjector
{
   Point2D pointA = new Point2D();
   Vector2D difference = new Vector2D();
   double zA;
   double zDifference;
   boolean useX = true;
   double qA;
   double qMult;

   public void setLine(Point2D point0, Point2D point1, double point0z, double point1z)
   {
      pointA.set(point0);
      difference.sub(point1, point0);
      zA = point0z;
      zDifference = point1z - point0z;
      useX = Math.abs(difference.getX()) > Math.abs(difference.getY());

      if (useX)
      {
         qA = pointA.getX();
         qMult = 1 / difference.getX();
      }
      else
      {
         qA = pointA.getY();
         qMult = 1 / difference.getY();
      }

      if (!Double.isFinite(qMult))
         qMult = 0;

   }

   public double unProject(double x, double y)
   {
      double s = ((useX ? x : y) - qA) * qMult;
      return zA + zDifference * s;
   }
}


<|start_filename|>robot-environment-awareness/src/visualizers/java/us/ihmc/octoMap/NormalOcTreeVisualizer.java<|end_filename|>
package us.ihmc.octoMap;

import javafx.application.Application;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.MeshView;
import javafx.scene.shape.Sphere;
import javafx.stage.Stage;
import us.ihmc.commons.Conversions;
import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.rotationConversion.RotationMatrixConversion;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Point3D32;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.jOctoMap.iterators.OcTreeIterable;
import us.ihmc.jOctoMap.iterators.OcTreeIteratorFactory;
import us.ihmc.jOctoMap.node.NormalOcTreeNode;
import us.ihmc.jOctoMap.ocTree.NormalOcTree;
import us.ihmc.jOctoMap.pointCloud.PointCloud;
import us.ihmc.jOctoMap.pointCloud.ScanCollection;
import us.ihmc.javaFXToolkit.scenes.View3DFactory;
import us.ihmc.javaFXToolkit.shapes.JavaFXMeshBuilder;
import us.ihmc.javaFXToolkit.shapes.JavaFXMultiColorMeshBuilder;
import us.ihmc.javaFXToolkit.shapes.TextureColorPalette1D;
import us.ihmc.robotEnvironmentAwareness.geometry.IntersectionPlaneBoxCalculator;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.commons.lists.SupplierBuilder;

public class NormalOcTreeVisualizer extends Application
{
   public final NormalOcTree ocTree = new NormalOcTree(0.025);
   private static final boolean SHOW_FREE_CELLS = false;
   private static final Color FREE_COLOR = new Color(Color.YELLOW.getRed(), Color.YELLOW.getGreen(), Color.YELLOW.getBlue(), 0.0);
   private static final boolean SHOW_POINT_CLOUD = true;
   private static final boolean SHOW_HIT_LOCATIONS = true;

   public NormalOcTreeVisualizer()
   {

      Point3D lidarPosition = new Point3D(0.0, 0.0, 2.0);
      //      callUpdateNode();
      //      callInsertPointCloud();
      createPlane(lidarPosition, 12.0, 0.0, -0.05);
//      createBowl(0.5, new Point3D());
      System.out.println("Number of leafs: " + ocTree.getNumberOfLeafNodes());
      System.out.println("Initialized octree");
      System.out.println("Computing normals");
      long startTime = System.nanoTime();
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
      long endTime = System.nanoTime();
      System.out.println("Done computing normals: time it took = " + Conversions.nanosecondsToSeconds(endTime - startTime));
   }
   
   PointCloud pointcloud = new PointCloud();

   private void callInsertPointCloud()
   {
      Point3D origin = new Point3D(0.01, 0.01, 0.02);
      Point3D pointOnSurface = new Point3D(4.01, 0.01, 0.01);

      for (int i = 0; i < 100; i++)
      {
         for (int j = 0; j < 100; j++)
         {
            Point3D rotated = new Point3D(pointOnSurface);
            RotationMatrix rotation = new RotationMatrix();
            RotationMatrixConversion.convertYawPitchRollToMatrix(Math.toRadians(i * 0.5), Math.toRadians(j * 0.5), 0.0, rotation);
            rotation.transform(rotated);
            pointcloud.add(rotated);
         }
      }

      ocTree.update(new ScanCollection(pointcloud, origin));
   }

   public void createPlane(Point3D lidarPosition, double pitch, double roll, double z)
   {
      pointcloud.clear();

      double planeSize = 1.00;

      for (double x = -0.5 * planeSize; x < 0.5 * planeSize; x += 0.7 * ocTree.getResolution())
      {
         for (double y = -0.5 * planeSize; y < 0.5 * planeSize; y += 0.7 * ocTree.getResolution())
         {
            Point3D point = new Point3D(x, y, 0.0);
            RotationMatrix rotation = new RotationMatrix();
            RotationMatrixConversion.convertYawPitchRollToMatrix(0.0, Math.toRadians(pitch), Math.toRadians(roll), rotation);
            rotation.transform(point);
            point.setZ(point.getZ() + z);

            pointcloud.add(point);
         }
      }
      ocTree.update(new ScanCollection(pointcloud, lidarPosition));
   }

   public void createBowl(double radius, Point3D center)
   {
      Point3D origin = new Point3D(0.0, 0.0, center.getZ() + 0.0);

      double res = 0.05;
      for (double yaw = 0.0; yaw < 2.0 * Math.PI; yaw += res)
      {
         for (double pitch = 0.0; pitch < 0.5 * Math.PI; pitch += res)
         {
            double x = Math.cos(pitch) * Math.cos(yaw) * radius + center.getX();
            double y = Math.cos(pitch) * Math.sin(yaw) * radius + center.getY();
            double z = - Math.sin(pitch) * radius + center.getZ();
            pointcloud.add(x, y, z);
         }
      }

      ocTree.update(new ScanCollection(pointcloud, origin));
   }

   private final RecyclingArrayList<Point3D> plane = new RecyclingArrayList<>(0, Point3D.class);
   private final IntersectionPlaneBoxCalculator intersectionPlaneBoxCalculator = new IntersectionPlaneBoxCalculator();

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      primaryStage.setTitle("OcTree Visualizer");

      View3DFactory view3dFactory = new View3DFactory(600, 400);
      view3dFactory.addCameraController();
      view3dFactory.addWorldCoordinateSystem(0.3);

      TextureColorPalette1D palette = new TextureColorPalette1D();
      palette.setHueBased(0.9, 0.8);
      JavaFXMultiColorMeshBuilder occupiedMeshBuilder = new JavaFXMultiColorMeshBuilder(palette);
      JavaFXMeshBuilder freeMeshBuilder = new JavaFXMeshBuilder();

      OcTreeIterable<NormalOcTreeNode> leafIterable = OcTreeIteratorFactory.createLeafIterable(ocTree.getRoot(), 14);
      for (NormalOcTreeNode node : leafIterable)
      {
         double boxSize = node.getSize();
         Point3D nodeCenter = new Point3D();
         node.getCoordinate(nodeCenter);

         if (ocTree.isNodeOccupied(node))
         {
            Vector3D planeNormal = new Vector3D();
            node.getNormal(planeNormal);
            boolean isNormalSet = node.isNormalSet();
            Color normalBasedColor = getNormalBasedColor(planeNormal, isNormalSet);
            if (isNormalSet)
            {
               Point3D pointOnPlane = new Point3D();
               if (node.isHitLocationSet())
                  node.getHitLocation(pointOnPlane);
               else
                  pointOnPlane.set(nodeCenter);
               intersectionPlaneBoxCalculator.setCube(boxSize, nodeCenter);
               intersectionPlaneBoxCalculator.setPlane(pointOnPlane, planeNormal);
               intersectionPlaneBoxCalculator.computeIntersections(plane);
               occupiedMeshBuilder.addPolyon(plane, normalBasedColor);
               if (SHOW_HIT_LOCATIONS)
                  occupiedMeshBuilder.addCube(0.01, pointOnPlane, DEFAULT_COLOR);
               
               

               for (int j = 0; j < plane.size(); j++)
               {
                  Point3D intersection = plane.get(j);

                  Vector3D sub = new Vector3D();
                  sub.sub(intersection, pointOnPlane);

                  Vector3D v0 = new Vector3D();
                  Vector3D v1 = new Vector3D();
                  Vector3D v3 = new Vector3D();
                  Point3D nextIntersection = plane.get((j + 1) % plane.size());
                  Point3D previousIntersection = plane.get(j == 0 ? plane.size() - 1 : j - 1);
                  v0.sub(intersection, nextIntersection);
                  v1.sub(intersection, previousIntersection);
                  v3.cross(v0, v1);
                  if (v3.dot(planeNormal) < 0.0 || Math.abs(sub.dot(planeNormal)) > 0.01)
                  {
                     System.err.println("node size: " + boxSize);
                     System.err.println("      Point3D cubeCenter = new Point3D" + nodeCenter + ";");
                     System.err.println("      Point3D pointOnPlane = new Point3D" + pointOnPlane + ";");
                     System.err.println("      Vector3d planeNormal = new Vector3d" + planeNormal + ";");
                     System.out.println();
                  }
               }
               
               
               
               
            }
            else
               occupiedMeshBuilder.addCube((float) boxSize, new Point3D32(nodeCenter), normalBasedColor);
         }
         else if (SHOW_FREE_CELLS)
         {
            freeMeshBuilder.addCube((float) boxSize, new Point3D32(nodeCenter));
         }
      }

      MeshView occupiedMeshView = new MeshView();
      occupiedMeshView.setMesh(occupiedMeshBuilder.generateMesh());
      occupiedMeshView.setMaterial(occupiedMeshBuilder.generateMaterial());
      occupiedMeshView.setMouseTransparent(true);
      view3dFactory.addNodeToView(occupiedMeshView);

      if (SHOW_FREE_CELLS)
      {
         MeshView freeMeshView = new MeshView();
         freeMeshView.setMesh(freeMeshBuilder.generateMesh());
         PhongMaterial material = new PhongMaterial();
         material.setDiffuseColor(FREE_COLOR);
         freeMeshView.setMaterial(material);
         view3dFactory.addNodeToView(freeMeshView);
      }

      if (SHOW_POINT_CLOUD)
      {
         for (int i = 0; i < pointcloud.getNumberOfPoints(); i++)
         {
            Sphere sphere = new Sphere(0.0025);
            sphere.setTranslateX(pointcloud.getPoint(i).getX());
            sphere.setTranslateY(pointcloud.getPoint(i).getY());
            sphere.setTranslateZ(pointcloud.getPoint(i).getZ());
            view3dFactory.addNodeToView(sphere);
         }
      }

      primaryStage.setScene(view3dFactory.getScene());
      primaryStage.show();
   }

   private static final Color DEFAULT_COLOR = Color.DARKCYAN;

   public Color getNormalBasedColor(Vector3D normal, boolean isNormalSet)
   {
      Color color = DEFAULT_COLOR;

      if (isNormalSet)
      {
         Vector3D zUp = new Vector3D(0.0, 0.0, 1.0);
         normal.normalize();
         double angle = Math.abs(zUp.dot(normal));
         double hue = 120.0 * angle;
         color = Color.hsb(hue, 1.0, 1.0);
      }
      return color;
   }

   public static void main(String[] args)
   {

      //      new OcTreeVisualizer();

      Application.launch(args);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/perturbance/ForcePerturbable.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.perturbance;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;


public interface ForcePerturbable
{
   public abstract void setForcePerturbance(Vector3DReadOnly force, double duration);
   public abstract void resetPerturbanceForceIfNecessary(); // need to do it this way because of thread issues.
   public abstract Point3D getForcePerturbanceApplicationPoint();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/boards/SliderboardType.java<|end_filename|>
package us.ihmc.robotics.sliderboard.boards;

import javax.sound.midi.MidiDevice;
import javax.sound.midi.MidiSystem;

import us.ihmc.commons.PrintTools;
import us.ihmc.robotics.sliderboard.MidiControlMap;

public enum SliderboardType
{
   BCF2000;

   public String getStringIdentifier()
   {
      switch (this)
      {
      case BCF2000:
         return "BCF2000";
      default:
         throw new RuntimeException("Unknown string identifier for sliderboard " + this + ".");
      }
   }

   public MidiControlMap getChannelMapper()
   {
      switch (this)
      {
      case BCF2000:
         return new BCF2000ControlMap();
      default:
         throw new RuntimeException("Unknown channel mapping for sliderboard " + this + ".");
      }
   }

   public static SliderboardType findConnectedSliderBoard()
   {
      for (MidiDevice.Info info : MidiSystem.getMidiDeviceInfo())
      {
         String name = info.getName();
         String description = info.getDescription();

         for (SliderboardType sliderboardType : SliderboardType.values())
         {
            String identifier = sliderboardType.getStringIdentifier();

            if (name.contains(identifier) || description.contains(identifier))
            {
               return sliderboardType;
            }
         }
      }

      PrintTools.info("Was not able to find a supported sliderboard.");
      return null;
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/PelvisOrientationTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.PelvisOrientationTrajectoryMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.PelvisOrientationTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class PelvisOrientationTrajectoryTask extends BehaviorAction
{
   private final PelvisOrientationTrajectoryMessage pelvisOrientationTrajectoryMessage;
   private final PelvisOrientationTrajectoryBehavior pelvisOrientationTrajectoryBehavior;

   public PelvisOrientationTrajectoryTask(PelvisOrientationTrajectoryMessage pelvisOrientationTrajectoryMessage,
                                          PelvisOrientationTrajectoryBehavior pelvisOrientationTrajectoryBehavior)
   {
      super(pelvisOrientationTrajectoryBehavior);
      this.pelvisOrientationTrajectoryBehavior = pelvisOrientationTrajectoryBehavior;
      this.pelvisOrientationTrajectoryMessage = pelvisOrientationTrajectoryMessage;
   }

   @Override
   protected void setBehaviorInput()
   {
      pelvisOrientationTrajectoryBehavior.setInput(pelvisOrientationTrajectoryMessage);
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/footstepPlanning/ConcurrentMap.java<|end_filename|>
package us.ihmc.avatar.footstepPlanning;

import us.ihmc.concurrent.ConcurrentCopier;

import java.util.HashMap;
import java.util.Map;

class ConcurrentMap<K, V> extends ConcurrentCopier<HashMap<K, V>>
{
   public ConcurrentMap()
   {
      super(HashMap::new);
   }

   public boolean isEmpty()
   {
      Map<K, V> existingMap = getCopyForReading();

      if (existingMap != null)
         return getCopyForReading().isEmpty();
      else
         return true;
   }

   public Iterable<K> iterator()
   {
      Map<K, V> existingMap = getCopyForReading();
      if (existingMap != null)
         return existingMap.keySet();
      else
         return null;
   }

   public void clear()
   {
      Map<K, V> updatedMap = getCopyForWriting();
      updatedMap.clear();
      commit();
   }

   public void put(K key, V value)
   {
      Map<K, V> existingMap = getCopyForReading();
      Map<K, V> updatedMap = getCopyForWriting();
      updatedMap.clear();
      if (existingMap != null)
         updatedMap.putAll(existingMap);
      updatedMap.put(key, value);

      commit();
   }

   public V remove(K key)
   {
      Map<K, V> existingMap = getCopyForReading();
      Map<K, V> updatedMap = getCopyForWriting();
      updatedMap.clear();
      if (existingMap != null)
         updatedMap.putAll(existingMap);
      V objectToReturn = updatedMap.remove(key);

      commit();

      return objectToReturn;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LidarScanParametersMessage.java<|end_filename|>
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;

/**
       * Message used for simulated LIDAR
       */
public class LidarScanParametersMessage extends Packet<LidarScanParametersMessage> implements Settable<LidarScanParametersMessage>, EpsilonComparable<LidarScanParametersMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public long timestamp_;
   public float sweep_yaw_max_;
   public float sweep_yaw_min_;
   public float height_pitch_max_;
   public float height_pitch_min_;
   public float time_increment_;
   public float scan_time_;
   public float min_range_;
   public float max_range_;
   public int points_per_sweep_;
   public int scan_height_;

   public LidarScanParametersMessage()
   {
   }

   public LidarScanParametersMessage(LidarScanParametersMessage other)
   {
      this();
      set(other);
   }

   public void set(LidarScanParametersMessage other)
   {
      sequence_id_ = other.sequence_id_;

      timestamp_ = other.timestamp_;

      sweep_yaw_max_ = other.sweep_yaw_max_;

      sweep_yaw_min_ = other.sweep_yaw_min_;

      height_pitch_max_ = other.height_pitch_max_;

      height_pitch_min_ = other.height_pitch_min_;

      time_increment_ = other.time_increment_;

      scan_time_ = other.scan_time_;

      min_range_ = other.min_range_;

      max_range_ = other.max_range_;

      points_per_sweep_ = other.points_per_sweep_;

      scan_height_ = other.scan_height_;

   }

   /**
            * 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 void setTimestamp(long timestamp)
   {
      timestamp_ = timestamp;
   }
   public long getTimestamp()
   {
      return timestamp_;
   }

   public void setSweepYawMax(float sweep_yaw_max)
   {
      sweep_yaw_max_ = sweep_yaw_max;
   }
   public float getSweepYawMax()
   {
      return sweep_yaw_max_;
   }

   public void setSweepYawMin(float sweep_yaw_min)
   {
      sweep_yaw_min_ = sweep_yaw_min;
   }
   public float getSweepYawMin()
   {
      return sweep_yaw_min_;
   }

   public void setHeightPitchMax(float height_pitch_max)
   {
      height_pitch_max_ = height_pitch_max;
   }
   public float getHeightPitchMax()
   {
      return height_pitch_max_;
   }

   public void setHeightPitchMin(float height_pitch_min)
   {
      height_pitch_min_ = height_pitch_min;
   }
   public float getHeightPitchMin()
   {
      return height_pitch_min_;
   }

   public void setTimeIncrement(float time_increment)
   {
      time_increment_ = time_increment;
   }
   public float getTimeIncrement()
   {
      return time_increment_;
   }

   public void setScanTime(float scan_time)
   {
      scan_time_ = scan_time;
   }
   public float getScanTime()
   {
      return scan_time_;
   }

   public void setMinRange(float min_range)
   {
      min_range_ = min_range;
   }
   public float getMinRange()
   {
      return min_range_;
   }

   public void setMaxRange(float max_range)
   {
      max_range_ = max_range;
   }
   public float getMaxRange()
   {
      return max_range_;
   }

   public void setPointsPerSweep(int points_per_sweep)
   {
      points_per_sweep_ = points_per_sweep;
   }
   public int getPointsPerSweep()
   {
      return points_per_sweep_;
   }

   public void setScanHeight(int scan_height)
   {
      scan_height_ = scan_height;
   }
   public int getScanHeight()
   {
      return scan_height_;
   }


   public static Supplier<LidarScanParametersMessagePubSubType> getPubSubType()
   {
      return LidarScanParametersMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return LidarScanParametersMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(LidarScanParametersMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.timestamp_, other.timestamp_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sweep_yaw_max_, other.sweep_yaw_max_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sweep_yaw_min_, other.sweep_yaw_min_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.height_pitch_max_, other.height_pitch_max_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.height_pitch_min_, other.height_pitch_min_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_increment_, other.time_increment_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.scan_time_, other.scan_time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.min_range_, other.min_range_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.max_range_, other.max_range_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.points_per_sweep_, other.points_per_sweep_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.scan_height_, other.scan_height_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof LidarScanParametersMessage)) return false;

      LidarScanParametersMessage otherMyClass = (LidarScanParametersMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.timestamp_ != otherMyClass.timestamp_) return false;

      if(this.sweep_yaw_max_ != otherMyClass.sweep_yaw_max_) return false;

      if(this.sweep_yaw_min_ != otherMyClass.sweep_yaw_min_) return false;

      if(this.height_pitch_max_ != otherMyClass.height_pitch_max_) return false;

      if(this.height_pitch_min_ != otherMyClass.height_pitch_min_) return false;

      if(this.time_increment_ != otherMyClass.time_increment_) return false;

      if(this.scan_time_ != otherMyClass.scan_time_) return false;

      if(this.min_range_ != otherMyClass.min_range_) return false;

      if(this.max_range_ != otherMyClass.max_range_) return false;

      if(this.points_per_sweep_ != otherMyClass.points_per_sweep_) return false;

      if(this.scan_height_ != otherMyClass.scan_height_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("LidarScanParametersMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("timestamp=");
      builder.append(this.timestamp_);      builder.append(", ");
      builder.append("sweep_yaw_max=");
      builder.append(this.sweep_yaw_max_);      builder.append(", ");
      builder.append("sweep_yaw_min=");
      builder.append(this.sweep_yaw_min_);      builder.append(", ");
      builder.append("height_pitch_max=");
      builder.append(this.height_pitch_max_);      builder.append(", ");
      builder.append("height_pitch_min=");
      builder.append(this.height_pitch_min_);      builder.append(", ");
      builder.append("time_increment=");
      builder.append(this.time_increment_);      builder.append(", ");
      builder.append("scan_time=");
      builder.append(this.scan_time_);      builder.append(", ");
      builder.append("min_range=");
      builder.append(this.min_range_);      builder.append(", ");
      builder.append("max_range=");
      builder.append(this.max_range_);      builder.append(", ");
      builder.append("points_per_sweep=");
      builder.append(this.points_per_sweep_);      builder.append(", ");
      builder.append("scan_height=");
      builder.append(this.scan_height_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/depthData/DepthDataFilter.java<|end_filename|>
package us.ihmc.ihmcPerception.depthData;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.humanoidRobotics.communication.packets.sensing.DepthDataFilterParameters;
import us.ihmc.robotics.dataStructures.DecayingResolutionFilter;
import us.ihmc.robotics.quadTree.Box;
import us.ihmc.robotics.quadTree.QuadTreeForGroundParameters;
import us.ihmc.sensorProcessing.pointClouds.combinationQuadTreeOctTree.QuadTreeForGroundHeightMap;
import us.ihmc.sensorProcessing.pointClouds.combinationQuadTreeOctTree.QuadTreeHeightMapInterface;

public class DepthDataFilter
{

   public static final double QUAD_TREE_EXTENT = 200;
   protected final QuadTreeHeightMapInterface quadTree;
   protected final DecayingResolutionFilter nearScan;
   protected DepthDataFilterParameters parameters;

   // Adjustment which is applied to LIDAR points.  Must be applied while points are still in LIDAR frame.  Allows users correct for errors
   // See DRCManualLidarTransform and DRCLidarVisualizationManager. This is a bit of a hack but less likely to have unintended consequences.
   private final RigidBodyTransform worldToCorrected = new RigidBodyTransform();


   public DepthDataFilter()
   {
      this(DepthDataFilterParameters.getDefaultParameters());
   }
   
   public DepthDataFilter(DepthDataFilterParameters parameters)
   {
      this.parameters = parameters;
      nearScan = new DecayingResolutionFilter(parameters.nearScanResolution, parameters.nearScanDecayMillis, parameters.nearScanCapacity);
      quadTree = setupGroundOnlyQuadTree(parameters);

   }

   public static QuadTreeForGroundHeightMap setupGroundOnlyQuadTree(DepthDataFilterParameters parameters)
   {
      Box bounds = new Box(-QUAD_TREE_EXTENT, -QUAD_TREE_EXTENT, QUAD_TREE_EXTENT, QUAD_TREE_EXTENT);
      QuadTreeForGroundParameters quadTreeParameters = new QuadTreeForGroundParameters(DepthDataFilterParameters.GRID_RESOLUTION,
            parameters.quadtreeHeightThreshold, parameters.quadTreeMaxMultiLevelZChangeToFilterNoise, parameters.maxSameHeightPointsPerNode,
            parameters.maxAllowableXYDistanceForAPointToBeConsideredClose, parameters.maximumNumberOfPoints);

      return new QuadTreeForGroundHeightMap(bounds, quadTreeParameters);
   }


   public void setWorldToCorrected(RigidBodyTransform adjustment)
   {
      this.worldToCorrected.set(adjustment);
   }

   public boolean addNearScanPoint(Point3D point, Point3D sensorOrigin)
   {
      boolean send = false;
      if(!pointInRange(point, sensorOrigin))
         return false;

      // This is here so the user can manually correct for calibration errors.  It should only be not identity in the user interface
      if (DepthDataFilterParameters.LIDAR_ADJUSTMENT_ACTIVE)
         worldToCorrected.transform(point);

      if (parameters.nearScan && isValidNearScan(point, sensorOrigin))
      {
         send = nearScan.add(point.getX(), point.getY(), point.getZ()) || send;
      }

      return send;
   }
   public boolean addQuatreePoint(Point3D point, Point3D sensorOrigin)
   {
      boolean send = false;
      if(!pointInRange(point, sensorOrigin))
         return false;

      // This is here so the user can manually correct for calibration errors.  It should only be not identity in the user interface
      if (DepthDataFilterParameters.LIDAR_ADJUSTMENT_ACTIVE)
         worldToCorrected.transform(point);

      if (isValidPoint(point, sensorOrigin))
      {
         if (isPossibleGround(point, sensorOrigin))
         {
            send = quadTree.addPoint(point.getX(), point.getY(), point.getZ()) || send;
         }
      }

      return send;
   }

   public DecayingResolutionFilter getNearScan()
   {
      return nearScan;
   }

   public void setParameters(DepthDataFilterParameters parameters)
   {
      this.parameters = parameters;

      nearScan.setResolution(parameters.nearScanResolution);
      nearScan.setCapacity(parameters.nearScanCapacity);
      nearScan.setDecay(parameters.nearScanDecayMillis);

      quadTree.setHeightThreshold(parameters.quadtreeHeightThreshold);

   }

   public boolean addPoint(Point3D point,Point3D sensorOrigin)
   {
      return addNearScanPoint(point, sensorOrigin)| addQuatreePoint(point, sensorOrigin);
   }
   

   protected boolean isValidNearScan(Point3D point, Point3D lidarOrigin)
   {
      boolean valid = true;
      valid &= point.getZ() < lidarOrigin.getZ() + parameters.nearScanZMaxAboveHead;

      Point3D center = new Point3D(lidarOrigin.getX(), lidarOrigin.getY(), point.getZ());
      valid &= point.distance(center) < parameters.nearScanRadius;


      return valid;
   }

   protected boolean isValidPoint(Point3D point, Point3D lidarOrigin)
   {
      boolean valid = true;

      valid &= point.getZ() < lidarOrigin.getZ() + parameters.octreeZMaxAboveHead;;

      return valid;
   }

   protected boolean isPossibleGround(Point3D point, Point3D lidarOrigin)
   {
      final double footZ = 0;

      return (point.getZ() - footZ) < parameters.quadTreeZMax;
   }

   private boolean pointInRange(Point3D point, Point3D sensorOrigin)
   {
      double dist = sensorOrigin.distance(point);

      return (dist > parameters.minRange) && (dist < parameters.maxRange);
   }

}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/pointClouds/combinationQuadTreeOctTree/QuadTreeHeightMapInterface.java<|end_filename|>
package us.ihmc.sensorProcessing.pointClouds.combinationQuadTreeOctTree;

import java.util.Collection;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.dataStructures.HeightMapWithPoints;
import us.ihmc.robotics.hyperCubeTree.HyperCubeTreeListener;
import us.ihmc.robotics.quadTree.QuadTreeForGroundListener;

public interface QuadTreeHeightMapInterface extends HeightMapWithPoints
{

	/**
	 * 
	 * @param defaultGroundHeight, set to Double.NaN so it would take the value from first point.z
	 */
	public abstract void clearTree(double defaultGroundHeight); 
   public abstract double getDefaultHeightWhenNoPoints();


	public abstract void setHeightThreshold(double quadtreeHeightThreshold);

   public abstract void addListener(HyperCubeTreeListener<GroundAirDescriptor, GroundOnlyQuadTreeData> jmeGroundONlyQuadTreeVisualizer);
   public abstract void addQuadTreeListener(QuadTreeForGroundListener jmeGroundONlyQuadTreeVisualizer);

	public abstract boolean addToQuadtree(double x, double y, double z);
	
   public void getStoredPoints(Collection<Point3D> points);
   public void getCellAverageStoredPoints(Collection<Point3D> points);

   public abstract void lock();

   public abstract void unlock();
   public abstract boolean hasPoints();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/extra/EventState.java<|end_filename|>
package us.ihmc.robotics.stateMachine.extra;

import us.ihmc.robotics.stateMachine.core.State;
import us.ihmc.robotics.stateMachine.core.StateMachine;
import us.ihmc.robotics.stateMachine.factories.EventBasedStateMachineFactory;

/**
 * As a {@link State}, an {@link EventState} is used to create a {@link StateMachine}.
 * <p>
 * When used with a {@link EventBasedStateMachineFactory} to build a new state machine,
 * {@code EventState} provides an alternative to the usual state transitions. Instead of
 * implementing state transition conditions, the user registers a series of events which will
 * trigger specific transitions from a given source state to a given target state.
 * {@code EventState} adds to possibility to fire these events from within the active state.
 * </p>
 * 
 * @author Sylvain
 */
public interface EventState extends State
{
   /**
    * Invoked frequently by a state machine to collect events to be processed.
    * <p>
    * When an event is fired, the state machine will then look for a transition associated with it. If
    * there exists a transition with a target state, the state machine will then perform a state change
    * to that target state. Otherwise, this state will remains the active state.
    * </p>
    * 
    * @param timeInState the time spent in this state or {@link Double#NaN} if the time information is
    *           unavailable.
    * @return the event to be processed by the state machine, or {@code null} if no event is to be
    *         fired.
    */
   Enum<?> fireEvent(double timeInState);
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/FlatGroundEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class FlatGroundEnvironment implements CommonAvatarEnvironmentInterface
{
   private final CombinedTerrainObject3D flatGround;
   private final ArrayList<Robot> environmentRobots = new ArrayList<>();

   public FlatGroundEnvironment()
   {
      flatGround = DefaultCommonAvatarEnvironment.setUpGround("Ground");
   }

   public void addEnvironmentRobot(Robot robot)
   {
      environmentRobots.add(robot);
   }

   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return flatGround;
   }

   @Override
   public List<Robot> getEnvironmentRobots()
   {
      return environmentRobots;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/JoystickUpdater.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick;

import java.util.ArrayList;
import java.util.ConcurrentModificationException;
import java.util.HashMap;
import java.util.Map;

import net.java.games.input.Component.Identifier;
import net.java.games.input.Controller;
import net.java.games.input.Event;
import net.java.games.input.EventQueue;
import us.ihmc.commons.PrintTools;

public class JoystickUpdater implements Runnable
{
   private static final boolean DEBUG = false;
   
   private final Controller joystickController;
   private final Object listnerConch = new Object();
   private final ArrayList<JoystickEventListener> listeners = new ArrayList<JoystickEventListener>();
   private final ArrayList<JoystickStatusListener> generalListenersList;
   private final Map<Identifier, JoystickCompatibilityFilter> compatibilityFilterMap = new HashMap<>();
   private final Map<Identifier, JoystickCustomizationFilter> customizationFilterMap = new HashMap<>();
   
   private HashMap<String, Float> lastValues = new HashMap<String, Float>();
   private int pollIntervalMillis = 5;
   private boolean connected;
   private boolean threadRunning = false;

   public JoystickUpdater(Controller joystickController, JoystickModel model, ArrayList<JoystickStatusListener> generalListenersList)
   {
      this.joystickController = joystickController;
      this.generalListenersList = generalListenersList;
      
      for (JoystickCompatibilityFilter compatibilityFilter : model.getCompatibilityFilters())
      {
         compatibilityFilterMap.put(compatibilityFilter.getIdentifier(), compatibilityFilter);
      }
      
      connected = true;
   }
   
   public void addListener(JoystickEventListener listener)
   {
      synchronized (listnerConch)
      {
         listeners.add(listener);
      }
   }
   
   public void clearListeners()
   {
      synchronized (listnerConch)
      {
         listeners.clear();
      }
   }

   @Override
   public void run()
   {
      threadRunning = true;
      while (threadRunning)
      {
         if (!joystickController.poll())
         {
            connected = false;
            for (JoystickStatusListener listener : generalListenersList)
            {
               listener.updateConnectivity(connected);
            }
            this.stopThread();
         }
         
         EventQueue queue = joystickController.getEventQueue();
         Event event = new Event();

         while (queue.getNextEvent(event))
         {
            if (compatibilityFilterMap.containsKey(event.getComponent().getIdentifier()))
            {
               event.set(event.getComponent(), (float) compatibilityFilterMap.get(event.getComponent().getIdentifier()).apply(event.getValue()), event.getNanos());
            }
            if (customizationFilterMap.containsKey(event.getComponent().getIdentifier()))
            {
               event.set(event.getComponent(), (float) customizationFilterMap.get(event.getComponent().getIdentifier()).apply(event.getValue()), event.getNanos());
            }
            
            if (isNewValue(event))
            {
               if (DEBUG)
               {
                  System.out.println("event = " + event);
               }

               synchronized (listnerConch)
               {
                  try
                  {
                     for (JoystickEventListener listener : listeners)
                     {
                        listener.processEvent(event);
                     }
                  }
                  catch (ConcurrentModificationException e)
                  {
                     // Some listeners might not be notified.
                     PrintTools.error(this, e.getMessage());
                  }
               }

               for (JoystickStatusListener listener : generalListenersList)
               {
                  listener.updateConnectivity(connected);
               }
            }
         }

         try
         {
            Thread.sleep(pollIntervalMillis);
         }
         catch (InterruptedException e)
         {
            e.printStackTrace();
         }
      }
   }
   
   public void stopThread()
   {
      threadRunning = false;
   }

   private boolean isNewValue(Event event)
   {
      Float value = lastValues.get(event.getComponent().getName());
      if (value != null)
      {
         if (event.getValue() == value)
         {
            lastValues.put(event.getComponent().getName(), value);

            return false;
         }
      }

      lastValues.put(event.getComponent().getName(), event.getValue());

      return true;
   }

   public void setPollIntervalMillis(int pollIntervalMillis)
   {
      this.pollIntervalMillis = pollIntervalMillis;
   }
   
   public void setCustomizationFilter(JoystickCustomizationFilter componentFilter)
   {
      customizationFilterMap.put(componentFilter.getIdentifier(), componentFilter);
   }
}

<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/FootstepTestHelper.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator;

import java.util.ArrayList;
import java.util.List;

import controller_msgs.msg.dds.FootstepDataListMessage;
import controller_msgs.msg.dds.FootstepDataMessage;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.humanoidRobotics.footstep.Footstep;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class FootstepTestHelper
{
   private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
   private final SideDependentList<? extends ContactablePlaneBody> contactableFeet;

   public FootstepTestHelper(SideDependentList<? extends ContactablePlaneBody> contactableFeet)
   {
      this.contactableFeet = contactableFeet;
   }

   public List<Footstep> createFootsteps(double stepWidth, double stepLength, int numberOfSteps)
   {
      final ArrayList<Footstep> footsteps = new ArrayList<>();

      RobotSide currentSide = RobotSide.LEFT;

      FramePoint3D lastFootstepPosition = new FramePoint3D(contactableFeet.get(currentSide).getSoleFrame());
      lastFootstepPosition.changeFrame(worldFrame);

      for (int i = 0; i < numberOfSteps; i++)
      {
         currentSide = currentSide.getOppositeSide();
         double x = lastFootstepPosition.getX();
         if (i < numberOfSteps - 1)
            x += stepLength;
         double y = lastFootstepPosition.getY() + currentSide.negateIfRightSide(stepWidth);
         Footstep footstep = createFootstep(currentSide, x, y);
         footstep.getPosition(lastFootstepPosition);
         lastFootstepPosition.changeFrame(worldFrame);
         footsteps.add(footstep);
      }

      return footsteps;
   }

   private Footstep createFootstep(RobotSide robotSide, double x, double y)
   {
      return createFootstep(robotSide, new Point3D(x, y, 0.0), new Quaternion(0.0, 0.0, 0.0, 1.0));
   }

   public Footstep createFootstep(RobotSide robotSide, Point3D position, Quaternion orientation)
   {
      FramePose3D footstepPose = new FramePose3D();
      footstepPose.set(position, orientation);

      return createFootstep(robotSide, footstepPose);
   }

   public Footstep createFootstep(RobotSide robotSide, FramePose3D footstepPose)
   {
      RigidBodyBasics foot = contactableFeet.get(robotSide).getRigidBody();
      Footstep ret = new Footstep(robotSide, footstepPose);
      ret.setPredictedContactPoints(contactableFeet.get(robotSide).getContactPoints2d());

      return ret;
   }

   public List<Footstep> convertToFootsteps(FootstepDataListMessage footstepDataListMessage)
   {
      List<Footstep> ret = new ArrayList<>();
      for (int i = 0; i < footstepDataListMessage.getFootstepDataList().size(); i++)
      {
         ret.add(convertToFootstep(footstepDataListMessage.getFootstepDataList().get(i)));
      }
      return ret;
   }

   public Footstep convertToFootstep(FootstepDataMessage footstepDataMessage)
   {
      RobotSide robotSide = RobotSide.fromByte(footstepDataMessage.getRobotSide());
      RigidBodyBasics foot = contactableFeet.get(robotSide).getRigidBody();
      FramePose3D solePose = new FramePose3D(worldFrame, footstepDataMessage.getLocation(), footstepDataMessage.getOrientation());
      Footstep footstep = new Footstep(robotSide, solePose);
      if (footstepDataMessage.getPredictedContactPoints2d() != null && !footstepDataMessage.getPredictedContactPoints2d().isEmpty())
         footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(footstepDataMessage));
      else
         footstep.setPredictedContactPoints(contactableFeet.get(robotSide).getContactPoints2d());

      return footstep;
   }
}


<|start_filename|>simulation-construction-set-tools/src/test/java/us/ihmc/simulationConstructionSetTools/simulationTesting/SimulationTester.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationTesting;

import java.util.ArrayList;

import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.UnreasonableAccelerationException;

public abstract class SimulationTester
{
   protected static final int NUMBER_OF_SIMULATIONS = 2;
   private static final boolean DESTROY_SIMULATIONS_WHEN_DONE = true;
   
   protected final SimulationComparer simulationComparer;
   protected abstract ArrayList<SimulationConstructionSet> createAndStartSimulations();

   public SimulationTester(SimulationComparer simulationComparer)
   {
      this.simulationComparer = simulationComparer;
   }

   public boolean test(double simulationTime)
   {
      ArrayList<SimulationConstructionSet> simulationConstructionSets = createAndStartSimulations();
      simulate(simulationConstructionSets, simulationTime);
      waitForSimulationsToFinish(simulationConstructionSets);
      
      // Some stupid bug requires us to sleep a little or else things fail every so often...
      try
      {
         Thread.sleep(4000);
      }
      catch (InterruptedException e)
      {
      }
      
      if (!didSimulationsGetToEndTime(simulationTime, simulationConstructionSets))
      {
         System.err.println("Simlulation didn't get to end time!");
         return false;
      }
      
      boolean ret = simulationComparer.compare(simulationConstructionSets.get(0), simulationConstructionSets.get(1));
      
      if (DESTROY_SIMULATIONS_WHEN_DONE)
      {
         for (SimulationConstructionSet scs : simulationConstructionSets)
         {
            scs.closeAndDispose();
         }
      }
      
      return ret;
   }
   
   private boolean didSimulationsGetToEndTime(double simulationTime, ArrayList<SimulationConstructionSet> simulationConstructionSets)
   {
      double endTime0 = simulationConstructionSets.get(0).getRobots()[0].getTime();
      double endTime1 = simulationConstructionSets.get(0).getRobots()[0].getTime();
      
      double errorTime0 = Math.abs(simulationTime - endTime0);
      double errorTime1 = Math.abs(simulationTime - endTime1);
      
//      System.out.println("errorTime0 = " + errorTime0);
//      System.out.println("errorTime1 = " + errorTime1);
      
      if (errorTime0 > 1e-3) return false;
      if (errorTime1 > 1e-3) return false;
      
      return true;
   }
   
   
   public double findTimeOfFirstDifference(double maxSimulationTime)
   {
      ArrayList<SimulationConstructionSet> simulationConstructionSets = createAndStartSimulations();
      boolean differenceFound = false;
      Robot robotToUseForTime = simulationConstructionSets.get(0).getRobots()[0];
      double time = robotToUseForTime.getTime();
      while (!differenceFound && (time < maxSimulationTime))
      {
         for (SimulationConstructionSet scs : simulationConstructionSets)
         {
            try
            {
               scs.simulateOneTimeStep();
            }
            catch (UnreasonableAccelerationException e)
            {
               e.printStackTrace();
            }
         }

         differenceFound = !simulationComparer.compare(simulationConstructionSets.get(0), simulationConstructionSets.get(1));
         time = robotToUseForTime.getTime();
      }

      double ret = differenceFound ? time : Double.NaN;
      
      if (DESTROY_SIMULATIONS_WHEN_DONE)
      {
         for (SimulationConstructionSet scs : simulationConstructionSets)
         {
            scs.closeAndDispose();
         }
      }
      return ret;
   }

   private void simulate(ArrayList<SimulationConstructionSet> simulationConstructionSets, double simulationTime)
   {
      for (SimulationConstructionSet scs : simulationConstructionSets)
      {
         scs.simulate(simulationTime);
      }
   }

   private void waitForSimulationsToFinish(ArrayList<SimulationConstructionSet> simulationConstructionSets)
   {
      boolean allDone = false;
      while (!allDone)
      {
         allDone = true;

         for (SimulationConstructionSet scs : simulationConstructionSets)
         {
            if (scs.isSimulating())
            {
               allDone = false;
            }
         }
      }
   }
   

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/globalParameters/IntGlobalParameter.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.globalParameters;

import us.ihmc.yoVariables.variable.YoInteger;

public class IntGlobalParameter extends GlobalParameter
{
   public IntGlobalParameter(String name, String description, int value, GlobalParameterChangedListener listener)
   {
      super(listener);

      yoVariable = new YoInteger(name, description, registry);
      ((YoInteger)yoVariable).set(value);

      if (changedListener != null)
         changedListener.globalParameterCreated(this);
   }

   protected IntGlobalParameter(String name, String description, GlobalParameter[] parents, GlobalParameterChangedListener listener)
   {
      super(parents, listener);

      yoVariable = new YoInteger(name, description, registry);

      if (changedListener != null)
         changedListener.globalParameterCreated(this);
   }

   public int getValue()
   {
      return ((YoInteger)yoVariable).getIntegerValue();
   }

   protected int getNumberOfCharactersForDisplay()
   {
      return Integer.toString(Integer.MIN_VALUE).length();
   }

   public void set(int value)
   {
      set(value, "");
   }

   public void set(int value, String comment)
   {
      verifyNoParents();
      setIntegerValue(value, comment);
   }

   public void setOnlyIfChange(int value)
   {
      setOnlyIfChange(value, "");
   }

   public void setOnlyIfChange(int value, String comment)
   {
      verifyNoParents();

      if (value == ((YoInteger)yoVariable).getIntegerValue())
         return;
      else
         set(value, comment);
   }


   @Override
   public String getValueInStringFormat()
   {
      String ret = Integer.toString(((YoInteger)yoVariable).getIntegerValue());

      return padWithSpaces(ret, numberOfCharactersForDisplay);
   }



   private void setIntegerValue(int newValue, String comment)
   {
      int previousValue = ((YoInteger)yoVariable).getIntegerValue();
      ((YoInteger)yoVariable).set(newValue);

      if (changedListener != null)
      {
         changedListener.integerValueChanged(this, comment, previousValue, newValue);
      }

      updateChildren(comment);
   }




}


<|start_filename|>simulation-construction-set-tools/src/test/java/us/ihmc/simulationConstructionSetTools/optimization/SimpleRobotToOptimize.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.optimization;

import us.ihmc.simulationconstructionset.Robot;

public class SimpleRobotToOptimize extends Robot
{
   public SimpleRobotToOptimize()
   {
      super("SimpleRobotToOptimize");
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/RobotProtocolCommand.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication;

public enum RobotProtocolCommand
{
   HELLO, PERIOD, REQ_ALL_REGISTRIES_AND_VARIABLES, ALL_REGISTRIES_AND_VARIABLES, REGISTRY_SETTINGS, REGISTRY_SETTINGS_PROCESSED, SENT_VAR_INDICES, UPDATE_VARS,
   CHECK_CONNECTED, DISCONNECT, SET, USR_CMD, DATA, TEXT_MESSAGE;
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/GroupParameter.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

import java.util.Arrays;
import java.util.Collections;
import java.util.List;

/**
 * This class is a data structure that can hold a parameter that is shared by a group. This can
 * be a joint group or a body group for example. When tunable parameters are created, only one
 * tunable (through YoVariables or Parameters) implementation of the parameter {@code T} will
 * be instantiated and shared between all members of the group.
 * <p>
 * For example, if all arm joints have a common parameter then the name-prefix of the tunable
 * controller parameter will be the groupName ('arm'). The member name list should contain
 * all arm joint names of the robot. The parameter {@code T} will contain the initial (for
 * YoVariables) or the default (for Parameters) values.
 * </p>
 * @param <T> is the type of parameter.
 */
public class GroupParameter<T>
{
   private final T parameter;
   private final String groupName;
   private final List<String> memberNames;

   public GroupParameter(String groupAndMemberName)
   {
      this(groupAndMemberName, groupAndMemberName);
   }

   public GroupParameter(String groupName, String memberName)
   {
      this(groupName, Collections.singletonList(memberName));
   }

   public GroupParameter(String groupName, String... memberNames)
   {
      this(groupName, Arrays.asList(memberNames));
   }

   public GroupParameter(String groupName, List<String> memberNames)
   {
      this(groupName, null, memberNames);
   }

   public GroupParameter(String groupAndMemberName, T parameter)
   {
      this(groupAndMemberName, parameter, groupAndMemberName);
   }

   public GroupParameter(String groupName, T parameter, String memberName)
   {
      this(groupName, parameter, Collections.singletonList(memberName));
   }

   public GroupParameter(String groupName, T parameter, String... memberNames)
   {
      this(groupName, parameter, Arrays.asList(memberNames));
   }

   public GroupParameter(String groupName, T parameter, List<String> memberNames)
   {
      this.groupName = groupName;
      this.parameter = parameter;
      this.memberNames = memberNames;
   }

   /**
    * Whether this {@link #GroupParameter} was created with a parameter.
    */
   public boolean hasParameter()
   {
      return parameter != null;
   }

   /**
    * Get the default value for the shared parameter.
    */
   public T getParameter()
   {
      return parameter;
   }

   /**
    * Get the name of the group that should share a single parameter.
    */
   public String getGroupName()
   {
      return groupName;
   }

   /**
    * Get the names of the members of the group that share the parameter.
    */
   public List<String> getMemberNames()
   {
      return memberNames;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/activationFunction/Relu.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction;

/**
 * The output = the input if the input > 0 otherwise output = 0
 */
public class Relu implements ActivationFunction
{
   @Override
   public double computeOutput(double input)
   {
      return Math.max(0, input);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/EmergencyValveEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.simulationConstructionSetTools.util.environments.environmentRobots.ContactableValveRobot;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationConstructionSetTools.robotController.ContactController;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class EmergencyValveEnvironment implements CommonAvatarEnvironmentInterface
{
   private final List<ContactableValveRobot> valveRobot = new ArrayList<ContactableValveRobot>();
   private final CombinedTerrainObject3D combinedTerrainObject;

   private final ArrayList<ExternalForcePoint> contactPoints = new ArrayList<ExternalForcePoint>();

   public EmergencyValveEnvironment()
   {
      
      this(0.75f,-0.38f,1.1811f, 0.0);
   }

   public EmergencyValveEnvironment(ArrayList<Point3D> valveLocations, LinkedHashMap<Point3D, Double> valveYawAngles_degrees)
   {
      double forceVectorScale = 1.0 / 50.0;

      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject.addTerrainObject(DefaultCommonAvatarEnvironment.setUpGround("Ground"));

      int i = 0;
      for (Point3D valveLocation : valveLocations)
      {
         String valveRobotName = "ValveRobot" + i;
         createValve(valveRobotName, ValveType.BIG_VALVE, valveLocation.getX(), valveLocation.getY(), valveLocation.getZ(), valveYawAngles_degrees.get(valveLocation),
               forceVectorScale);
         i++;
      }
   }

   public EmergencyValveEnvironment(double valveX, double valveY, double valveZ, double valveYaw_degrees)
   {
      double forceVectorScale = 1.0 / 50.0;

      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject.addTerrainObject(DefaultCommonAvatarEnvironment.setUpGround("Ground"));

      createValve("ValveRobot", ValveType.SMALL_VALVE, valveX, valveY, valveZ, valveYaw_degrees, forceVectorScale);

   }

   private void createValve(String valveRobotName, ValveType valveType, double x, double y, double z, double yaw_degrees, double forceVectorScale)
   {
      FramePose3D valvePose = new FramePose3D(ReferenceFrame.getWorldFrame());
      Point3D position = new Point3D(x, y, z);
      Quaternion orientation = new Quaternion();

      orientation.setYawPitchRoll(Math.toRadians(yaw_degrees), Math.toRadians(0), Math.toRadians(0));
      valvePose.set(position, orientation);

      ContactableValveRobot valve = new ContactableValveRobot(valveRobotName, valveType, 0.5, valvePose);

      valve.createValveRobot();
      valve.createAvailableContactPoints(1, 30, forceVectorScale, true);

      valveRobot.add(valve);
   }

   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject;
   }

   @Override
   public List<ContactableValveRobot> getEnvironmentRobots()
   {
      return valveRobot;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
      ContactController contactController = new ContactController();
      contactController.setContactParameters(1000.0, 100.0, 0.5, 0.3);
      contactController.addContactPoints(contactPoints);
      contactController.addContactables(valveRobot);
      valveRobot.get(0).setController(contactController);
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
      this.contactPoints.addAll(externalForcePoints);

   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {

   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MovingMidFrame.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;

/**
 * {@code MovingMidFrame} represents a the average of two given reference frames.
 *
 */
public class MovingMidFrame extends MovingReferenceFrame
{
   private final MovingReferenceFrame frameOne, frameTwo;
   private final FramePose3D pose = new FramePose3D();
   private final FramePose3D poseOne = new FramePose3D();
   private final FramePose3D poseTwo = new FramePose3D();

   public MovingMidFrame(String name, MovingReferenceFrame frameOne, MovingReferenceFrame frameTwo)
   {
      super(name, frameOne.getRootFrame());

      if (frameOne == frameTwo)
         throw new IllegalArgumentException("The frames have to be different.");
      frameOne.verifySameRoots(frameTwo);

      this.frameOne = frameOne;
      this.frameTwo = frameTwo;
   }

   @Override
   protected void updateTransformToParent(RigidBodyTransform transformToParent)
   {
      poseOne.setToZero(frameOne);
      poseTwo.setToZero(frameTwo);

      poseOne.changeFrame(getParent());
      poseTwo.changeFrame(getParent());

      pose.setToZero(getParent());
      pose.interpolate(poseOne, poseTwo, 0.5);
      pose.get(transformToParent);
   }

   private final FrameVector3D linearVelocity = new FrameVector3D();
   private final FrameVector3D linearVelocityOne = new FrameVector3D();
   private final FrameVector3D linearVelocityTwo = new FrameVector3D();

   private final FrameVector3D angularVelocity = new FrameVector3D();
   private final FrameVector3D angularVelocityOne = new FrameVector3D();
   private final FrameVector3D angularVelocityTwo = new FrameVector3D();

   private final Quaternion difference = new Quaternion();
   private final Quaternion quaternionFuture = new Quaternion();
   private final Quaternion quaternionFutureOne = new Quaternion();
   private final Quaternion quaternionFutureTwo = new Quaternion();

   @Override
   protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack)
   {
      twistRelativeToParentToPack.setToZero(this, getParent(), this);

      TwistReadOnly twistOfFrameOne = frameOne.getTwistOfFrame();
      TwistReadOnly twistOfFrameTwo = frameTwo.getTwistOfFrame();

      linearVelocityOne.setIncludingFrame(twistOfFrameOne.getLinearPart());
      linearVelocityTwo.setIncludingFrame(twistOfFrameTwo.getLinearPart());

      linearVelocityOne.changeFrame(this);
      linearVelocityTwo.changeFrame(this);
      linearVelocity.setToZero(this);
      linearVelocity.interpolate(linearVelocityOne, linearVelocityTwo, 0.5);
      twistRelativeToParentToPack.getLinearPart().set(linearVelocity);

      angularVelocityOne.setIncludingFrame(twistOfFrameOne.getAngularPart());
      angularVelocityTwo.setIncludingFrame(twistOfFrameTwo.getAngularPart());

      computeAngularVelocityNumeric();

      twistRelativeToParentToPack.getAngularPart().set(angularVelocity);
   }

   /**
    * Not happy with this solution, but computing the angular velocity of the average orientation
    * appears to be rather complex.
    * <p>
    * The variables are the following:
    * <ul>
    * <li>The frame one has an orientation q<sub>1</sub> and an angular velocity &omega<sub>1</sub>.
    * <li>The frame two has an orientation q<sub>2</sub> and an angular velocity &omega<sub>2</sub>.
    * </ul>
    * This mid frame has an orientation q<sub>i</sub> located in the middle of q<sub>1</sub> and
    * q<sub>2</sub>. It is calculated as follows:<br>
    * q<sub>i</sub> = q<sub>1</sub> (q<sub>1</sub><sup>-1</sup>q<sub>2</sub>)<sup>0.5</sup>
    * </p>
    * When comparing against finite difference, it is clear that: &omega;<sub>i</sub> &ne; 0.5
    * (&omega;<sub>1</sub> + &omega;<sub>2</sub>).
    * <p>
    * <p>
    * Therefore, the time-derivative of the equation used to compute q<sub>i</sub> needs to be
    * calculated using quaternion differential calculus. However, I keep getting stuck at computing
    * the time-derivative of a quaternion raised to a power:<br>
    * 
    * <pre>
    * d q(t)<sup>&alpha;</sup>
    * ------   = ???
    *  dt
    * </pre>
    * 
    * where in our case &alpha; = 0.5.
    * </p>
    * <p>
    * So as a workaround, using the known angular velocities, q<sub>i</sub> is evaluated at
    * t+&Delta;t and the angular velocity computed from finite difference. This should behave better
    * than simply using {@code NumericalMovingReferenceFrame} which does not take into account the
    * angular velocities of the two input frames.
    * </p>
    */
   public void computeAngularVelocityNumeric()
   {
      double integrationDT = 0.00001;

      angularVelocityOne.scale(integrationDT);
      angularVelocityTwo.scale(integrationDT);

      quaternionFutureOne.setRotationVector(angularVelocityOne);
      quaternionFutureTwo.setRotationVector(angularVelocityTwo);

      quaternionFutureOne.preMultiply(poseOne.getOrientation());
      quaternionFutureTwo.preMultiply(poseTwo.getOrientation());

      quaternionFuture.interpolate(quaternionFutureOne, quaternionFutureTwo, 0.5);
      difference.difference(pose.getOrientation(), quaternionFuture);
      angularVelocity.setToZero(this);
      difference.getRotationVector(angularVelocity);
      angularVelocity.scale(1.0 / integrationDT);
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/producers/CompressedVideoDataServer.java<|end_filename|>
package us.ihmc.communication.producers;

public interface CompressedVideoDataServer extends VideoDataServer
{
   public void setVideoControlSettings(VideoControlSettings object);
   
   public void dispose();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedSteppingStateChangeMessage.java<|end_filename|>
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 quadruped controller API.
       * This message notifies the user of a change in the stepping controller state.
       * This message's primary use is to signal a requested state change is completed.
       */
public class QuadrupedSteppingStateChangeMessage extends Packet<QuadrupedSteppingStateChangeMessage> implements Settable<QuadrupedSteppingStateChangeMessage>, EpsilonComparable<QuadrupedSteppingStateChangeMessage>
{
   public static final byte STAND = (byte) 0;
   public static final byte STEP = (byte) 1;
   public static final byte SOLE_WAYPOINT = (byte) 2;
   /**
            * Specifies the controller's state prior to transition.
            */
   public byte initial_quadruped_stepping_state_enum_ = (byte) 255;
   /**
            * Specifies the state the controller has transitioned into.
            */
   public byte end_quadruped_stepping_state_enum_ = (byte) 255;

   public QuadrupedSteppingStateChangeMessage()
   {
   }

   public QuadrupedSteppingStateChangeMessage(QuadrupedSteppingStateChangeMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedSteppingStateChangeMessage other)
   {
      initial_quadruped_stepping_state_enum_ = other.initial_quadruped_stepping_state_enum_;

      end_quadruped_stepping_state_enum_ = other.end_quadruped_stepping_state_enum_;

   }

   /**
            * Specifies the controller's state prior to transition.
            */
   public void setInitialQuadrupedSteppingStateEnum(byte initial_quadruped_stepping_state_enum)
   {
      initial_quadruped_stepping_state_enum_ = initial_quadruped_stepping_state_enum;
   }
   /**
            * Specifies the controller's state prior to transition.
            */
   public byte getInitialQuadrupedSteppingStateEnum()
   {
      return initial_quadruped_stepping_state_enum_;
   }

   /**
            * Specifies the state the controller has transitioned into.
            */
   public void setEndQuadrupedSteppingStateEnum(byte end_quadruped_stepping_state_enum)
   {
      end_quadruped_stepping_state_enum_ = end_quadruped_stepping_state_enum;
   }
   /**
            * Specifies the state the controller has transitioned into.
            */
   public byte getEndQuadrupedSteppingStateEnum()
   {
      return end_quadruped_stepping_state_enum_;
   }


   public static Supplier<QuadrupedSteppingStateChangeMessagePubSubType> getPubSubType()
   {
      return QuadrupedSteppingStateChangeMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedSteppingStateChangeMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedSteppingStateChangeMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.initial_quadruped_stepping_state_enum_, other.initial_quadruped_stepping_state_enum_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.end_quadruped_stepping_state_enum_, other.end_quadruped_stepping_state_enum_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedSteppingStateChangeMessage)) return false;

      QuadrupedSteppingStateChangeMessage otherMyClass = (QuadrupedSteppingStateChangeMessage) other;

      if(this.initial_quadruped_stepping_state_enum_ != otherMyClass.initial_quadruped_stepping_state_enum_) return false;

      if(this.end_quadruped_stepping_state_enum_ != otherMyClass.end_quadruped_stepping_state_enum_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedSteppingStateChangeMessage {");
      builder.append("initial_quadruped_stepping_state_enum=");
      builder.append(this.initial_quadruped_stepping_state_enum_);      builder.append(", ");
      builder.append("end_quadruped_stepping_state_enum=");
      builder.append(this.end_quadruped_stepping_state_enum_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/hyperCubeTree/HyperCubeTreeStaticTestingUtilities.java<|end_filename|>
package us.ihmc.robotics.hyperCubeTree;

import java.util.Random;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotics.random.RandomGeometry;

public class HyperCubeTreeStaticTestingUtilities
{

   public static void shootTreeIntoSphere(Octree tree, double radius, double shootingDistance, double xCenter, double yCenter, double zCenter, int numberOfLidarBullets)
   {
      Random rand = new Random(-1985L);
      double[] centerAsArray = new double[] { xCenter, yCenter, zCenter };
      double[] startPointAsArray = new double[3];
      double[] endPointAsArray = new double[3];
      double[] vectorAsArray = new double[3];
      double radiusToShootFrom = shootingDistance;
      for (int k = 0; k < numberOfLidarBullets; k++)
      {
         Vector3D vector3d = RandomGeometry.nextVector3D(rand, 1.0);
         vector3d.get(vectorAsArray);
         for (int j = 0; j < 3; j++)
         {
            startPointAsArray[j] = vectorAsArray[j] * radiusToShootFrom + centerAsArray[j];
            endPointAsArray[j] = vectorAsArray[j] * radius + centerAsArray[j];
         }
         Point3D start = new Point3D(startPointAsArray);
         Point3D end = new Point3D(endPointAsArray);
         tree.putLidarAtGraduallyMoreAccurateResolution(start, end);
      }
//      tree.mergeIfPossible();
   }

   public static void fillOctreeDefaultTest(HyperCubeTree<Boolean, Void> tree, double d1, double d2, double d3, double d4)
   {
      tree.put(new double[] { d1, d3, d1 }, true);
      tree.put(new double[] { d1, d3, d1 }, true);
      tree.put(new double[] { d2, d3, d1 }, false);
      tree.put(new double[] { d1, d4, d1 }, true);
      tree.put(new double[] { d2, d4, d1 }, true);
      tree.put(new double[] { d1, d3, d2 }, true);
      tree.put(new double[] { d2, d3, d2 }, false);
      tree.put(new double[] { d1, d4, d2 }, true);
      tree.put(new double[] { d2, d4, d2 }, true);
   
      //LLL Oct
      //LLL Oct left intentionally undefined.
   
      //HLL Oct 
      tree.put(new double[] { d3, d1, d1 }, true);
   
      //HHL Oct         
      tree.put(new double[] { d3, d3, d1 }, true);
   
      //LLH Oct   
      tree.put(new double[] { d1, d1, d3 }, true);
      tree.put(new double[] { d2, d1, d3 }, false);
      tree.put(new double[] { d1, d2, d3 }, true);
      tree.put(new double[] { d2, d2, d3 }, false);
      tree.put(new double[] { d1, d1, d4 }, true);
      tree.put(new double[] { d2, d1, d4 }, false);
      tree.put(new double[] { d1, d2, d4 }, true);
      tree.put(new double[] { d2, d2, d4 }, false);
   
      //HLH Oct
      tree.put(new double[] { d3, d1, d3 }, false);
      tree.put(new double[] { d4, d1, d3 }, true);
      tree.put(new double[] { d3, d2, d3 }, false);
      tree.put(new double[] { d4, d2, d3 }, true);
      tree.put(new double[] { d3, d1, d4 }, false);
      tree.put(new double[] { d4, d1, d4 }, false);
      tree.put(new double[] { d3, d2, d4 }, false);
      tree.put(new double[] { d4, d2, d4 }, false);
   
      //LHH Oct
      tree.put(new double[] { d1, d3, d3 }, false);
   
      //HHH Oct
      tree.put(new double[] { d3, d3, d3 }, false);
      tree.put(new double[] { d4, d3, d3 }, true);
      tree.put(new double[] { d3, d4, d3 }, false);
      tree.put(new double[] { d4, d4, d3 }, false);
      tree.put(new double[] { d3, d3, d4 }, false);
      tree.put(new double[] { d4, d3, d4 }, false);
      tree.put(new double[] { d3, d4, d4 }, false);
      tree.put(new double[] { d4, d4, d4 }, false);
   }



}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/outputData/LowLevelStateList.java<|end_filename|>
package us.ihmc.sensorProcessing.outputData;

import java.util.HashMap;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.tools.lists.PairList;

public class LowLevelStateList implements LowLevelStateListReadOnly
{
   private final PairList<OneDoFJointBasics, LowLevelState> jointsAndData = new PairList<>();
   private final HashMap<OneDoFJointBasics, LowLevelState> jointMap = new HashMap<>();

   public LowLevelStateList(OneDoFJointBasics[] joints)
   {
      for (OneDoFJointBasics joint : joints)
      {
         LowLevelState data = new LowLevelState();
         jointsAndData.add(joint, data);
         jointMap.put(joint, data);
      }
   }

   @Override
   public boolean hasDataForJoint(OneDoFJointBasics joint)
   {
      return jointMap.containsKey(joint);
   }

   @Override
   public OneDoFJointBasics getOneDoFJoint(int index)
   {
      return jointsAndData.first(index);
   }

   @Override
   public LowLevelState getLowLevelState(OneDoFJointBasics joint)
   {
      return jointMap.get(joint);
   }

   @Override
   public int getNumberOfJointsWithLowLevelState()
   {
      return jointsAndData.size();
   }

   @Override
   public LowLevelStateReadOnly getLowLevelState(int index)
   {
      return jointsAndData.second(index);
   }

   public String getJointName(int index)
   {
      return jointsAndData.first(index).getName();
   }

   public void clear()
   {
      for (int jointIdx = 0; jointIdx < jointsAndData.size(); jointIdx++)
      {
         jointsAndData.get(jointIdx).getValue().clear();
      }
   }

   public void overwriteWith(LowLevelStateListReadOnly other)
   {
      for (int i = 0; i < jointsAndData.size(); i++)
      {

         OneDoFJointBasics joint = jointsAndData.first(i);
         LowLevelState data = jointsAndData.second(i);

         LowLevelStateReadOnly otherData = other.getLowLevelState(joint);
         if (otherData != null)
         {
            data.set(otherData);
         }

      }
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/wholeBodyTrajectoryToolboxModule/WholeBodyTrajectoryToolboxDataHandler.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.wholeBodyTrajectoryToolboxModule;

import us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace.CTTaskNode;
import us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace.CTTaskNodeTree;

public class WholeBodyTrajectoryToolboxDataHandler
{
   private CTTaskNode rootNode;
   private CTTaskNodeTree tree;

   public WholeBodyTrajectoryToolboxDataHandler(WholeBodyTrajectoryToolboxData toolboxData)
   {

   }

   public void addExplorationConfigurationSpace(ConstrainedRigidBodyTrajectory constrainedRigidBodyTrajectory)
   {

   }

   public void setTrajectoryTime()
   {

   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/MessageCollectionNotification.java<|end_filename|>
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.
       * Notifies the user that a MessageCollection has been received.
       * This is used to know when the controller is ready to start collecting the actual collection of messages.
       */
public class MessageCollectionNotification extends Packet<MessageCollectionNotification> implements Settable<MessageCollectionNotification>, EpsilonComparable<MessageCollectionNotification>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * This is the sequence ID of the MessageCollection that has just been received.
            */
   public long message_collection_sequence_id_;

   public MessageCollectionNotification()
   {
   }

   public MessageCollectionNotification(MessageCollectionNotification other)
   {
      this();
      set(other);
   }

   public void set(MessageCollectionNotification other)
   {
      sequence_id_ = other.sequence_id_;

      message_collection_sequence_id_ = other.message_collection_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_;
   }

   /**
            * This is the sequence ID of the MessageCollection that has just been received.
            */
   public void setMessageCollectionSequenceId(long message_collection_sequence_id)
   {
      message_collection_sequence_id_ = message_collection_sequence_id;
   }
   /**
            * This is the sequence ID of the MessageCollection that has just been received.
            */
   public long getMessageCollectionSequenceId()
   {
      return message_collection_sequence_id_;
   }


   public static Supplier<MessageCollectionNotificationPubSubType> getPubSubType()
   {
      return MessageCollectionNotificationPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return MessageCollectionNotificationPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(MessageCollectionNotification other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.message_collection_sequence_id_, other.message_collection_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 MessageCollectionNotification)) return false;

      MessageCollectionNotification otherMyClass = (MessageCollectionNotification) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.message_collection_sequence_id_ != otherMyClass.message_collection_sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("MessageCollectionNotification {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("message_collection_sequence_id=");
      builder.append(this.message_collection_sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/NetworkedObjectCommunicator.java<|end_filename|>
package us.ihmc.communication.net;

public interface NetworkedObjectCommunicator extends ObjectCommunicator
{
   /**
    * Same as consumeObject, but returns bytes send.
    * 
    * @param object
    * @return bytes send
    */
   int send(Object object);

   /**
    * TCP connections may want to change settings like keepAlive or timeOut.
    */
   void attachStateListener(TcpNetStateListener stateListener);

   /**
    * Disconnect the connection, but leave the executor listeners alive. This allows re-connecting at a later moment. 
    * Use disconnect() when you want to shutdown the connection completely. 
    */
   void closeConnection();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/MultisenseParameterPacket.java<|end_filename|>
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;

public class MultisenseParameterPacket extends Packet<MultisenseParameterPacket> implements Settable<MultisenseParameterPacket>, EpsilonComparable<MultisenseParameterPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean initialize_;
   public double gain_;
   public double motor_speed_;
   public boolean led_enable_;
   public boolean flash_enable_;
   public double duty_cycle_;
   public boolean auto_exposure_;
   public boolean auto_white_balance_;

   public MultisenseParameterPacket()
   {
   }

   public MultisenseParameterPacket(MultisenseParameterPacket other)
   {
      this();
      set(other);
   }

   public void set(MultisenseParameterPacket other)
   {
      sequence_id_ = other.sequence_id_;

      initialize_ = other.initialize_;

      gain_ = other.gain_;

      motor_speed_ = other.motor_speed_;

      led_enable_ = other.led_enable_;

      flash_enable_ = other.flash_enable_;

      duty_cycle_ = other.duty_cycle_;

      auto_exposure_ = other.auto_exposure_;

      auto_white_balance_ = other.auto_white_balance_;

   }

   /**
            * 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 void setInitialize(boolean initialize)
   {
      initialize_ = initialize;
   }
   public boolean getInitialize()
   {
      return initialize_;
   }

   public void setGain(double gain)
   {
      gain_ = gain;
   }
   public double getGain()
   {
      return gain_;
   }

   public void setMotorSpeed(double motor_speed)
   {
      motor_speed_ = motor_speed;
   }
   public double getMotorSpeed()
   {
      return motor_speed_;
   }

   public void setLedEnable(boolean led_enable)
   {
      led_enable_ = led_enable;
   }
   public boolean getLedEnable()
   {
      return led_enable_;
   }

   public void setFlashEnable(boolean flash_enable)
   {
      flash_enable_ = flash_enable;
   }
   public boolean getFlashEnable()
   {
      return flash_enable_;
   }

   public void setDutyCycle(double duty_cycle)
   {
      duty_cycle_ = duty_cycle;
   }
   public double getDutyCycle()
   {
      return duty_cycle_;
   }

   public void setAutoExposure(boolean auto_exposure)
   {
      auto_exposure_ = auto_exposure;
   }
   public boolean getAutoExposure()
   {
      return auto_exposure_;
   }

   public void setAutoWhiteBalance(boolean auto_white_balance)
   {
      auto_white_balance_ = auto_white_balance;
   }
   public boolean getAutoWhiteBalance()
   {
      return auto_white_balance_;
   }


   public static Supplier<MultisenseParameterPacketPubSubType> getPubSubType()
   {
      return MultisenseParameterPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return MultisenseParameterPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(MultisenseParameterPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.initialize_, other.initialize_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.gain_, other.gain_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.motor_speed_, other.motor_speed_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.led_enable_, other.led_enable_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.flash_enable_, other.flash_enable_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.duty_cycle_, other.duty_cycle_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.auto_exposure_, other.auto_exposure_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.auto_white_balance_, other.auto_white_balance_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof MultisenseParameterPacket)) return false;

      MultisenseParameterPacket otherMyClass = (MultisenseParameterPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.initialize_ != otherMyClass.initialize_) return false;

      if(this.gain_ != otherMyClass.gain_) return false;

      if(this.motor_speed_ != otherMyClass.motor_speed_) return false;

      if(this.led_enable_ != otherMyClass.led_enable_) return false;

      if(this.flash_enable_ != otherMyClass.flash_enable_) return false;

      if(this.duty_cycle_ != otherMyClass.duty_cycle_) return false;

      if(this.auto_exposure_ != otherMyClass.auto_exposure_) return false;

      if(this.auto_white_balance_ != otherMyClass.auto_white_balance_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("MultisenseParameterPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("initialize=");
      builder.append(this.initialize_);      builder.append(", ");
      builder.append("gain=");
      builder.append(this.gain_);      builder.append(", ");
      builder.append("motor_speed=");
      builder.append(this.motor_speed_);      builder.append(", ");
      builder.append("led_enable=");
      builder.append(this.led_enable_);      builder.append(", ");
      builder.append("flash_enable=");
      builder.append(this.flash_enable_);      builder.append(", ");
      builder.append("duty_cycle=");
      builder.append(this.duty_cycle_);      builder.append(", ");
      builder.append("auto_exposure=");
      builder.append(this.auto_exposure_);      builder.append(", ");
      builder.append("auto_white_balance=");
      builder.append(this.auto_white_balance_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/controlModules/foot/QuadrupedFootStates.java<|end_filename|>
package us.ihmc.quadrupedRobotics.controlModules.foot;

public enum QuadrupedFootStates
{
   SUPPORT,
   SWING,
   MOVE_VIA_WAYPOINTS;

   public static final QuadrupedFootStates[] values = values();
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/communication/BehaviorPacketPassthroughList.java<|end_filename|>
package us.ihmc.humanoidBehaviors.communication;

import controller_msgs.msg.dds.StampedPosePacket;

public class BehaviorPacketPassthroughList
{
   public static Class[] PACKETS_TO_ALWAYS_PASS_FROM_NP_TO_CONTROLLER_THROUGH_BEHAVIORS = {
      StampedPosePacket.class
 };
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/MidiChannelMapper.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public interface MidiChannelMapper
{
   int getKnobChannel(int knob);
   int getSliderChannel(int slider);
   int getButtonChannel(int button);
   int getKnobButtonChannel(int knob);
}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/controller/toolbox/QuadrupedWaypointCallback.java<|end_filename|>
package us.ihmc.quadrupedRobotics.controller.toolbox;

import us.ihmc.robotics.robotSide.RobotQuadrant;

public interface QuadrupedWaypointCallback
{
   void isDoneMoving(RobotQuadrant robotQuadrant, boolean doneMoving);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/partNames/QuadrupedJointName.java<|end_filename|>
package us.ihmc.robotics.partNames;

import us.ihmc.robotics.robotSide.RobotQuadrant;

public enum QuadrupedJointName
{
   /* Neck joints */
   PROXIMAL_NECK_YAW(NeckJointName.PROXIMAL_NECK_YAW),
   PROXIMAL_NECK_PITCH(NeckJointName.PROXIMAL_NECK_PITCH),
   PROXIMAL_NECK_ROLL(NeckJointName.PROXIMAL_NECK_ROLL),
   DISTAL_NECK_YAW(NeckJointName.DISTAL_NECK_YAW),
   DISTAL_NECK_PITCH(NeckJointName.DISTAL_NECK_PITCH),
   DISTAL_NECK_ROLL(NeckJointName.DISTAL_NECK_ROLL),

   /* Leg joints */
   FRONT_LEFT_HIP_YAW(RobotQuadrant.FRONT_LEFT, LegJointName.HIP_YAW),
   FRONT_LEFT_HIP_ROLL(RobotQuadrant.FRONT_LEFT, LegJointName.HIP_ROLL),
   FRONT_LEFT_HIP_PITCH(RobotQuadrant.FRONT_LEFT, LegJointName.HIP_PITCH),
   FRONT_LEFT_KNEE_PITCH(RobotQuadrant.FRONT_LEFT, LegJointName.KNEE_PITCH),
   FRONT_LEFT_ANKLE_ROLL(RobotQuadrant.FRONT_LEFT, LegJointName.ANKLE_ROLL),
   FRONT_LEFT_ANKLE_PITCH(RobotQuadrant.FRONT_LEFT, LegJointName.ANKLE_PITCH),
   FRONT_RIGHT_HIP_YAW(RobotQuadrant.FRONT_RIGHT, LegJointName.HIP_YAW),
   FRONT_RIGHT_HIP_ROLL(RobotQuadrant.FRONT_RIGHT, LegJointName.HIP_ROLL),
   FRONT_RIGHT_HIP_PITCH(RobotQuadrant.FRONT_RIGHT, LegJointName.HIP_PITCH),
   FRONT_RIGHT_KNEE_PITCH(RobotQuadrant.FRONT_RIGHT, LegJointName.KNEE_PITCH),
   FRONT_RIGHT_ANKLE_ROLL(RobotQuadrant.FRONT_RIGHT, LegJointName.ANKLE_ROLL),
   FRONT_RIGHT_ANKLE_PITCH(RobotQuadrant.FRONT_RIGHT, LegJointName.ANKLE_PITCH),
   HIND_RIGHT_HIP_YAW(RobotQuadrant.HIND_RIGHT, LegJointName.HIP_YAW),
   HIND_RIGHT_HIP_ROLL(RobotQuadrant.HIND_RIGHT, LegJointName.HIP_ROLL),
   HIND_RIGHT_HIP_PITCH(RobotQuadrant.HIND_RIGHT, LegJointName.HIP_PITCH),
   HIND_RIGHT_KNEE_PITCH(RobotQuadrant.HIND_RIGHT, LegJointName.KNEE_PITCH),
   HIND_RIGHT_ANKLE_ROLL(RobotQuadrant.HIND_RIGHT, LegJointName.ANKLE_ROLL),
   HIND_RIGHT_ANKLE_PITCH(RobotQuadrant.HIND_RIGHT, LegJointName.ANKLE_PITCH),
   HIND_LEFT_HIP_YAW(RobotQuadrant.HIND_LEFT, LegJointName.HIP_YAW),
   HIND_LEFT_HIP_ROLL(RobotQuadrant.HIND_LEFT, LegJointName.HIP_ROLL),
   HIND_LEFT_HIP_PITCH(RobotQuadrant.HIND_LEFT, LegJointName.HIP_PITCH),
   HIND_LEFT_KNEE_PITCH(RobotQuadrant.HIND_LEFT, LegJointName.KNEE_PITCH),
   HIND_LEFT_ANKLE_ROLL(RobotQuadrant.HIND_LEFT, LegJointName.ANKLE_ROLL),
   HIND_LEFT_ANKLE_PITCH(RobotQuadrant.HIND_LEFT, LegJointName.ANKLE_PITCH),
   ;
   
   public static final QuadrupedJointName[] values = values();
   
   private final JointRole jointRole;
   private final NeckJointName neckJointName;
   private final RobotQuadrant robotQuadrant;
   private final LegJointName legJointName;
   
   private QuadrupedJointName(NeckJointName neckJointName)
   {
      this.jointRole = JointRole.NECK;
      this.neckJointName = neckJointName;
      this.robotQuadrant = null;
      this.legJointName = null;
   }
   
   private QuadrupedJointName(RobotQuadrant robotQuadrant, LegJointName legJointName)
   {
      this.jointRole = JointRole.LEG;
      this.robotQuadrant = robotQuadrant;
      this.legJointName = legJointName;
      this.neckJointName = null;
   }
   
   public JointRole getRole()
   {
      return jointRole;
   }

   public RobotQuadrant getQuadrant()
   {
      if (jointRole.equals(JointRole.NECK))
      {
         throw new IllegalArgumentException("Neck joints do not have a quadrant");
      }
      else
      {
         return robotQuadrant;
      }
   }

   public LegJointName getLegJointName()
   {
      return legJointName;
   }

   @SuppressWarnings("incomplete-switch")
   public static QuadrupedJointName getName(RobotQuadrant quadrant, LegJointName legJointName)
   {
      switch(quadrant)
      {
      case FRONT_LEFT:
         switch(legJointName)
         {
         case HIP_YAW:
            return FRONT_LEFT_HIP_YAW;
         case HIP_ROLL:
            return FRONT_LEFT_HIP_ROLL;
         case HIP_PITCH:
            return FRONT_LEFT_HIP_PITCH;
         case KNEE_PITCH:
            return FRONT_LEFT_KNEE_PITCH;
         case ANKLE_PITCH:
            return FRONT_LEFT_ANKLE_PITCH;
         case ANKLE_ROLL:
            return FRONT_LEFT_ANKLE_ROLL;
         }
         break;
      case FRONT_RIGHT:
         switch(legJointName)
         {
         case HIP_YAW:
            return FRONT_RIGHT_HIP_YAW;
         case HIP_ROLL:
            return FRONT_RIGHT_HIP_ROLL;
         case HIP_PITCH:
            return FRONT_RIGHT_HIP_PITCH;
         case KNEE_PITCH:
            return FRONT_RIGHT_KNEE_PITCH;
         case ANKLE_PITCH:
            return FRONT_RIGHT_ANKLE_PITCH;
         case ANKLE_ROLL:
            return FRONT_RIGHT_ANKLE_ROLL;
         }
         break;
      case HIND_RIGHT:
         switch(legJointName)
         {
         case HIP_YAW:
            return HIND_RIGHT_HIP_YAW;
         case HIP_ROLL:
            return HIND_RIGHT_HIP_ROLL;
         case HIP_PITCH:
            return HIND_RIGHT_HIP_PITCH;
         case KNEE_PITCH:
            return HIND_RIGHT_KNEE_PITCH;
         case ANKLE_PITCH:
            return HIND_RIGHT_ANKLE_PITCH;
         case ANKLE_ROLL:
            return HIND_RIGHT_ANKLE_ROLL;
         }
         break;
      case HIND_LEFT:
         switch(legJointName)
         {
         case HIP_YAW:
            return HIND_LEFT_HIP_YAW;
         case HIP_ROLL:
            return HIND_LEFT_HIP_ROLL;
         case HIP_PITCH:
            return HIND_LEFT_HIP_PITCH;
         case KNEE_PITCH:
            return HIND_LEFT_KNEE_PITCH;
         case ANKLE_PITCH:
            return HIND_LEFT_ANKLE_PITCH;
         case ANKLE_ROLL:
            return HIND_LEFT_ANKLE_ROLL;
         }
         break;
      }

      throw new IllegalArgumentException("Leg joint does not exist: " + quadrant + " " + legJointName);
   }
   
   public String getUnderBarName()
   {
      if (jointRole.equals(JointRole.NECK))
      {
         return neckJointName.getUnderBarName();
      }
      else
      {
         return robotQuadrant.getUnderBarName() + "_" + legJointName.getUnderBarName();
      }
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/posePlayback/PlaybackPoseSequenceReader.java<|end_filename|>
package us.ihmc.avatar.posePlayback;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.Reader;
import java.util.ArrayList;

import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.tools.MultiBodySystemTools;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.robotics.screwTheory.ScrewTools;

public class PlaybackPoseSequenceReader
{
   public static void appendFromFile(PlaybackPoseSequence posePlaybackRobotPoseSequence, String fileName)
   {
      String fullFileName = PlaybackPoseSequenceWriter.directory + fileName;
      File file = new File(fullFileName);
      appendFromFile(posePlaybackRobotPoseSequence, file);
   }

   
   
   public static void appendFromFile(PlaybackPoseSequence posePlaybackRobotPoseSequence, InputStream selectedFile)
   {
      appendFromFile(posePlaybackRobotPoseSequence, new InputStreamReader(selectedFile));
   }
   
   public static void appendFromFile(PlaybackPoseSequence posePlaybackRobotPoseSequence, File selectedFile)
   {
      try
      {
         FileReader fr = new FileReader(selectedFile);
         appendFromFile(posePlaybackRobotPoseSequence, fr);
         fr.close();

      }
      catch (FileNotFoundException e)
      {
         e.printStackTrace();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }
   }

   public static void appendFromFile(PlaybackPoseSequence posePlaybackRobotPoseSequence, Reader reader)
   {
      FullRobotModel fullRobotModel = posePlaybackRobotPoseSequence.getFullRobotModel();

      try
      {
         BufferedReader br = new BufferedReader(reader);

         String jointNamesOnOneLine = br.readLine();
         String[] jointNamesArray = jointNamesOnOneLine.split("\\s+");
         ArrayList<String> jointNames = new ArrayList<String>();

         for (int i = 0; i < jointNamesArray.length; i++)
         {
            jointNames.add(jointNamesArray[i]);
         }

         if (!jointNames.get(0).equals("delayBeforePose"))
            throw new RuntimeException("Expecting delayBeforePose on first line. Got " + jointNames.get(0));
         jointNames.remove(0);

         if (!jointNames.get(0).equals("poseDuration"))
            throw new RuntimeException("Expecting poseDuration on first line. Got " + jointNames.get(0));
         jointNames.remove(0);

         OneDoFJointBasics[] allJoints = fullRobotModel.getOneDoFJoints();

         jointNamesArray = new String[jointNames.size()];
         jointNames.toArray(jointNamesArray);

         JointBasics[] inverseDynamicsJoints = ScrewTools.findJointsWithNames(allJoints, jointNamesArray);
         OneDoFJointBasics[] oneDoFJoints = MultiBodySystemTools.filterJoints(inverseDynamicsJoints, OneDoFJointBasics.class);
         double[] jointAngles = new double[oneDoFJoints.length];

         String textPose = br.readLine();    // read one line of text (one pose)

         while (textPose != null)    // check for end of file
         {
            String[] poseData = textPose.split("\\s+");    // separate each piece of data in one line
            PlaybackPose robotPose;

            if (oneDoFJoints.length + 2 != poseData.length)
               throw new RuntimeException("oneDoFJoints.length + 2 != poseData.length");

            double delay = Double.parseDouble(poseData[0]);
            double duration = Double.parseDouble(poseData[1]);

            for (int i = 0; i < oneDoFJoints.length; i++)
            {
               jointAngles[i] = Double.parseDouble(poseData[i + 2]);
            }

            robotPose = new PlaybackPose(oneDoFJoints, jointAngles, delay, duration);
            posePlaybackRobotPoseSequence.addPose(robotPose);

            textPose = br.readLine();    // read next pose
         }

      }
      catch (Exception ex)
      {
         ex.printStackTrace();
      }

   }

   public static PlaybackPoseSequence readFromFile(FullRobotModel fullRobotModel, String fileName)
   {
      PlaybackPoseSequence ret = new PlaybackPoseSequence(fullRobotModel);
      appendFromFile(ret, fileName);

      return ret;
   }

   public static PlaybackPoseSequence readFromInputStream(FullRobotModel fullRobotModel, InputStream inputStream)
   {
      PlaybackPoseSequence ret = new PlaybackPoseSequence(fullRobotModel);

      InputStreamReader inputStreamReader = new InputStreamReader(inputStream);
      appendFromFile(ret, inputStreamReader);
  
      return ret;
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/SliderBoardConfiguration.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public interface SliderBoardConfiguration
{
   public void activateConfiguration(SliderBoardConfigurationManager sliderBoardConfigurationManager);
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/StandReadyControllerStateFactory.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.StandReadyControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;

public class StandReadyControllerStateFactory implements HighLevelControllerStateFactory
{
   private StandReadyControllerState standReadyControllerState;

   @Override
   public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper)
   {
      if (standReadyControllerState == null)
      {
         standReadyControllerState = new StandReadyControllerState(controllerFactoryHelper.getHighLevelHumanoidControllerToolbox().getControlledOneDoFJoints(),
                                                                   controllerFactoryHelper.getHighLevelControllerParameters(),
                                                                   controllerFactoryHelper.getLowLevelControllerOutput());
      }

      return standReadyControllerState;
   }

   @Override
   public HighLevelControllerName getStateEnum()
   {
      return HighLevelControllerName.STAND_READY;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/partNames/QuadrupedJointNameMap.java<|end_filename|>
package us.ihmc.robotics.partNames;

import us.ihmc.robotics.robotSide.RobotQuadrant;

public interface QuadrupedJointNameMap extends LeggedJointNameMap<RobotQuadrant>
{
   QuadrupedJointName getJointNameForSDFName(String name);

   String getSDFNameForJointName(QuadrupedJointName name);

   String getBodyName();
   
   String getLegJointName(RobotQuadrant robotQuadrant, LegJointName legJointName);

   @Override
   default RobotQuadrant[] getRobotSegments()
   {
      return RobotQuadrant.values;
   }

   @Override
   default String getRootBodyName()
   {
      return getBodyName();
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/BooleanTuner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import javafx.application.Platform;
import javafx.beans.value.ChangeListener;
import javafx.geometry.Pos;
import javafx.scene.Node;
import javafx.scene.control.CheckBox;
import javafx.scene.layout.HBox;
import javafx.scene.text.Text;
import us.ihmc.parameterTuner.guiElements.GuiParameter;

public class BooleanTuner extends HBox implements InputNode
{
   private final CheckBox value = new CheckBox();

   public BooleanTuner(GuiParameter parameter)
   {
      setupNode();

      value.setSelected(Boolean.parseBoolean(parameter.getCurrentValue()));

      parameter.addChangedListener(p -> {
         // This listener will be triggered by an external change and is called from the animation timer.
         value.setSelected(Boolean.parseBoolean(parameter.getCurrentValue()));
      });

      value.selectedProperty().addListener((ChangeListener<Boolean>) (observable, oldValue, newValue) -> {
         Platform.runLater(() -> parameter.setValue(Boolean.toString(value.isSelected())));
      });
   }

   private void setupNode()
   {
      setSpacing(10.0);
      setAlignment(Pos.CENTER_LEFT);
      setMaxHeight(Double.NEGATIVE_INFINITY);
      setMaxWidth(Double.NEGATIVE_INFINITY);
      setPrefWidth(800.0);
      getChildren().add(new Text("Value"));
      getChildren().add(value);
   }


   @Override
   public Node getSimpleInputNode(double width, double height)
   {
      CheckBox duplicate = new CheckBox(value.getText());
      duplicate.setPrefHeight(height);
      duplicate.setPrefWidth(width);
      duplicate.setSelected(value.isSelected());
      duplicate.selectedProperty().addListener((observable, oldValue, newValue) -> value.setSelected(newValue));
      value.selectedProperty().addListener((observable, oldValue, newValue) -> duplicate.setSelected(newValue));
      return duplicate;
   }

   @Override
   public Node getFullInputNode()
   {
      return this;
   }

   @Override
   public void setValueFromPercent(double percent)
   {
      value.setSelected(percent > 0.5);
   }

   @Override
   public double getValuePercent()
   {
      return value.isSelected() ? 1.0 : 0.0;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/alphaToAlpha/MultipleSegmentConstantSlope.java<|end_filename|>
package us.ihmc.robotics.alphaToAlpha;

import us.ihmc.euclid.tuple2D.Point2D;

@SuppressWarnings("all") public class MultipleSegmentConstantSlope implements AlphaToAlphaFunction
{
   private Point2D[] segmentPoints;

   public MultipleSegmentConstantSlope(Point2D[] segmentPoints)
   {
      // slope betwen points must be positive
      for (int i = 0; i < segmentPoints.length - 1; i++)
      {
         double xDifference = segmentPoints[i + 1].getX() - segmentPoints[i].getX();
         double yDifference = segmentPoints[i + 1].getY() - segmentPoints[i].getY();
         if ((xDifference <= 0.0) || (yDifference < 0.0))
         {
            throw new RuntimeException("Slope of line must be greater than zero");
         }
      }

      this.segmentPoints = segmentPoints;
   }

   public double getAlphaPrime(double alpha)
   {
      if (alpha < 0.0)
         return segmentPoints[0].getY();

      for (int i = 0; i < segmentPoints.length - 1; i++)
      {
         if (alpha < segmentPoints[i + 1].getX())
         {
            double x0 = segmentPoints[i].getX();
            double y0 = segmentPoints[i].getY();
            double x1 = segmentPoints[i + 1].getX();
            double y1 = segmentPoints[i + 1].getY();

            double alphaPrime = y0 + (alpha - x0) / (x1 - x0) * (y1 - y0);

            return alphaPrime;
         }
      }

      return segmentPoints[segmentPoints.length - 1].getY();
   }

   public static void main(String[] args)
   {
      //    Point2d[] listOfPoints = new Point2d[]{
      //        new Point2d(0.2, 0.2),
      //        new Point2d(0.4, 0.7),
      //        new Point2d(0.5, 1.0),
      //    };

      Point2D[] listOfPoints = new Point2D[] {new Point2D(0.0, 0.0), new Point2D(0.5, 0.0), new Point2D(2.0, 1.0)};

      MultipleSegmentConstantSlope multipleSegmentConstantSlope = new MultipleSegmentConstantSlope(listOfPoints);

      for (double i = -1.0; i <= 3.0; i = i + 0.1)
      {
         System.out.println("alpha=" + i + ", alpha prime=" + multipleSegmentConstantSlope.getAlphaPrime(i));
      }

   }

   public double getMaxAlpha()
   {
      Point2D lastPoint = this.segmentPoints[segmentPoints.length];

      return lastPoint.getX();
   }

   @Override public double getDerivativeAtAlpha(double alpha)
   {

      return 0;
   }

   @Override public double getSecondDerivativeAtAlpha(double alpha)
   {

      return 0;
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasLegConfigurationParameters.java<|end_filename|>
package us.ihmc.atlas.parameters;

import us.ihmc.commonWalkingControlModules.configurations.LegConfigurationParameters;
import us.ihmc.commonWalkingControlModules.controlModules.legConfiguration.LegConfigurationGains;

public class AtlasLegConfigurationParameters extends LegConfigurationParameters
{
   private final boolean runningOnRealRobot;

   public AtlasLegConfigurationParameters(boolean runningOnRealRobot)
   {
      this.runningOnRealRobot = runningOnRealRobot;
   }

   @Override
   /** {@inheritDoc} */
   public boolean attemptToStraightenLegs()
   {
      return false;
   }

   @Override
   /** {@inheritDoc} */
   public LegConfigurationGains getBentLegGains()
   {
      LegConfigurationGains gains = new LegConfigurationGains();
      gains.setJointSpaceKp(runningOnRealRobot ? 40.0 : 150.0);
      gains.setJointSpaceKd(6.0);

      return gains;
   }

   /** {@inheritDoc} */
   @Override
   public LegConfigurationGains getStraightLegGains()
   {
      LegConfigurationGains gains = new LegConfigurationGains();
      gains.setActuatorSpaceKp(750.0);
      gains.setJointSpaceKd(runningOnRealRobot ? 3.0 : 10.0);

      return gains;
   }

   /** {@inheritDoc} */
   @Override
   public double getLegPrivilegedHighWeight()
   {
      return runningOnRealRobot ? 50.0 : 100.0;
   }

   /** {@inheritDoc} */
   @Override
   public double getFractionOfSwingToStraightenLeg()
   {
      return runningOnRealRobot ? 0.6 : 0.4;
   }

   /** {@inheritDoc} */
   @Override
   public double getKneeAngleWhenExtended()
   {
      return runningOnRealRobot ? 0.2 : 0.0;
   }

   /** {@inheritDoc} */
   @Override
   public double getKneeAngleWhenStraight()
   {
      return runningOnRealRobot ? 0.35 : 0.25;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java<|end_filename|>
package us.ihmc.robotics.math.filters;

import org.apache.commons.lang3.NotImplementedException;

import us.ihmc.euclid.transform.interfaces.Transform;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;
import us.ihmc.yoVariables.providers.DoubleProvider;

public class AlphaFilteredTuple2D implements Tuple2DBasics
{
   private final DoubleProvider alpha;

   private boolean resetX;
   private boolean resetY;

   private double x;
   private double y;

   public AlphaFilteredTuple2D(DoubleProvider alpha)
   {
      this.alpha = alpha;
      reset();
   }

   public AlphaFilteredTuple2D(Tuple2DReadOnly other, DoubleProvider alpha)
   {
      this.alpha = alpha;
      reset(other);
   }

   public void reset()
   {
      resetX = true;
      resetY = true;
   }

   public void reset(Tuple2DReadOnly other)
   {
      resetX = true;
      resetY = true;
      set(other);
   }

   @Override
   public double getX()
   {
      return x;
   }

   @Override
   public double getY()
   {
      return y;
   }

   @Override
   public void setX(double x)
   {
      if (resetX)
      {
         this.x = x;
         resetX = false;
      }
      else
      {
         this.x = alpha.getValue() * this.x + (1.0 - alpha.getValue()) * x;
      }
   }

   @Override
   public void setY(double y)
   {
      if (resetY)
      {
         this.y = y;
         resetY = false;
      }
      else
      {
         this.y = alpha.getValue() * this.y + (1.0 - alpha.getValue()) * y;
      }
   }

   @Override
   public void applyTransform(Transform transform, boolean checkIfTransformInXYPlane)
   {
      throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + ".");
   }

   @Override
   public void applyInverseTransform(Transform transform, boolean checkIfTransformInXYPlane)
   {
      throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + ".");
   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/local/IntraprocessCommunicationNetwork.java<|end_filename|>
package us.ihmc.communication.net.local;

import java.util.ArrayList;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;

import gnu.trove.map.hash.TIntObjectHashMap;
import us.ihmc.commons.thread.ThreadTools;

/* package-private */class IntraprocessCommunicationNetwork
{
   private static TIntObjectHashMap<IntraprocessCommunicator> communicators = null;

   private static void throwNotConnectedException(int port)
   {
      throw new RuntimeException("Client is not connected to port " + port + ". Make sure to call connect()");

   }

   /* package-private */ static int sendObject(IntraprocessObjectCommunicator sender, int port, Object object)
   {
      IntraprocessCommunicator communicator;
      synchronized (IntraprocessCommunicationNetwork.class)
      {
         if (communicators == null)
         {
            throwNotConnectedException(port);
         }

         communicator = communicators.get(port);
         if (communicator == null)
         {
            throwNotConnectedException(port);
         }
      }
      communicator.send(sender, object);
      return 0;
   }

   /* package-private */static void connect(IntraprocessObjectCommunicator client, int port)
   {

      synchronized (IntraprocessCommunicationNetwork.class)
      {
         IntraprocessCommunicator communicator;
         if (communicators == null)
         {
            communicators = new TIntObjectHashMap<>();
         }

         communicator = communicators.get(port);
         if (communicator == null)
         {
            communicator = new IntraprocessCommunicator(port);
            communicators.put(port, communicator);
         }
         communicator.connect(client);
      }

   }

   /* package-private */static void disconnect(IntraprocessObjectCommunicator client, int port)
   {

      synchronized (IntraprocessCommunicationNetwork.class)
      {
         if (communicators != null)
         {
            IntraprocessCommunicator communicator = communicators.get(port);
            if (communicator != null)
            {
               communicator.disconnect(client);
               if (!communicator.hasClients())
               {
                  communicators.remove(port);
               }
               if (communicators.size() == 0)
               {
                  communicators = null;
               }
            }
         }

      }
   }

   /* package-private */static synchronized boolean isConnected(IntraprocessObjectCommunicator client, int port)
   {
      IntraprocessCommunicator communicator;

      synchronized (IntraprocessCommunicationNetwork.class)
      {
         if (communicators != null)
         {
            communicator = communicators.get(port);
            if (communicator == null)
            {
               return false;
            }
         }
         else
         {
            return false;
         }
      }

      return communicator.isConnected(client);
   }

   private static class IntraprocessCommunicator
   {
      private final int port;
      private final ArrayList<IntraprocessObjectCommunicator> clients = new ArrayList<>();
      private ExecutorService callBackExecutor;

      private IntraprocessCommunicator(int port)
      {
         this.port = port;
         createCallbackExecuter();
      }

      private void createCallbackExecuter()
      {
         this.callBackExecutor = Executors.newSingleThreadExecutor(ThreadTools.getNamedThreadFactory("IntraprocessCommunicatorCallback-" + port));
      }

      private synchronized boolean hasClients()
      {
         return clients.size() != 0;
      }

      private synchronized void connect(IntraprocessObjectCommunicator client)
      {
         clients.add(client);
         client.connected();
         
         if(callBackExecutor.isShutdown())
         {
            createCallbackExecuter();
         }
      }

      private synchronized void disconnect(IntraprocessObjectCommunicator client)
      {
         clients.remove(client);
         client.disconnected();

         callBackExecutor.shutdownNow();
      }

      private synchronized boolean isConnected(IntraprocessObjectCommunicator client)
      {
         for (int i = 0; i < clients.size(); i++)
         {
            if (clients.get(i) == client)
            {
               return true;
            }
         }

         return false;
      }

      private synchronized void send(IntraprocessObjectCommunicator sender, final Object object)
      {
         if (isConnected(sender))
         {
            for (int i = 0; i < clients.size(); i++)
            {
               final IntraprocessObjectCommunicator client = clients.get(i);
               if (client != sender)
               {
                  final Object copy = sender.copyPacket(object);
                  
                  callBackExecutor.execute(new Runnable()
                  {
                     @Override
                     public void run()
                     {
                        client.receiveObject(copy);
                     }
                  });
               }
            }
         }
         else
         {
            throwNotConnectedException(port);
         }
      }
   }

   // Helpers for test classes
   /* package-private */synchronized static int getOpenPorts()
   {
      if (communicators == null)
      {
         return 0;
      }
      else
      {
         return communicators.size();
      }
   }

   /* package-private */synchronized static boolean hasMap()
   {
      return communicators != null;
   }

   /* package-private */synchronized static void closeAllConnectionsForMyJUnitTests()
   {
      communicators = null;
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/communication/GlobalObjectConsumer.java<|end_filename|>
package us.ihmc.humanoidBehaviors.communication;

import us.ihmc.communication.packetCommunicator.interfaces.GlobalPacketConsumer;
import us.ihmc.communication.packets.Packet;

public class GlobalObjectConsumer implements GlobalPacketConsumer
{
   private final CommunicationBridgeInterface communicationBridgeInterface;
   public GlobalObjectConsumer(CommunicationBridgeInterface communicationBridgeInterface)
   {
      this.communicationBridgeInterface = communicationBridgeInterface;
   }

   @Override
   public void receivedPacket(Packet<?> packet)
   {
      communicationBridgeInterface.consumeObjectFromNetwork(packet);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/RequestStereoPointCloudMessage.java<|end_filename|>
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;

/**
       * Use this message to request a new point cloud from the stereo camera.
       */
public class RequestStereoPointCloudMessage extends Packet<RequestStereoPointCloudMessage> implements Settable<RequestStereoPointCloudMessage>, EpsilonComparable<RequestStereoPointCloudMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;

   public RequestStereoPointCloudMessage()
   {
   }

   public RequestStereoPointCloudMessage(RequestStereoPointCloudMessage other)
   {
      this();
      set(other);
   }

   public void set(RequestStereoPointCloudMessage 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<RequestStereoPointCloudMessagePubSubType> getPubSubType()
   {
      return RequestStereoPointCloudMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return RequestStereoPointCloudMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(RequestStereoPointCloudMessage 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 RequestStereoPointCloudMessage)) return false;

      RequestStereoPointCloudMessage otherMyClass = (RequestStereoPointCloudMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("RequestStereoPointCloudMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-communication/src/test/java/us/ihmc/communication/streamingData/SimpleStreamingDataProducer.java<|end_filename|>
package us.ihmc.communication.streamingData;

import java.util.ArrayList;

import us.ihmc.commons.thread.ThreadTools;


public class SimpleStreamingDataProducer implements StreamingDataProducer
{
   private ArrayList<StreamingDataConsumer> consumers = new ArrayList<StreamingDataConsumer>();
   private final long dataIdentifier;
   
   public SimpleStreamingDataProducer()
   {
      this(SimpleStreamedData.getSerialVersionUID());
   }
   
   public SimpleStreamingDataProducer(long dataIdentifier)
   {
      this.dataIdentifier = dataIdentifier;
   }
   
   public void registerConsumer(StreamingDataConsumer consumer)
   {
      consumers.add(consumer);
   }
   
   public void notifyConsumers(SimpleStreamedData dataObject)
   {
      for (StreamingDataConsumer consumer : consumers)
      {
         consumer.consume(getDataIdentifier(), dataObject);
      }
   }
   
   public void startProducingData()
   {
      Runnable runnable = new Runnable()
      {
         private int value = 0;
         
         public void run()
         {
            while(true)
            {
               SimpleStreamedData data = new SimpleStreamedData(value++);
//               System.out.println("Notifying consumers " + data.getValue());
               notifyConsumers(data);
               ThreadTools.sleep(10L);
            } 
         } 
      };
      
      Thread thread = new Thread(runnable);
      thread.setDaemon(true);
      thread.start();
   }

   public long getDataIdentifier()
   {
      return dataIdentifier;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/RequestWristForceSensorCalibrationPacket.java<|end_filename|>
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 will request a calibration the wrist force sensors to the IHMC controller (does not do hardware calibration).
       * It is strongly suggested to perform the calibration when the hands are not moving nor interacting with the environment.
       */
public class RequestWristForceSensorCalibrationPacket extends Packet<RequestWristForceSensorCalibrationPacket> implements Settable<RequestWristForceSensorCalibrationPacket>, EpsilonComparable<RequestWristForceSensorCalibrationPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;

   public RequestWristForceSensorCalibrationPacket()
   {
   }

   public RequestWristForceSensorCalibrationPacket(RequestWristForceSensorCalibrationPacket other)
   {
      this();
      set(other);
   }

   public void set(RequestWristForceSensorCalibrationPacket 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<RequestWristForceSensorCalibrationPacketPubSubType> getPubSubType()
   {
      return RequestWristForceSensorCalibrationPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return RequestWristForceSensorCalibrationPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(RequestWristForceSensorCalibrationPacket 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 RequestWristForceSensorCalibrationPacket)) return false;

      RequestWristForceSensorCalibrationPacket otherMyClass = (RequestWristForceSensorCalibrationPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("RequestWristForceSensorCalibrationPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/core/StateChangedListener.java<|end_filename|>
package us.ihmc.robotics.stateMachine.core;

/**
 * Listener interface to use with {@link StateMachine} for getting notified of state changes.
 * 
 * 
 * @author Sylvain
 *
 * @param <K> Type of {@link Enum} that lists the potential states and that is used by the state
 *           machine this listener is to be added to.
 */
public interface StateChangedListener<K>
{
   /**
    * Invoked when the active state is changing.
    * 
    * @param from key of the previous active state.
    * @param to key of the new active state.
    */
   void stateChanged(K from, K to);
}


<|start_filename|>websitedocs/website/node_modules/docusaurus/lib/core/routing.js<|end_filename|>
/**
 * Copyright (c) 2017-present, Facebook, Inc.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */
const escape = require('escape-string-regexp');

function blogRouting(baseUrl) {
  return new RegExp(`^${escape(baseUrl)}blog\/.*html$`);
}

function docsRouting(baseUrl) {
  return new RegExp(`^${escape(baseUrl)}docs\/.*html$`);
}

function dotRouting() {
  return /(?!.*html$)^\/.*\.[^\n\/]+$/;
}

function feedRouting(baseUrl) {
  return new RegExp(`^${escape(baseUrl)}blog\/(feed\.xml|atom\.xml)$`);
}

function noExtRouting() {
  return /\/[^\.]*\/?$/;
}

function pageRouting(baseUrl) {
  const gr = regex => regex.toString().replace(/(^\/|\/$)/gm, '');
  return new RegExp(
    `(?!${gr(docsRouting(baseUrl))}|${gr(blogRouting(baseUrl))})^${escape(
      baseUrl
    )}.*\.html$`
  );
}

function sitemapRouting(baseUrl) {
  return new RegExp(`^${escape(baseUrl)}sitemap.xml$`);
}

module.exports = {
  blogRouting,
  docsRouting,
  dotRouting,
  feedRouting,
  pageRouting,
  noExtRouting,
  sitemapRouting,
};


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/GuiParameter.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements;

import java.util.ArrayList;
import java.util.List;

import gnu.trove.map.TObjectIntMap;
import javafx.beans.property.SimpleStringProperty;
import javafx.beans.property.StringProperty;
import us.ihmc.yoVariables.parameters.ParameterLoadStatus;

public class GuiParameter extends GuiElement
{
   private final String type;
   private final TObjectIntMap<String> valueOptions;

   private final StringProperty value = new SimpleStringProperty();
   private final StringProperty min = new SimpleStringProperty();
   private final StringProperty max = new SimpleStringProperty();
   private final StringProperty description = new SimpleStringProperty();

   private GuiParameter resetState;
   private GuiParameterStatus status;

   private final List<ParameterChangeListener> listeners = new ArrayList<>();

   public GuiParameter(String name, String type, GuiRegistry parent)
   {
      this(name, type, null, parent);
   }

   public GuiParameter(String name, String type, TObjectIntMap<String> valueOptions, GuiRegistry parent)
   {
      super(name, parent);
      this.type = type;
      this.valueOptions = valueOptions;
   }

   /**
    * This constructor creates a "shallow" copy of the provided parameter. The new parameter
    * will not have a parent and {@link #getParent()} will return {@code null}. However, the
    * unique name of the parameter {@link #getUniqueName()} will include the full namespace
    * of the original parameter.
    */
   public GuiParameter(GuiParameter other)
   {
      super(other.getName(), null, other.getUniqueName());
      this.type = other.getType();
      this.valueOptions = other.getValueOptions();
      set(other);
   }

   public void addStatusUpdater()
   {
      // Only consider the parameter modified if the value changes for now.
      value.addListener((observable, oldValue, newValue) -> status = GuiParameterStatus.MODIFIED);
   }

   public void markAsModified()
   {
      if (status != GuiParameterStatus.MODIFIED)
      {
         status = GuiParameterStatus.MODIFIED;
         informListeners();
      }
   }

   public void setValue(String value)
   {
      if (!this.value.getValueSafe().equals(value))
      {
         this.value.set(value);
         informListeners();
      }
   }

   public void setMin(String min)
   {
      if (!this.min.getValueSafe().equals(min))
      {
         this.min.set(min);
         informListeners();
      }
   }

   public void setMax(String max)
   {
      if (!this.max.getValueSafe().equals(max))
      {
         this.max.set(max);
         informListeners();
      }
   }

   public void setDescription(String description)
   {
      if (!this.description.getValueSafe().equals(description))
      {
         this.description.set(description);
         informListeners();
      }
   }

   public void setLoadStatus(ParameterLoadStatus loadStatus)
   {
      setStatus(GuiParameterStatus.get(loadStatus));
   }

   public void setStatus(GuiParameterStatus status)
   {
      if (status != this.status)
      {
         this.status = status;
         informListeners();
      }
   }

   public String getType()
   {
      return type;
   }

   public TObjectIntMap<String> getValueOptions()
   {
      return valueOptions;
   }

   public String getCurrentValue()
   {
      return value.get();
   }

   public String getCurrentMin()
   {
      return min.get();
   }

   public String getCurrentMax()
   {
      return max.get();
   }

   public String getCurrentDescription()
   {
      return description.get();
   }

   public GuiParameterStatus getStatus()
   {
      return status;
   }

   public void removeChangeListener(ParameterChangeListener listener)
   {
      listeners.remove(listener);
   }

   public void addChangedListener(ParameterChangeListener listener)
   {
      listeners.add(listener);
   }

   private void informListeners()
   {
      listeners.stream().forEach(listener -> listener.changed(this));
   }

   public GuiParameter createCopy(GuiRegistry parent)
   {
      GuiParameter copy = new GuiParameter(getName(), getType(), getValueOptions(), parent);
      copy.set(this);
      return copy;
   }

   public void set(GuiParameter other)
   {
      value.set(other.getCurrentValue());
      min.set(other.getCurrentMin());
      max.set(other.getCurrentMax());
      description.set(other.getCurrentDescription());
      status = other.getStatus();
      informListeners();
   }

   public void setValueAndStatus(GuiParameter other)
   {
      value.set(other.getCurrentValue());
      status = other.getStatus();
      informListeners();
   }

   public void reset()
   {
      if (resetState != null && status == GuiParameterStatus.MODIFIED)
      {
         set(resetState);
      }
   }

   public void saveStateForReset()
   {
      resetState = createCopy(null);
   }

   @Override
   public String toString()
   {
      String ret = getUniqueName();
      ret += "\ntype: " + type;
      ret += "\nvalue: " + value.getValueSafe();
      return ret;
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/CompositeFootstepOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;

/**
 * Created by agrabertilton on 3/6/15.
 */
public class CompositeFootstepOverheadPath extends FootstepOverheadPath
{
   ReferenceFrame referenceFrame;
   double totalDistance = 0;
   FramePose2D endPose;
   ArrayList<FootstepOverheadPath> overheadPaths = new ArrayList<>();

   public CompositeFootstepOverheadPath(FootstepOverheadPath startingPath){
      this.referenceFrame = startingPath.getReferenceFrame();
      addPath(startingPath);
   }

   public CompositeFootstepOverheadPath(List<FootstepOverheadPath> startingPaths){
      for (FootstepOverheadPath path : startingPaths){
         if (referenceFrame == null){
            referenceFrame = path.getReferenceFrame();
         }else{
            this.checkReferenceFrameMatch(path);
         }
         addPath(path);
      }
   }

   public CompositeFootstepOverheadPath(FootstepOverheadPath[] startingPaths){
      for (FootstepOverheadPath path : startingPaths){
         if (referenceFrame == null){
            referenceFrame = path.getReferenceFrame();
         }else{
            this.checkReferenceFrameMatch(path);
         }
         addPath(path);
      }
   }

   public void addPath(FootstepOverheadPath pathToAdd){
      this.checkReferenceFrameMatch(pathToAdd);
      overheadPaths.add(pathToAdd);
      totalDistance += pathToAdd.getTotalDistance();
      endPose = pathToAdd.getPoseAtDistance(pathToAdd.getTotalDistance());
   }


   @Override
   public FramePose2D getPoseAtDistance(double distanceAlongPath)
   {
      double currentDistanceInPath = distanceAlongPath;
      if (distanceAlongPath > totalDistance){
         return endPose;
      }

      for (FootstepOverheadPath path : overheadPaths){
         if (currentDistanceInPath > path.getTotalDistance()){
            currentDistanceInPath -= path.getTotalDistance();
         }else{
            return path.getPoseAtDistance(currentDistanceInPath);
         }
      }

      //should not get here, means a numerical error somewhere
      return endPose;
   }

   @Override
   public double getTotalDistance()
   {
      return totalDistance;
   }

   @Override
   public FootstepOverheadPath changeFrameCopy(ReferenceFrame desiredFrame)
   {
      CompositeFootstepOverheadPath newPath = null;
      for (FootstepOverheadPath path : overheadPaths){
         if (newPath == null){
            newPath = new CompositeFootstepOverheadPath(path.changeFrameCopy(desiredFrame));
         }else{
            newPath.addPath(path.changeFrameCopy(desiredFrame));
         }
      }
      return newPath;
   }

   @Override
   public ReferenceFrame getReferenceFrame()
   {
      return referenceFrame;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/kinematics/InverseKinematicsCalculator.java<|end_filename|>
package us.ihmc.robotics.kinematics;

import us.ihmc.euclid.transform.RigidBodyTransform;

public interface InverseKinematicsCalculator
{
   public abstract boolean solve(RigidBodyTransform desiredTransform);
   
   public abstract double getErrorScalar();
   
   public abstract int getNumberOfIterations();

   public abstract void attachInverseKinematicsStepListener(InverseKinematicsStepListener stepListener);

   public abstract void setLimitJointAngles(boolean limitJointAngles);
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/PacketRouter.java<|end_filename|>
package us.ihmc.communication;

import java.util.EnumMap;
import java.util.HashMap;

import us.ihmc.commons.PrintTools;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packetCommunicator.interfaces.GlobalPacketConsumer;
import us.ihmc.communication.packets.Packet;
import us.ihmc.communication.packets.PacketDestination;

public class PacketRouter<T extends Enum<T>>
{
   private boolean DEBUG = false;

   private final T[] destinationConstants;
   private int sourceCommunicatorIdToDebug = Integer.MIN_VALUE; //set to Integer.MIN_VALUE to debug all sources
   private int destinationCommunicatorIdToDebug = Integer.MIN_VALUE; //set to Integer.MIN_VALUE to debug all destinations
   private String[] packetTypesToDebug = {"TextToSpeechPacket"}; //set to null to debug all packets

   private final int BROADCAST = 0;

   private final EnumMap<T, PacketCommunicator> communicators;
   private final HashMap<PacketCommunicator, T> communicatorDestinations;

   private final EnumMap<T, GlobalPacketConsumer> consumers;
   private final EnumMap<T, T> redirects;

   public PacketRouter(Class<T> destinationType)
   {
      destinationConstants = destinationType.getEnumConstants();
      communicators = new EnumMap<>(destinationType);
      consumers = new EnumMap<>(destinationType);
      communicatorDestinations = new HashMap<>();
      redirects = new EnumMap<>(destinationType);

      PrintTools.debug(DEBUG, this, "Creating Packet Router");
   }

   public void attachPacketCommunicator(T destination, final PacketCommunicator packetCommunicator)
   {
      checkCommunicatorId(destination);

      // check if this communicator already points to the other destination
      GlobalPacketConsumer packetRoutingAction = null;
      T otherDestination = communicatorDestinations.get(packetCommunicator);

      if (otherDestination != null)
      {
         packetRoutingAction = consumers.get(otherDestination);
         // this listener is already attached to that communicator
      }
      else
      {
         packetRoutingAction = new PacketRoutingAction(packetCommunicator);
         packetCommunicator.attachGlobalListener(packetRoutingAction);
      }

      consumers.put(destination, packetRoutingAction);
      communicators.put(destination, packetCommunicator);
      communicatorDestinations.put(packetCommunicator, destination);

      PrintTools.debug(DEBUG, this, "Attached " + destination + " to the network processor");
   }

   private void checkCommunicatorId(final T destination)
   {
      if (isBroadcast(destination))
      {
         throw new IllegalArgumentException("packetCommunicator cannot have an id of zero, it's reserved for broadcast!!");
      }
      if (communicators.containsKey(destination))
      {
         throw new IllegalArgumentException("Tried to register " + destination + " but already registerd communicator with that id!");
      }
   }

   /**
    * will send to the destination of or if a redirect is set, it will
    * forward to the redirect.
    * If a the redirect happens to be set to the senders id it will assume 
    * the sender new about the redirect and send to the original destination,  
    * ignoring the redirect
    * @param sourceCommunicator the source communicator that sent the packet
    * @param packet
    */
   private void processPacketRouting(PacketCommunicator sourceCommunicator, Packet<?> packet)
   {
      T source = communicatorDestinations.get(sourceCommunicator);

      if (shouldPrintDebugStatement(sourceCommunicator, packet.getDestination(), packet))
      {
         PrintTools.debug(this, "NP received " + packet.getClass().getSimpleName() + " heading for " + destinationConstants[packet.destination] + " from "
               + source + " at " + System.nanoTime());
      }

      T destination = getPacketDestination(sourceCommunicator, packet);

      packet.setSource(source.ordinal());

      PacketCommunicator destinationCommunicator = communicators.get(destination);
      if (isBroadcast(destination))
      {
         broadcastPacket(sourceCommunicator, packet);
      }
      else if (destinationCommunicator != null && destinationCommunicator.isConnected())
      {
         if (shouldPrintDebugStatement(sourceCommunicator, destination.ordinal(), packet))
         {
            PrintTools.debug(this, "Sending " + packet.getClass().getSimpleName() + " from " + source + " to " + destination + " at " + System.nanoTime());
         }

         forwardPacket(packet, destinationCommunicator);
      }

   }

   private boolean isBroadcast(T destination)
   {
      return destination.ordinal() == BROADCAST;
   }

   private void forwardPacket(Packet<?> packet, PacketCommunicator destinationCommunicator)
   {
      destinationCommunicator.send(packet);
   }

   private T getPacketDestination(PacketCommunicator sourceCommunicator, Packet<?> packet)
   {
      if (packet.getDestination() < 0 || packet.getDestination() >= destinationConstants.length)
      {
         PrintTools.error(this, packet.getClass().getSimpleName() + " from: " + PacketDestination.fromOrdinal(packet.getSource()) + " Invalid destination: "
               + packet.getDestination() + " sending to Broadcast");
         return destinationConstants[0];
      }

      T destination = destinationConstants[packet.getDestination()];
      if (redirects.containsKey(destination))
      {
         destination = getRedirectDestination(destination);
         if (destination != communicatorDestinations.get(sourceCommunicator))
         {
            packet.setDestination(destination.ordinal());
         }
      }
      return destinationConstants[packet.getDestination()];
   }

   /**
    * sends the packet to every communicator once, except the sender or any
    * communicators with redirects
   **/
   private void broadcastPacket(PacketCommunicator sourceCommunicator, Packet<?> packet)
   {
      for (T destination : destinationConstants)
      {
         if (isBroadcast(destination) || !communicators.containsKey(destination))
         {
            continue;
         }

         PacketCommunicator destinationCommunicator = communicators.get(destination);
         if (sourceCommunicator != destinationCommunicator && !redirects.containsKey(destination))
         {
            if (destinationCommunicator != null && destinationCommunicator.isConnected())
            {
               if (shouldPrintDebugStatement(sourceCommunicator, destination.ordinal(), packet))
               {
                  PrintTools.debug(this, "Sending " + packet.getClass().getSimpleName() + " from " + communicatorDestinations.get(sourceCommunicator) + " to "
                        + destination + " at " + System.nanoTime());
               }
               forwardPacket(packet, destinationCommunicator);
            }
         }
      }
   }

   /**
    * possible infinite recursion for funs.
    */
   private T getRedirectDestination(T destination)
   {
      if (redirects.containsKey(destination))
      {
         destination = getRedirectDestination(redirects.get(destination));
      }
      return destination;
   }

   public void detatchObjectCommunicator(T id)
   {
      PacketCommunicator communicator = communicators.remove(id);
      GlobalPacketConsumer consumer = consumers.remove(id);

      if (communicator != null)
      {
         communicator.detachGlobalListener(consumer);
      }

      redirects.remove(id);

      for (T destination : destinationConstants)
      {
         if (redirects.get(destination) == id)
         {
            redirects.remove(destination);
         }
      }
   }

   public void setPacketRedirects(T redirectFrom, T redirectTo)
   {
      if (redirects.containsValue(redirectFrom))
      {
         throw new IllegalArgumentException(redirectTo + " is currently redirecting and can't be chained");
      }
      redirects.put(redirectFrom, redirectTo);
   }

   public void removePacketRedirect(int redirectFrom)
   {
      redirects.remove(redirectFrom);
   }

   private boolean shouldPrintDebugStatement(PacketCommunicator sourceCommunicator, int destinationCommunicatorId, Packet<?> packetType)
   {
      if (!DEBUG)
         return false;

      if (sourceCommunicatorIdToDebug != Integer.MIN_VALUE && sourceCommunicatorIdToDebug != communicatorDestinations.get(sourceCommunicator).ordinal())
      {
         return false;
      }

      if (destinationCommunicatorIdToDebug != Integer.MIN_VALUE && destinationCommunicatorIdToDebug != destinationCommunicatorId)
      {
         return false;
      }

      if (packetTypesToDebug != null)
      {
         for (int i = 0; i < packetTypesToDebug.length; i++)
         {
            if (packetTypesToDebug[i].equals(packetType.getClass().getSimpleName()))
            {
               return true;
            }
         }
         return false;
      }
      return true;
   }

   // Put these here so they are not final, I can change these debug variables at runtime.
   @SuppressWarnings("unused")
   private void setDEBUG(boolean debug)
   {
      DEBUG = debug;
   }

   public void setSourceCommunicatorIdToDebug(int sourceCommunicatorIdToDebug)
   {
      this.sourceCommunicatorIdToDebug = sourceCommunicatorIdToDebug;
   }

   public void setDestinationCommunicatorIdToDebug(int destinationCommunicatorIdToDebug)
   {
      this.destinationCommunicatorIdToDebug = destinationCommunicatorIdToDebug;
   }

   private class PacketRoutingAction implements GlobalPacketConsumer
   {
      private final PacketCommunicator communicator;

      private PacketRoutingAction(PacketCommunicator packetCommunicator)
      {
         this.communicator = packetCommunicator;
      }

      @Override
      public void receivedPacket(Packet<?> packet)
      {
         processPacketRouting(communicator, packet);
      }
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/DynamicReachabilityParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

public class DynamicReachabilityParameters
{
   /**
    * This is the change in height during the step which signifies stepping up.
    * When stepping up, the leading leg is allowed to bend more.
    *
    * @return step up (m).
    */
   public double getThresholdForStepUp()
   {
      return 0.10;
   }

   /**
    * This is the change in height during the step which signifies stepping down.
    * When stepping down, the trailing leg is allowed to bend more.
    *
    * @return step down (m).
    */
   public double getThresholdForStepDown()
   {
      return -0.10;
   }

   /**
    * This is the maximum desired knee bend that we try to adjust the dynamic plan to achieve.
    *
    * @return knee bend (rad).
    */
   public double getMaximumDesiredKneeBend()
   {
      return 0.25;
   }

   /**
    * This is the maximum number of adjustments that the solver attempts to achieve the desired adjustment.
    *
    * @return number of adjustments
    */
   public int getMaximumNumberOfCoMAdjustments()
   {
      return 3;
   }

   /**
    * This sets whether or not the solver attempts to use "higher order" steps. These tends to have
    * very small gradients, so they are typically not used.
    *
    * @return whether or not to use higher order steps.
    */
   public boolean useHigherOrderSteps()
   {
      return false;
   }

   /**
    * This is the percent of the transfer duration that we modify the ICP plan by to calculate the gradient.
    *
    * @return percent (0.0 to 1.0)
    */
   public double getPercentOfTransferDurationToCalculateGradient()
   {
      return 0.4;
   }

   /**
    * This is the percent of the swing duration that we modify the ICP plan by to calculate the gradient.
    *
    * @return percent (0.0 to 1.0)
    */
   public double getPercentOfSwingDurationToCalculateGradient()
   {
      return 0.4;
   }

   /**
    * This is the additional adjustment added on to the required adjustment, as a percentage of the
    * width of the reachable region.
    *
    * @return percent (0.0 to 1.0)
    */
   public double getRequiredAdjustmentSafetyFactor()
   {
      return 0.0;
   }

   /**
    * This gain modified the required adjustment used by the solver based on the achieved adjustment and the
    * actual desired adjustment.
    *
    * @return feedback gain.
    */
   public double getRequiredAdjustmentFeedbackGain()
   {
      return 0.1;
   }

   /**
    * Minimum allowable transfer duration computed by the solver.
    */
   public double getMinimumTransferDuration()
   {
      return 0.20;
   }

   /**
    * Maximum allowable transfer duration computed by the solver.
    */
   public double getMaximumTransferDuration()
   {
      return 5.0;
   }

   /**
    * Minimum allowable swing duration computed by the solver.
    */
   public double getMinimumSwingDuration()
   {
      return 0.4;
   }

   /**
    * Maximum allowable swing duration computed by the solver.
    */
   public double getMaximumSwingDuration()
   {
      return 10.0;
   }

   /**
    * Minimum allowable initial transfer duration computed by the solver.
    */
   public double getMinimumInitialTransferDuration()
   {
      return 0.4 * getMinimumTransferDuration();
   }

   /**
    * Minimum allowable ending transfer duration computed by the solver.
    */
   public double getMinimumEndTransferDuration()
   {
      return 0.08;
   }

   /**
    * Minimum allowable initial swing duration computed by the solver.
    */
   public double getMinimumInitialSwingDuration()
   {
      return 0.15;
   }

   /**
    * Minimum allowable ending swing duration computed by the solver.
    */
   public double getMinimumEndSwingDuration()
   {
      return 0.15;
   }

   /**
    * This is the weight placed on achieving the desired adjustment in the QP Solver.
    */
   public double getConstraintWeight()
   {
      return 10000.0;
   }

   /**
    * This weight penalizes adjustments perpendicular to the required adjustment to achieved dynamic feasibility.
    * That is, it penalizes adjustment of the CoM position perpendicular to the stepping direction.
    */
   public double getPerpendicularWeight()
   {
      return 0.1;
   }

   /**
    * This is the weight penalizing adjusting the swing entry and exit duration in the solver.
    */
   public double getSwingAdjustmentWeight()
   {
      return 20.0;
   }

   /**
    * This is the weight penalizing adjusting the transfer entry and exit duration in the solver.
    */
   public double getTransferAdjustmentWeight()
   {
      return 1.0;
   }

   /**
    * This is the weight penalizing adjusting the swing duration in the solver.
    */
   public double getTotalSwingAdjustmentWeight()
   {
      return 0.1;
   }

   /**
    * This is the weight penalizing adjusting the transfer duration in the solver.
    */
   public double getTotalTransferAdjustmentWeight()
   {
      return 0.01;
   }

   public double getTransferEqualAdjustmentWeight()
   {
      return 50.0;
   }

   public double getSwingEqualAdjustmentWeight()
   {
      return 5.0;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/DecayingResolutionFilter.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import java.util.ArrayList;
import java.util.LinkedList;

import gnu.trove.map.hash.TLongObjectHashMap;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Point3D32;
import us.ihmc.euclid.tuple3D.Vector3D32;

public class DecayingResolutionFilter
{
   private double resolution;
   private long decayMillis;
   int capacity = 100000;

   private final TLongObjectHashMap<TimestampedPoint> map;
   //   private final HashMap<Long, TimestampedPoint> map;
   private final LinkedList<TimestampedPoint> list;

   public DecayingResolutionFilter(double resolution, long decayMillis, int maxSizeAllowed)
   {
      this.resolution = resolution;
      this.decayMillis = decayMillis;
      this.capacity = maxSizeAllowed;

      map = new TLongObjectHashMap<TimestampedPoint>();
      list = new LinkedList<TimestampedPoint>();
   }

   public boolean add(Point3D p)
   {
      return add(p.getX(), p.getY(), p.getZ());
   }

   public boolean add(double x, double y, double z)
   {
      return add((float) x, (float) y, (float) z);
   }

   public boolean add(float x, float y, float z)
   {
      long hash = hash(x, y, z, resolution);

      removeDecayedPoints();
      
      if (!map.containsKey(hash))
      {
         TimestampedPoint point = new TimestampedPoint(x, y, z, System.currentTimeMillis());
         synchronized (list)
         {
            atomicAddToMapAndList(hash, point);
         }
         return true;
      }

      return false;
   }

   private synchronized void atomicAddToMapAndList(long hash, TimestampedPoint point)
   {
      map.put(hash, point);
      list.addLast(point);
   }

   private void removeDecayedPoints()
   {
      TimestampedPoint timePoint;

      synchronized (list)
      {
         if (list.size() >= capacity && capacity > 0)
         {
            atomicRemoveFromListAndMap();
         }
      }

      long time = System.currentTimeMillis();
      while ((timePoint = list.peekFirst()) != null)
      {
         if (timePoint.timestamp + decayMillis > time || decayMillis<0)
         {
            return;
         }

         synchronized (list)
         {
            atomicRemoveFromListAndMap();
         }
      }
   }

   private synchronized void atomicRemoveFromListAndMap()
   {
      TimestampedPoint timePoint;
      timePoint = list.removeFirst();
      map.remove(hash(timePoint));
   }

   //================================================================================
   // Private Utility Functions
   //================================================================================

   private long hash(TimestampedPoint point)
   {
      return hash(point.getX32(), point.getY32(), point.getZ32(), resolution);
   }

   private static final int bits = 20;
   private static final int mask = 0xFFFFFFFF >>> bits;
   private static final int offset = 1 << (bits - 1);

   private long hash(float x, float y, float z, double res)
   {
      long hash = 0;
      hash += (((int) (x / res) + offset) & mask) << 2 * bits;
      hash += (((int) (y / res) + offset) & mask) << 1 * bits;
      hash += (((int) (z / res) + offset) & mask) << 0 * bits;

      return hash;
   }

   //================================================================================
   // Setters
   //================================================================================

   public void clear()
   {
      synchronized (list)
      {
         map.clear();
         list.clear();
      }
   }

   public void setResolution(double resolution)
   {
      this.resolution = resolution;
   }

   public void setDecay(long decayMillis)
   {
      this.decayMillis = decayMillis;
   }

   public void setCapacity(int capacity)
   {
      this.capacity = capacity;
   }

   //================================================================================
   // Getters
   //================================================================================

   /**
    * Get the points in a new array. The points them self are not copied.
    * @return new Point3f array
    */
   public Point3D32[] getPoints3f()
   {
      synchronized (list)
      {
         Point3D32[] points = new Point3D32[list.size()];

         for (int i = 0; i < list.size(); i++)
         {
            points[i] = list.get(i);
         }
         
         return points;
      }
   }

   /**
    * Unsafe when reading/writing from different threads, but it is quicker.
    * @return
    */
   public Point3D32[] getPoints3fUnsynchronized()
   {
      return list.toArray(new Point3D32[0]);
   }

   public ArrayList<TimestampedPoint> getPointsCopy()
   {
      // Thread-safely copies points into a new array
      ArrayList<TimestampedPoint> copy;

      synchronized (list)
      {
         copy = new ArrayList<TimestampedPoint>(list);
      }

      return copy;
   }

   public Point3D32 getNearestIntersection(Point3D32 origin, Vector3D32 direction)
   {
      ArrayList<TimestampedPoint> points = getPointsCopy();
      direction.normalize();
      float dx, dy, dz, dot;
      double distanceToLine, distance;

      double nearestDistance = Double.POSITIVE_INFINITY;
      Point3D32 nearestPoint = null;

      for (Point3D32 p : points)
      {
         dx = origin.getX32() - p.getX32();
         dy = origin.getY32() - p.getY32();
         dz = origin.getZ32() - p.getZ32();

         dot = dx * direction.getX32() + dy * direction.getY32() + dz * direction.getZ32();

         dx = dx - dot * direction.getX32();
         dy = dy - dot * direction.getY32();
         dz = dz - dot * direction.getZ32();

         distanceToLine = Math.sqrt(dx * dx + dy * dy + dz * dz);

         if (distanceToLine < resolution / 2)
         {
            distance = origin.distance(p);
            if (distance < nearestDistance)
            {
               nearestDistance = distance;
               nearestPoint = p;
            }
         }
      }

      return nearestPoint;
   }
}

<|start_filename|>ihmc-ros-tools/generated-src/ihmc_msgs/DesiredAccelerationsRosMessage.java<|end_filename|>
package ihmc_msgs;

public interface DesiredAccelerationsRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_msgs/DesiredAccelerationsRosMessage";
  static final java.lang.String _DEFINITION = "## DesiredAccelerationsRosMessage\n# \n\n# Specifies the desired joint accelerations.\nfloat64[] desired_joint_accelerations\n\n# Properties for queueing trajectories.\nihmc_msgs/QueueableRosMessage queueing_properties\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  double[] getDesiredJointAccelerations();
  void setDesiredJointAccelerations(double[] value);
  ihmc_msgs.QueueableRosMessage getQueueingProperties();
  void setQueueingProperties(ihmc_msgs.QueueableRosMessage value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/GoHomeTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.GoHomeMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.GoHomeBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class GoHomeTask extends BehaviorAction
{
   private final GoHomeMessage goHomeMessage;
   private final GoHomeBehavior goHomeBehavior;

   public GoHomeTask(GoHomeMessage goHomeMessage, GoHomeBehavior goHomeBehavior)
   {
      super(goHomeBehavior);
      this.goHomeBehavior = goHomeBehavior;
      this.goHomeMessage = goHomeMessage;
   }

   @Override
   protected void setBehaviorInput()
   {
      goHomeBehavior.setInput(goHomeMessage);
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/IntegerSpinner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import javafx.scene.control.SpinnerValueFactory.IntegerSpinnerValueFactory;
import us.ihmc.commons.PrintTools;

public class IntegerSpinner extends NumericSpinner<Integer>
{
   public IntegerSpinner()
   {
      super(new IntegerSpinnerValueFactory(Integer.MIN_VALUE, Integer.MAX_VALUE, 0));
   }

   @Override
   public Integer convertStringToNumber(String numberString)
   {
      if (numberString == null)
      {
         return 0;
      }
      try
      {
         return Integer.parseInt(numberString);
      }
      catch (NumberFormatException e)
      {
         PrintTools.warn("Invalid integer string for IntegerParameter. Attempting to parse as double...");
         double doubleValue = Double.parseDouble(numberString);
         int intValue = (int) doubleValue;

         if (doubleValue != intValue)
         {
            throw new RuntimeException("Integer parameter has floating point precision. Fix your file!");
         }

         return intValue;
      }
   }

   @Override
   public String convertNumberToString(Integer number)
   {
      return Integer.toString(number);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/DoDisconnectListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication;

public interface DoDisconnectListener
{
   public abstract void doDisconnect();
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/pointClouds/shape/CubeCalibration.java<|end_filename|>
package us.ihmc.sensorProcessing.pointClouds.shape;

import java.util.List;

import georegression.metric.Intersection3D_F64;
import georegression.struct.line.LineParametric3D_F64;
import georegression.struct.plane.PlaneGeneral3D_F64;
import georegression.struct.point.Point3D_F64;

public class CubeCalibration
{
   public static void orient(List<PlaneGeneral3D_F64> planes)
   {
      if (planes.size() < 3)
         return;

      LineParametric3D_F64 line = new LineParametric3D_F64();
      Point3D_F64 point = new Point3D_F64();
      Intersection3D_F64.intersect(planes.get(0), planes.get(1), line);
      Intersection3D_F64.intersect(planes.get(2), line, point);

      System.out.println(point);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/FastFourierTransform.java<|end_filename|>
package us.ihmc.robotics.math;

import java.util.List;

import us.ihmc.robotics.dataStructures.ComplexNumber;

/**
 * Garbage free implementation of the FFT algorithm
 */
public class FastFourierTransform
{
   private static final double logTwo = Math.log(2.0);
   /**
    * standard FFT object available for all to use
    */
   public static final FastFourierTransform fft = new FastFourierTransform(8);

   public static FastFourierTransform getFourierTransformer()
   {
      return fft;
   }

   private int maxNumberOfCoefficients;
   private int logMaxNumberOfCoefficients;
   private int numberOfCoefficients;
   private List<ComplexNumber> rootsOfUnity;
   private ComplexNumber[] coefficients;
   private ComplexNumber[] transformedCoeffs;

   public FastFourierTransform(int maxNumberOfCoefficients)
   {
      this.logMaxNumberOfCoefficients = (int) Math.ceil(Math.log(maxNumberOfCoefficients) / logTwo);
      this.maxNumberOfCoefficients = (int) Math.pow(2, logMaxNumberOfCoefficients);
      this.rootsOfUnity = RootsOfUnity.getRootsOfUnity(this.maxNumberOfCoefficients);
      this.coefficients = new ComplexNumber[this.maxNumberOfCoefficients];
      this.transformedCoeffs = new ComplexNumber[this.maxNumberOfCoefficients];
      for (int i = 0; i < this.maxNumberOfCoefficients; i++)
      {
         coefficients[i] = new ComplexNumber();
         transformedCoeffs[i] = new ComplexNumber();
      }
   }

   public void setCoefficients(ComplexNumber[] coefficients)
   {
      numberOfCoefficients = coefficients.length;
      if (numberOfCoefficients > maxNumberOfCoefficients)
         throw new RuntimeException("Insufficient number of coefficients for FFT transform, max: " + maxNumberOfCoefficients + ", provided: "
               + numberOfCoefficients);
      int index = 0;
      for (; index < numberOfCoefficients; index++)
      {
         this.coefficients[index].set(coefficients[index]);
      }
      for (; index < maxNumberOfCoefficients; index++)
      {
         this.coefficients[index].setToPurelyReal(0.0);
      }
   }

   public void setCoefficients(double[] coefficients)
   {
      numberOfCoefficients = coefficients.length;
      if (numberOfCoefficients > maxNumberOfCoefficients)
         throw new RuntimeException("Insufficient number of coefficients for FFT transform, max: " + maxNumberOfCoefficients + ", provided: "
               + numberOfCoefficients);
      int index = 0;
      for (; index < numberOfCoefficients; index++)
      {
         this.coefficients[index].setToPurelyReal(coefficients[index]);
      }
      for (; index < maxNumberOfCoefficients; index++)
      {
         this.coefficients[index].setToPurelyReal(0.0);
      }
   }

   public void setCoefficients(double[] coefficients, int numberOfCoefficientsToUse)
   {
      numberOfCoefficients = numberOfCoefficientsToUse;
      if (numberOfCoefficientsToUse > maxNumberOfCoefficients)
         throw new RuntimeException("Insufficient number of coefficients for FFT transform, max: " + maxNumberOfCoefficients + ", provided: "
               + numberOfCoefficientsToUse);
      int index = 0;
      for (; index < numberOfCoefficientsToUse; index++)
      {
         this.coefficients[index].setToPurelyReal(coefficients[index]);
      }
      for (; index < maxNumberOfCoefficients; index++)
      {
         this.coefficients[index].setToPurelyReal(0.0);
      }
   }

   private ComplexNumber tempComplex1 = new ComplexNumber(), tempComplex2 = new ComplexNumber(), tempComplex = new ComplexNumber();

   public ComplexNumber[] getForwardTransform()
   {
      transform(false);
      return transformedCoeffs;
   }

   public ComplexNumber[] getInverseTransform()
   {
      transform(true);
      return transformedCoeffs;
   }

   private void transform(boolean inverse)
   {
      bitReverseCopy(transformedCoeffs, coefficients);

      int m = 1;

      for (int i = 1; i <= logMaxNumberOfCoefficients; i++)
      {
         int half_m = m;
         m = m << 1; // Computing m = 2^i

         tempComplex.setToPurelyReal(1.0);
         for (int j = 0; j < half_m; j++)
         {
            for (int k = j; k < maxNumberOfCoefficients - 1; k += m)
            {
               tempComplex1.timesAndStore(tempComplex, transformedCoeffs[k + half_m]);
               tempComplex2.set(transformedCoeffs[k]);
               transformedCoeffs[k].plusAndStore(tempComplex1, tempComplex2);
               transformedCoeffs[k + half_m].minusAndStore(tempComplex2, tempComplex1);
            }
            if (!inverse)
               tempComplex.timesAndStore(rootsOfUnity.get(maxNumberOfCoefficients - maxNumberOfCoefficients / m));
            else
               tempComplex.timesAndStore(rootsOfUnity.get(maxNumberOfCoefficients / m));
         }
      }
      if (inverse)
      {
         for (int i = 0; i < transformedCoeffs.length; i++)
            transformedCoeffs[i].scale(1.0 / maxNumberOfCoefficients);
      }
   }

   private void bitReverseCopy(ComplexNumber[] arrayToPack, ComplexNumber[] arrayToCopy)
   {
      int temp = (int) (Math.log(arrayToCopy.length) / logTwo);
      for (int i = 0; i < arrayToCopy.length; i++)
      {
         arrayToPack[i].set(arrayToCopy[bitReverse(i, temp)]);
      }
   }

   public int bitReverse(int a, int numberOfBits)
   {
      int temp = 0;
      for (; numberOfBits > 0; numberOfBits--, temp += a % 2, a /= 2)
         temp <<= 1;
      return temp;
   }

   private void clearTransformed()
   {
      for (int i = 0; i < maxNumberOfCoefficients; i++)
         transformedCoeffs[i].set(0.0, 0.0);
   }

   private void clearInput()
   {
      for (int i = 0; i < numberOfCoefficients; i++)
         coefficients[i].set(0.0, 0.0);
   }

   public void clear()
   {
      clearTransformed();
      clearInput();
   }

   public int getMaxNumberOfCoefficients()
   {
      return maxNumberOfCoefficients;
   }

   public List<ComplexNumber> getRootsOfUnity()
   {
      return rootsOfUnity;
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/genericQuadruped/model/GenericQuadrupedOrderedJointMap.java<|end_filename|>
package us.ihmc.exampleSimulations.genericQuadruped.model;

import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.robotSide.RobotQuadrant;

public enum GenericQuadrupedOrderedJointMap
{
   FRONT_LEFT_HIP_ROLL,
   FRONT_LEFT_HIP_PITCH,
   FRONT_LEFT_KNEE_PITCH,
   FRONT_RIGHT_HIP_ROLL,
   FRONT_RIGHT_HIP_PITCH,
   FRONT_RIGHT_KNEE_PITCH,
   HIND_LEFT_HIP_ROLL,
   HIND_LEFT_HIP_PITCH,
   HIND_LEFT_KNEE_PITCH,
   HIND_RIGHT_HIP_ROLL,
   HIND_RIGHT_HIP_PITCH,
   HIND_RIGHT_KNEE_PITCH
   ;

   public static final GenericQuadrupedOrderedJointMap[] values = values();
   private final String lowercaseName;

   private GenericQuadrupedOrderedJointMap()
   {
      lowercaseName = toString().toLowerCase();
   }
   
   public String getName()
   {
      return lowercaseName;
   }

   public static int getNumJoints()
   {
      return values.length;
   }

   public RobotQuadrant getRobotQuadrant()
   {
      switch (this)
      {
      case FRONT_LEFT_HIP_ROLL:  
      case FRONT_LEFT_HIP_PITCH: 
      case FRONT_LEFT_KNEE_PITCH:
         return RobotQuadrant.FRONT_LEFT;
      case FRONT_RIGHT_HIP_ROLL: 
      case FRONT_RIGHT_HIP_PITCH:
      case FRONT_RIGHT_KNEE_PITCH:
         return RobotQuadrant.FRONT_RIGHT;
      case HIND_LEFT_HIP_ROLL:  
      case HIND_LEFT_HIP_PITCH: 
      case HIND_LEFT_KNEE_PITCH:
         return RobotQuadrant.HIND_LEFT;
      case HIND_RIGHT_HIP_ROLL:  
      case HIND_RIGHT_HIP_PITCH: 
      case HIND_RIGHT_KNEE_PITCH:
         return RobotQuadrant.HIND_RIGHT;
      default:
         throw new RuntimeException("RobotQuadrant undefined for " + this.name());
      }
   }
   
   public LegJointName getLegJointName()
   {
      switch (this)
      {
      case FRONT_LEFT_HIP_ROLL:  
      case FRONT_RIGHT_HIP_ROLL: 
      case HIND_LEFT_HIP_ROLL:  
      case HIND_RIGHT_HIP_ROLL:  
         return LegJointName.HIP_ROLL;
      case FRONT_LEFT_HIP_PITCH: 
      case FRONT_RIGHT_HIP_PITCH:
      case HIND_LEFT_HIP_PITCH: 
      case HIND_RIGHT_HIP_PITCH: 
         return LegJointName.HIP_PITCH;
      case FRONT_LEFT_KNEE_PITCH:
      case FRONT_RIGHT_KNEE_PITCH:
      case HIND_LEFT_KNEE_PITCH:
      case HIND_RIGHT_KNEE_PITCH:
         return LegJointName.KNEE_PITCH;
      default:
         return null;
      }
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/robotSide/SegmentDependentList.java<|end_filename|>
package us.ihmc.robotics.robotSide;

import java.util.EnumMap;

public class SegmentDependentList<E extends Enum<E> & RobotSegment<E>, V> extends EnumMap<E, V>
{ 
   public SegmentDependentList(Class<E> keyClass)
   {
      super(keyClass);
   }

   public V get(E key)
   {
      return super.get(key);
   }

   public V set(E segment, V element)
   {
      return this.put(segment, element);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedBodyPathPlanMessage.java<|end_filename|>
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 commands the quadruped planner to execute a path composed of the given sequence of waypoints.
       * A waypoint represents a desired (x, y, yaw) position which the robot should pass through.
       * A Hermite based curve (third order) is used to interpolate the waypoints.
       */
public class QuadrupedBodyPathPlanMessage extends Packet<QuadrupedBodyPathPlanMessage> implements Settable<QuadrupedBodyPathPlanMessage>, EpsilonComparable<QuadrupedBodyPathPlanMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean is_expressed_in_absolute_time_ = true;
   /**
            * List of body path waypoint
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.EuclideanTrajectoryPointMessage>  body_path_points_;

   public QuadrupedBodyPathPlanMessage()
   {
      body_path_points_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.EuclideanTrajectoryPointMessage> (50, new controller_msgs.msg.dds.EuclideanTrajectoryPointMessagePubSubType());

   }

   public QuadrupedBodyPathPlanMessage(QuadrupedBodyPathPlanMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedBodyPathPlanMessage other)
   {
      sequence_id_ = other.sequence_id_;

      is_expressed_in_absolute_time_ = other.is_expressed_in_absolute_time_;

      body_path_points_.set(other.body_path_points_);
   }

   /**
            * 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_;
   }

   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public void setIsExpressedInAbsoluteTime(boolean is_expressed_in_absolute_time)
   {
      is_expressed_in_absolute_time_ = is_expressed_in_absolute_time;
   }
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean getIsExpressedInAbsoluteTime()
   {
      return is_expressed_in_absolute_time_;
   }


   /**
            * List of body path waypoint
            */
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.EuclideanTrajectoryPointMessage>  getBodyPathPoints()
   {
      return body_path_points_;
   }


   public static Supplier<QuadrupedBodyPathPlanMessagePubSubType> getPubSubType()
   {
      return QuadrupedBodyPathPlanMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedBodyPathPlanMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedBodyPathPlanMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_expressed_in_absolute_time_, other.is_expressed_in_absolute_time_, epsilon)) return false;

      if (this.body_path_points_.size() != other.body_path_points_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.body_path_points_.size(); i++)
         {  if (!this.body_path_points_.get(i).epsilonEquals(other.body_path_points_.get(i), epsilon)) return false; }
      }


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedBodyPathPlanMessage)) return false;

      QuadrupedBodyPathPlanMessage otherMyClass = (QuadrupedBodyPathPlanMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.is_expressed_in_absolute_time_ != otherMyClass.is_expressed_in_absolute_time_) return false;

      if (!this.body_path_points_.equals(otherMyClass.body_path_points_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedBodyPathPlanMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("is_expressed_in_absolute_time=");
      builder.append(this.is_expressed_in_absolute_time_);      builder.append(", ");
      builder.append("body_path_points=");
      builder.append(this.body_path_points_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/WobblySimulationContactPoints.java<|end_filename|>
package us.ihmc.wholeBodyController;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.robotics.partNames.LeggedJointNameMap;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SegmentDependentList;
import us.ihmc.robotics.robotSide.SideDependentList;

public class WobblySimulationContactPoints implements FootContactPoints<RobotSide>
{
   private final double zWobble;

   public WobblySimulationContactPoints(double zWobble)
   {
      this.zWobble = zWobble;
   }

   @Override
   public Map<String, List<Tuple3DBasics>> getSimulationContactPoints(double footLength, double footWidth, double toeWidth, LeggedJointNameMap<RobotSide> jointMap,
         SegmentDependentList<RobotSide, RigidBodyTransform> soleToAnkleFrameTransforms)
   {
      HashMap<String, List<Tuple3DBasics>> ret = new HashMap<>();

      for (RobotSide robotSide : RobotSide.values)
      {
         ArrayList<Tuple3DBasics> footContactPoints = new ArrayList<>();
         String parentJointName = jointMap.getJointBeforeFootName(robotSide);

         //SCS Sim contactPoints
         int nContactPointsX = 2;
         int nContactPointsY = 2;

         double dx = 1.01 * footLength / (nContactPointsX - 1.0);
         double xOffset = 1.01 * footLength / 2.0;

         for (int ix = 1; ix <= nContactPointsX; ix++)
         {
            double alpha = (ix - 1.0) / (nContactPointsX - 1.0);
            double footWidthAtCurrentX = (1.0 - alpha) * 1.01 * footWidth + alpha * 1.01 * toeWidth;
            double dy = footWidthAtCurrentX / (nContactPointsY - 1.0);
            double yOffset = footWidthAtCurrentX / 2.0;

            for (int iy = 1; iy <= nContactPointsY; iy++)
            {
               double x = (ix - 1.0) * dx - xOffset;
               double y = (iy - 1.0) * dy - yOffset;

               double z = 0.0;
               if (((ix == 1) && (iy == 1)) || ((ix == 2) && (iy == 2)))
               {
                  z = -zWobble;
               }

               Point3D contactPoint = new Point3D(x, y, z);
               RigidBodyTransform transformToParentJointFrame = soleToAnkleFrameTransforms.get(robotSide);
               transformToParentJointFrame.transform(contactPoint);
               footContactPoints.add(contactPoint);
            }
         }

         ret.put(parentJointName, footContactPoints);
      }

      return ret;
   }

   @Override
   public SideDependentList<List<Tuple2DBasics>> getControllerContactPoints(double footLength, double footWidth, double toeWidth)
   {
      SideDependentList<List<Tuple2DBasics>> ret = new SideDependentList<>();

      for (RobotSide robotSide : RobotSide.values)
      {
         ArrayList<Tuple2DBasics> contactPoints = new ArrayList<>();
         contactPoints.add(new Point2D(-footLength / 2.0, -footWidth / 2.0));
         contactPoints.add(new Point2D(-footLength / 2.0, footWidth / 2.0));
         contactPoints.add(new Point2D(footLength / 2.0, -toeWidth / 2.0));
         contactPoints.add(new Point2D(footLength / 2.0, toeWidth / 2.0));
         ret.put(robotSide, contactPoints);
      }

      return ret;
   }

   @Override
   public SideDependentList<Tuple2DBasics> getToeOffContactPoints(double footLength, double footWidth, double toeWidth)
   {
      SideDependentList<Tuple2DBasics> ret = new SideDependentList<>();

      for (RobotSide robotSide : RobotSide.values)
         ret.put(robotSide, new Point2D(footLength / 2.0, 0.0));

      return ret;
   }

   @Override
   public SideDependentList<LineSegment2D> getToeOffContactLines(double footLength, double footWidth, double toeWidth)
   {
      SideDependentList<LineSegment2D> ret = new SideDependentList<>();

      double footForward = footLength / 2.0;
      double halfToeWidth = toeWidth / 2.0;

      for (RobotSide robotSide : RobotSide.values)
         ret.put(robotSide, new LineSegment2D(new Point2D(footForward, -halfToeWidth), new Point2D(footForward, halfToeWidth)));

      return ret;
   }

   @Override
   public boolean useSoftContactPointParameters()
   {
      return false;
   }

}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/pointClouds/testbed/ComputeEstimatedToModel.java<|end_filename|>
package us.ihmc.sensorProcessing.pointClouds.testbed;

import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import com.thoughtworks.xstream.XStream;

import georegression.struct.se.Se3_F64;

/**
 * Processes transforms found be the automated algorithm and manually aligned.  It selects the transform which minimizes
 * the error across the set as the official transform from estimated to model.
 *
 * @author <NAME>
 */
public class ComputeEstimatedToModel {

   public static void main(String[] args) {
      String directory = "../SensorProcessing/data/testbed/2014-08-01/";


      int N = 16;

      List<Se3_F64> listEstimated = new ArrayList<>();
      List<Se3_F64> listModel = new ArrayList<>();
      List<Se3_F64> listEstimatedToModel = new ArrayList<>();

      for (int i = 0; i < N; i++) {
         String nameEstimated = String.format(directory + "estimatedTestbedToWorld%02d.xml", i);
         String nameModel = String.format(directory + "modelTestbedToWorld%02d.xml", i);


         Se3_F64 estimatedToWorld, modelToWorld;
         try {
            estimatedToWorld = (Se3_F64) new XStream().fromXML(new FileInputStream(nameEstimated));
            modelToWorld = (Se3_F64) new XStream().fromXML(new FileInputStream(nameModel));
         } catch (FileNotFoundException e) {
            throw new RuntimeException(e);
         }

         listEstimated.add(estimatedToWorld);
         listModel.add(modelToWorld);

         Se3_F64 estimatedToModel = estimatedToWorld.concat(modelToWorld.invert(null),null);
         listEstimatedToModel.add( estimatedToModel );

      }

      double bestError = Double.MAX_VALUE;
      Se3_F64 best = null;
      double errors[] = new double[N];
      for (int i = 0; i < N; i++) {
         Se3_F64 estimatedToModel = listEstimatedToModel.get(i);

         for (int j = 0; j < N; j++) {
            Se3_F64 estimatedToWorld = listEstimated.get(j);
            Se3_F64 modelToWorld = listModel.get(j);
            Se3_F64 found = estimatedToWorld.concat(modelToWorld.invert(null),null);

            errors[j] = found.getT().distance(estimatedToModel.getT());
         }

         Arrays.sort(errors);

         double error50 = errors[ N/2 ];
         System.out.println(i+" Error  = "+error50);
         if( error50 < bestError ) {
            bestError = error50;
            best = estimatedToModel;
         }
      }

      System.out.println("Best.  Translation error 50% = "+ bestError);

      try {
         new XStream().toXML(best, new FileOutputStream("estimatedToModel.xml"));
      } catch (FileNotFoundException e) {
         throw new RuntimeException(e);
      }
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/clouds/detect/wrapper/ConfigMultiShapeRansac.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.clouds.detect.wrapper;

import java.util.ArrayList;
import java.util.List;

import org.ddogleg.fitting.modelset.ModelFitter;
import org.ddogleg.fitting.modelset.ModelManager;
import org.ddogleg.fitting.modelset.ransac.RansacMulti;

import georegression.fitting.cylinder.FitCylinderToPoints_F64;
import georegression.fitting.cylinder.ModelManagerCylinder3D_F64;
import georegression.fitting.plane.ModelManagerPlaneGeneral3D_F64;
import georegression.fitting.sphere.FitSphereToPoints_F64;
import georegression.fitting.sphere.ModelManagerSphere3D_F64;
import us.ihmc.sensorProcessing.bubo.clouds.detect.CloudShapeTypes;
import us.ihmc.sensorProcessing.bubo.clouds.detect.alg.ModelFitter_P_to_PVNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.alg.PointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.DistanceCylinderToPointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.DistancePlaneToPointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.DistanceSphereToPointVectorNN;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.GenerateCylinderPointVector;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.GeneratePlanePointVector;
import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.GenerateSpherePointVector;

/**
 * Configures RANSAC which can match different shapes.
 *
 * @author <NAME>
 */
public class ConfigMultiShapeRansac {

	/**
	 * Random seed used by RANSAC
	 */
	public long randSeed = 0xFEEDBEEF;
	/**
	 * The maximum number of iterations RANSAC will perform.  Default 1000
	 */
	public int maxIterations = 1000;

	/**
	 * The minimum number of points a shape must have for it to be accepted
	 */
	public int minimumPoints = 0;

	/**
	 * The maximum number of shapes it will find
	 */
	public int maximumNumberOfShapes = Integer.MAX_VALUE;

	/**
	 * Description of the shapes RANSAC will search for.
	 */
	public List<RansacMulti.ObjectType> models;

	/**
	 * Which types of shapes are contains in the models list.  The elements in each list correspond to each other.
	 */
	public List<CloudShapeTypes> types;

	/**
	 * Used to create new shape parameter data structures. The elements in this list correspond to elements in models.
	 */
	public List<ModelManager> modelManagers;
	/**
	 * Used to create new shape parameter data structures. The elements in this list correspond to elements in models.
	 */
	public List<ModelFitter<Object, PointVectorNN>> fitters;

	/**
	 * Creates a default set of parameters which can detect
	 *
	 * @param fitIterations           Number of iterations when refining a shape
	 * @param angleTolerance          Tolerance in radians to reject a model from an initial sample set
	 * @param ransacDistanceThreshold Euclidean distance that RANSAC considers a point an inlier
	 * @param shapes                  A list of shape you wish to detect.  If empty or null then all possible shapes will be detected.
	 * @return ConfigSchnabel2007
	 */
	public static ConfigMultiShapeRansac createDefault(int fitIterations,
													   double angleTolerance,
													   double ransacDistanceThreshold,
													   CloudShapeTypes... shapes) {
		if (shapes == null || shapes.length == 0) {
			shapes = CloudShapeTypes.values();
		}
		List<RansacMulti.ObjectType> objects = new ArrayList<RansacMulti.ObjectType>();
		List<ModelManager> modelManagers = new ArrayList<ModelManager>();
		List<ModelFitter<Object, PointVectorNN>> fitters = new ArrayList<ModelFitter<Object, PointVectorNN>>();

		for (CloudShapeTypes shape : shapes) {
			switch (shape) {
				case SPHERE: {
					RansacMulti.ObjectType sphere = new RansacMulti.ObjectType();
					sphere.modelManager = new ModelManagerSphere3D_F64();
					sphere.modelDistance = new DistanceSphereToPointVectorNN(angleTolerance);
					sphere.modelGenerator = new GenerateSpherePointVector(angleTolerance, ransacDistanceThreshold);
					sphere.thresholdFit = ransacDistanceThreshold;
					objects.add(sphere);
					modelManagers.add(new ModelManagerSphere3D_F64());
					fitters.add(new ModelFitter_P_to_PVNN(new FitSphereToPoints_F64(fitIterations)));
				}
				break;

				case CYLINDER: {
					RansacMulti.ObjectType cylinder = new RansacMulti.ObjectType();
					cylinder.modelManager = new ModelManagerCylinder3D_F64();
					cylinder.modelDistance = new DistanceCylinderToPointVectorNN(angleTolerance);
					cylinder.modelGenerator = new GenerateCylinderPointVector(angleTolerance, ransacDistanceThreshold);
					cylinder.thresholdFit = ransacDistanceThreshold;
					objects.add(cylinder);
					modelManagers.add(new ModelManagerCylinder3D_F64());
					fitters.add(new ModelFitter_P_to_PVNN(new FitCylinderToPoints_F64(fitIterations)));
				}
				break;

				case PLANE: {
					RansacMulti.ObjectType plane = new RansacMulti.ObjectType();
					plane.modelManager = new ModelManagerPlaneGeneral3D_F64();
					plane.modelDistance = new DistancePlaneToPointVectorNN(angleTolerance);
					plane.modelGenerator = new GeneratePlanePointVector(angleTolerance);
					plane.thresholdFit = ransacDistanceThreshold;
					objects.add(plane);
					modelManagers.add(new ModelManagerPlaneGeneral3D_F64());
					fitters.add(new ModelFitter_P_to_PVNN(new PlaneGeneralSvd_to_ModelFitter()));
				}
				break;

				default:
					throw new IllegalArgumentException("Unsupported shape: " + shape);
			}
		}

		ConfigMultiShapeRansac config = new ConfigMultiShapeRansac();
		config.models = objects;
		config.modelManagers = modelManagers;
		config.fitters = fitters;
		config.types = new ArrayList<CloudShapeTypes>();
		for (CloudShapeTypes t : shapes) {
			config.types.add(t);
		}

		return config;
	}
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/RemoteLidarBasedREAUILauncher.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness;

import java.util.Map;

import javafx.application.Application;
import javafx.stage.Stage;
import us.ihmc.log.LogTools;
import us.ihmc.robotEnvironmentAwareness.ui.LIDARBasedEnvironmentAwarenessUI;

public class RemoteLidarBasedREAUILauncher extends Application
{
   public RemoteLidarBasedREAUILauncher()
   {
   }

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      Parameters parameters = getParameters();
      Map<String, String> namedParameters = parameters.getNamed();
      String host = namedParameters.getOrDefault("host", "localhost");
      LogTools.info("Creating REA UI with the module address: " + host);

      if (!parameters.getRaw().isEmpty())
         LogTools.info("Received the program arguments: " + parameters.getRaw());

      LIDARBasedEnvironmentAwarenessUI remoteUI = LIDARBasedEnvironmentAwarenessUI.creatRemoteUI(primaryStage, host);
      remoteUI.show();
   }

   public static void main(String[] args)
   {
      LogTools.info("To change the address of the module, enter its IP address as a program argument. For instance: " + "--host=127.0.0.1");
      launch(args);
   }
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullQuadrupedRobotModel.java<|end_filename|>
package us.ihmc.robotModels;

import java.util.List;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.kinematics.JointLimit;
import us.ihmc.robotics.kinematics.JointLimitData;
import us.ihmc.robotics.partNames.QuadrupedJointName;
import us.ihmc.robotics.robotSide.RobotQuadrant;

public interface FullQuadrupedRobotModel extends FullLeggedRobotModel<RobotQuadrant>
{
   /** Use {@link #getJointLimitData(OneDoFJointBasics joint)}*/
   @Deprecated
   JointLimit getJointLimit(QuadrupedJointName jointName);

   JointLimitData getJointLimitData(OneDoFJointBasics joint);

   QuadrupedJointName getNameForOneDoFJoint(OneDoFJointBasics oneDoFJoint);

   List<OneDoFJointBasics> getLegJointsList(RobotQuadrant robotQuadrant);

   /**
    * Returns the {@link RigidBodyBasics} describing the body of this robot.
    * In the current framework (on the day: 3/1/2014), the body is the the first successor of the root joint.
    */
   RigidBodyBasics getBody();

   default RobotQuadrant[] getRobotSegments()
   {
      return RobotQuadrant.values;
   }
}

<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/ObjectWeightTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.ObjectWeightPacket;
import us.ihmc.humanoidBehaviors.behaviors.primitives.ObjectWeightBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.robotSide.RobotSide;

public class ObjectWeightTask extends BehaviorAction
{
   private final ObjectWeightPacket objectWeightPacket;
   private final ObjectWeightBehavior objectWeightBehavior;

   public ObjectWeightTask(RobotSide robotSide, double weight, ObjectWeightBehavior objectWeightBehavior)
   {
      super(objectWeightBehavior);
      objectWeightPacket = HumanoidMessageTools.createObjectWeightPacket(robotSide, weight);
      this.objectWeightBehavior = objectWeightBehavior;
   }

   @Override
   protected void setBehaviorInput()
   {
      objectWeightBehavior.setInput(objectWeightPacket);
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AbortWalkingMessage.java<|end_filename|>
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 abort walking, forcing the robot to switch back to double support and clear the footstep list.
       */
public class AbortWalkingMessage extends Packet<AbortWalkingMessage> implements Settable<AbortWalkingMessage>, EpsilonComparable<AbortWalkingMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;

   public AbortWalkingMessage()
   {
   }

   public AbortWalkingMessage(AbortWalkingMessage other)
   {
      this();
      set(other);
   }

   public void set(AbortWalkingMessage 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<AbortWalkingMessagePubSubType> getPubSubType()
   {
      return AbortWalkingMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AbortWalkingMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AbortWalkingMessage 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 AbortWalkingMessage)) return false;

      AbortWalkingMessage otherMyClass = (AbortWalkingMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AbortWalkingMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/SimpleUILauncher.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui;

import javafx.application.Application;
import javafx.application.Platform;
import javafx.stage.Stage;

public class SimpleUILauncher extends Application
{
   private static final boolean SHOW_FILE_CHOOSER_ON_START = false;

   private SimpleVisibilityGraphsUI ui;
   

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      ui = new SimpleVisibilityGraphsUI(primaryStage);
      ui.show(SHOW_FILE_CHOOSER_ON_START);
   }

   @Override
   public void stop() throws Exception
   {
      super.stop();
      ui.stop();

      Platform.exit();
   }

   public static void main(String[] args)
   {
      launch();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/ButtonListener.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

public interface ButtonListener
{
   /**
    * Will be called when a button on a slider board was pressed.
    *
    * @param status whether the button was switched to on or off.
    */
   public void buttonPressed(boolean status);
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/diagnostics/DiagnosticsWhenHangingControllerStateFactory.java<|end_filename|>
package us.ihmc.wholeBodyController.diagnostics;

import java.util.ArrayList;

import us.ihmc.commonWalkingControlModules.controllers.Updatable;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.HighLevelControllerStateFactory;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.wholeBodyController.JointTorqueOffsetProcessor;

public class DiagnosticsWhenHangingControllerStateFactory implements HighLevelControllerStateFactory
{
   private FullRobotModel fullRobotModel;
   private final ArrayList<Updatable> updatables = new ArrayList<Updatable>();
   private JointTorqueOffsetProcessor jointTorqueOffsetProcessor;
   private boolean transitionRequested = false;

   private final boolean useArms;
   private final boolean robotIsHanging;
   private final TorqueOffsetPrinter torqueOffsetPrinter;
   
   private final HumanoidJointPoseList humanoidJointPoseList;
   private DiagnosticsWhenHangingControllerState controller;

   public DiagnosticsWhenHangingControllerStateFactory(HumanoidJointPoseList humanoidJointPoseList, boolean useArms, boolean robotIsHanging, TorqueOffsetPrinter torqueOffsetPrinter)
   {
      this.humanoidJointPoseList = humanoidJointPoseList;
      this.useArms = useArms;
      this.robotIsHanging = robotIsHanging;
      this.torqueOffsetPrinter = torqueOffsetPrinter;
   }

   public FullRobotModel getFullRobotModel()
   {
      return fullRobotModel;
   }

   public void attachJointTorqueOffsetProcessor(JointTorqueOffsetProcessor jointTorqueOffsetProcessor)
   {
      if (controller != null)
         controller.attachjointTorqueOffsetProcessor(jointTorqueOffsetProcessor);
      else
         this.jointTorqueOffsetProcessor = jointTorqueOffsetProcessor;
   }


   public DiagnosticsWhenHangingControllerState getController()
   {
      return controller;
   }

   @Override
   public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper)
   {
      controller = new DiagnosticsWhenHangingControllerState(humanoidJointPoseList, useArms, robotIsHanging,
                                                             controllerFactoryHelper.getHighLevelHumanoidControllerToolbox(),
                                                             controllerFactoryHelper.getHighLevelControllerParameters(), this.torqueOffsetPrinter);

      controller.addUpdatables(updatables);

      if (jointTorqueOffsetProcessor != null)
         controller.attachjointTorqueOffsetProcessor(jointTorqueOffsetProcessor);

      return controller;
   }

   @Override
   public boolean isTransitionToControllerRequested()
   {
      return transitionRequested;
   }

   @Override
   public HighLevelControllerName getStateEnum()
   {
      return HighLevelControllerName.DIAGNOSTICS;
   }

   public void setTransitionRequested(boolean transitionRequested)
   {
      this.transitionRequested = transitionRequested;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/SliderboardDemo.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

import us.ihmc.commons.Conversions;
import us.ihmc.commons.PrintTools;
import us.ihmc.commons.thread.ThreadTools;

public class SliderboardDemo
{
   private static final int buttons = 16;
   private static final int sliders = 16;

   public static void main(String[] args)
   {
      Sliderboard sliderboard = new Sliderboard();

      if (!sliderboard.isConnected())
      {
         return;
      }

      for (int slider = 1; slider <= sliders; slider++)
      {
         int sliderIndex = slider;

         sliderboard.addListener(new SliderboardListener()
         {
            @Override
            public void sliderMoved(double sliderPercentage)
            {
               PrintTools.info("Slider " + sliderIndex + " moved to: " + sliderPercentage);
            }
         }, slider);

         sliderboard.setSliderValue(0.0, sliderIndex);
      }

      for (int button = 1; button <= buttons; button++)
      {
         int buttonIndex = button;

         sliderboard.addListener(new ButtonListener()
         {
            @Override
            public void buttonPressed(boolean status)
            {
               PrintTools.info("Button " + buttonIndex + " pushed: " + status);
            }
         }, button);
      }

      goToZero(sliderboard);

      double time = 0.0;
      double dt = 0.05;
      double totalTime = 5.0;
      int button = 1;

      while (time < totalTime)
      {
         for (int slider = 1 ; slider <= sliders; slider++)
         {
            double phase = Math.PI * slider / sliders + Math.PI;
            sliderboard.setSliderValue(Math.max(Math.sin(time * 2.0 * Math.PI / totalTime + phase), 0.0), slider);
         }

         sliderboard.setButtonValue(false, (button - 1) == 0 ? buttons : button - 1);
         sliderboard.setButtonValue(true, button);
         button = button == buttons ? 1 : button + 1;

         ThreadTools.sleep((int) Conversions.secondsToMilliseconds(dt));
         time += dt;
      }

      goToZero(sliderboard);
      sliderboard.close();
   }

   private static void goToZero(Sliderboard sliderboard)
   {
      for (int button = 1 ; button <= buttons; button++)
      {
         sliderboard.setButtonValue(false, button);
      }
      for (int slider = 1 ; slider <= sliders; slider++)
      {
         sliderboard.setSliderValue(0.0, slider);
      }
      ThreadTools.sleep(100);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PlanarVelocityPacket.java<|end_filename|>
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 quadruped controller API.
       */
public class PlanarVelocityPacket extends Packet<PlanarVelocityPacket> implements Settable<PlanarVelocityPacket>, EpsilonComparable<PlanarVelocityPacket>
{
   public us.ihmc.euclid.tuple3D.Vector3D velocity_;

   public PlanarVelocityPacket()
   {
      velocity_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public PlanarVelocityPacket(PlanarVelocityPacket other)
   {
      this();
      set(other);
   }

   public void set(PlanarVelocityPacket other)
   {
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.velocity_, velocity_);   }


   public us.ihmc.euclid.tuple3D.Vector3D getVelocity()
   {
      return velocity_;
   }


   public static Supplier<PlanarVelocityPacketPubSubType> getPubSubType()
   {
      return PlanarVelocityPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PlanarVelocityPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PlanarVelocityPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!this.velocity_.epsilonEquals(other.velocity_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PlanarVelocityPacket)) return false;

      PlanarVelocityPacket otherMyClass = (PlanarVelocityPacket) other;

      if (!this.velocity_.equals(otherMyClass.velocity_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PlanarVelocityPacket {");
      builder.append("velocity=");
      builder.append(this.velocity_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/outputData/LowLevelStateListReadOnly.java<|end_filename|>
package us.ihmc.sensorProcessing.outputData;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;

public interface LowLevelStateListReadOnly
{
   boolean hasDataForJoint(OneDoFJointBasics joint);

   OneDoFJointBasics getOneDoFJoint(int index);

   LowLevelStateReadOnly getLowLevelState(OneDoFJointBasics joint);

   LowLevelStateReadOnly getLowLevelState(int index);

   int getNumberOfJointsWithLowLevelState();
}

<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/RosLocalizationServiceClient.java<|end_filename|>
package us.ihmc.ihmcPerception;

import org.ros.exception.RemoteException;
import org.ros.node.service.ServiceResponseListener;

import controller_msgs.msg.dds.LocalizationPacket;
import ethz_asl_icp_mapper.SetMode;
import ethz_asl_icp_mapper.SetModeRequest;
import ethz_asl_icp_mapper.SetModeResponse;
import std_srvs.Empty;
import std_srvs.EmptyRequest;
import std_srvs.EmptyResponse;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.RosServiceClient;


public class RosLocalizationServiceClient implements PacketConsumer<LocalizationPacket>
{
   private final RosServiceClient<SetModeRequest, SetModeResponse> localizationClient = new RosServiceClient<SetModeRequest,SetModeResponse>(SetMode._TYPE);
   private final RosServiceClient<EmptyRequest,EmptyResponse> resetClient = new RosServiceClient<EmptyRequest, EmptyResponse>(Empty._TYPE);
   private RosMainNode rosMainNode;
   private static final boolean DEBUG = false;
   
   public RosLocalizationServiceClient(RosMainNode rosMainNode)
   {
      this.rosMainNode = rosMainNode;
      
      rosMainNode.attachServiceClient("/mapper_humanoid/set_mode", localizationClient);
      rosMainNode.attachServiceClient("/mapper_humanoid/reset", resetClient);
   }
   
   public void sendLocalizationMessage(final LocalizationPacket localizationPacket)
   {
      try
      {
         Thread setupThread = new Thread("IHMC-RosLocalizationServiceSend")
         {
            public void run()
            {
               localizationClient.waitTillConnected();
               
               SetModeRequest request = localizationClient.getMessage();
               
               request.setLocalize(localizationPacket.getToggle());
               request.setMap(localizationPacket.getToggle());
               
               if (DEBUG)
               {
                  System.out.println("Sending message to ROS. Localization = " + localizationPacket.getToggle());
                  System.out.println("Reset = " + localizationPacket.getReset());
               }
               
               localizationClient.call(request, new ServiceResponseListener<SetModeResponse>()
               {
                  
                  @Override
                  public void onSuccess(SetModeResponse response)
                  {
                     boolean toggle = response.getLocalize();
                     System.out.println("Localization is set to: " + toggle);
                  }
                  
                  @Override
                  public void onFailure(RemoteException e)
                  {
                     e.printStackTrace();
                  }
               });
            }
         };
         
         setupThread.start();
      }
      catch (Exception e)
      {
         System.err.println(e.getMessage());
      }
   }
   
   public void sendResetReference(final LocalizationPacket object)
   {
      try
      {
         Thread setupThread = new Thread("IHMC-RosLocalizationServiceReset")
         {
            public void run()
            {
               resetClient.waitTillConnected();
               
               EmptyRequest request = resetClient.getMessage();
               

               
               resetClient.call(request, new ServiceResponseListener<EmptyResponse>()
               {
                  @Override
                  public void onSuccess(EmptyResponse response)
                  {
                     System.out.println("Map has been reset");
                  }
                  
                  @Override
                  public void onFailure(RemoteException e)
                  {
                     e.printStackTrace();
                  }
               });
            }
         };
         
         setupThread.start();
      }
      catch (Exception e)
      {
         System.err.println(e.getMessage());
      }
   }

   public void processLocalizationPacket(LocalizationPacket object)
   {
      if (object.getReset())
         sendResetReference(object);
      else
         sendLocalizationMessage(object);
   }

   public void receivedPacket(LocalizationPacket object)
   {
      processLocalizationPacket(object);
   }
   
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/bipedSupportPolygons/SimpleContactPointPlaneBody.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.bipedSupportPolygons;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.referenceFrames.PoseReferenceFrame;

/**
 * Simple implementation of {@link #ContactablePlaneBody} for a single contact point.
 *
 * @author Georg
 *
 */
public class SimpleContactPointPlaneBody implements ContactablePlaneBody
{
   private final String name;
   private final RigidBodyBasics rigidBody;
   private final PoseReferenceFrame contactFrame;

   private final List<FramePoint2D> contactPoints = new ArrayList<>();

   public SimpleContactPointPlaneBody(String name, RigidBodyBasics rigidBody, RigidBodyTransform contactFramePoseInJointFrame)
   {
      this.name = name;
      this.rigidBody = rigidBody;

      ReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint();
      contactFrame = new PoseReferenceFrame(name + "Frame", frameAfterJoint);
      contactFrame.setPoseAndUpdate(contactFramePoseInJointFrame);
      contactPoints.add(new FramePoint2D(contactFrame));
   }

   @Override
   public String getName()
   {
      return name;
   }

   @Override
   public RigidBodyBasics getRigidBody()
   {
      return rigidBody;
   }

   @Override
   public MovingReferenceFrame getFrameAfterParentJoint()
   {
      return rigidBody.getParentJoint().getFrameAfterJoint();
   }

   @Override
   public int getTotalNumberOfContactPoints()
   {
      return 1;
   }

   @Override
   public List<FramePoint3D> getContactPointsCopy()
   {
      List<FramePoint3D> contactPoints = new ArrayList<>();
      contactPoints.add(new FramePoint3D(contactFrame));
      return contactPoints;
   }

   @Override
   public ReferenceFrame getSoleFrame()
   {
      return contactFrame;
   }

   @Override
   public List<FramePoint2D> getContactPoints2d()
   {
      return contactPoints;
   }

   @Override
   public void setSoleFrameTransformFromParentJoint(RigidBodyTransform transform)
   {
      contactFrame.setPoseAndUpdate(transform);
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/gui/ToggleMode.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.gui;

public enum ToggleMode {NO_CHANGE, SET_TRUE, SET_FALSE}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/util/ProgressMonitorInterface.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger.util;

import java.io.PrintStream;

public interface ProgressMonitorInterface
{

   void setNote(String note);

   void setProgress(int i);

   PrintStream getPrintStream();

   void close();

   void setError(String string);

   void initialize(String message, String note, int min, int max);

}

<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/packets/ExecutionTiming.java<|end_filename|>
package us.ihmc.communication.packets;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;

public enum ExecutionTiming
{
   @RosEnumValueDocumentation(documentation = "During the execution of this message the controller will attempt to achieve the given durations for segments of the whole trajectory.")
   CONTROL_DURATIONS,
   @RosEnumValueDocumentation(documentation = "During the execution of this message the controller will attempt to achieve the absolute timings at the knot points relative to the start of execution.")
   CONTROL_ABSOLUTE_TIMINGS;

   public static final ExecutionTiming[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static ExecutionTiming fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/GuiElement.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements;

public abstract class GuiElement
{
   public static final String SEPERATOR = ":";

   private final String name;
   private final GuiRegistry parent;
   private final String uniqueName;

   public GuiElement(String name, GuiRegistry parent)
   {
      this(name, parent, createUniqueName(name, parent));
   }

   public GuiElement(String name, GuiRegistry parent, String uniqueName)
   {
      if (name.contains(SEPERATOR))
      {
         throw new RuntimeException("Name " + name + " should never contain " + SEPERATOR);
      }

      this.name = name;
      this.parent = parent;
      this.uniqueName = uniqueName;
   }

   public String getName()
   {
      return name;
   }

   public String getUniqueName()
   {
      return uniqueName;
   }

   public GuiRegistry getParent()
   {
      return parent;
   }

   public static String createUniqueName(String name, GuiRegistry parent)
   {
      if (parent == null)
      {
         return name;
      }
      else
      {
         return parent.getUniqueName() + SEPERATOR + name;
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasDesiredPumpPSIPacket.java<|end_filename|>
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;

/**
       * Send a request to change the desired PSI of the Atlas hydraulic pump.
       */
public class AtlasDesiredPumpPSIPacket extends Packet<AtlasDesiredPumpPSIPacket> implements Settable<AtlasDesiredPumpPSIPacket>, EpsilonComparable<AtlasDesiredPumpPSIPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The desired hydraulic pump PSI.
            */
   public int desired_pump_psi_;

   public AtlasDesiredPumpPSIPacket()
   {
   }

   public AtlasDesiredPumpPSIPacket(AtlasDesiredPumpPSIPacket other)
   {
      this();
      set(other);
   }

   public void set(AtlasDesiredPumpPSIPacket other)
   {
      sequence_id_ = other.sequence_id_;

      desired_pump_psi_ = other.desired_pump_psi_;

   }

   /**
            * 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_;
   }

   /**
            * The desired hydraulic pump PSI.
            */
   public void setDesiredPumpPsi(int desired_pump_psi)
   {
      desired_pump_psi_ = desired_pump_psi;
   }
   /**
            * The desired hydraulic pump PSI.
            */
   public int getDesiredPumpPsi()
   {
      return desired_pump_psi_;
   }


   public static Supplier<AtlasDesiredPumpPSIPacketPubSubType> getPubSubType()
   {
      return AtlasDesiredPumpPSIPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AtlasDesiredPumpPSIPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AtlasDesiredPumpPSIPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_pump_psi_, other.desired_pump_psi_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AtlasDesiredPumpPSIPacket)) return false;

      AtlasDesiredPumpPSIPacket otherMyClass = (AtlasDesiredPumpPSIPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.desired_pump_psi_ != otherMyClass.desired_pump_psi_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AtlasDesiredPumpPSIPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("desired_pump_psi=");
      builder.append(this.desired_pump_psi_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasAuxiliaryRobotDataPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "AtlasAuxiliaryRobotData" defined in "AtlasAuxiliaryRobotData_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from AtlasAuxiliaryRobotData_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit AtlasAuxiliaryRobotData_.idl instead.
*
*/
public class AtlasAuxiliaryRobotDataPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.AtlasAuxiliaryRobotData>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::AtlasAuxiliaryRobotData_";

   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.AtlasAuxiliaryRobotData 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.AtlasAuxiliaryRobotData data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (6 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (6 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (6 * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += ((15) * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += ((15) * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      for(int i0 = 0; i0 < (15); ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.Vector3PubSubType.getMaxCdrSerializedSize(current_alignment);}
      for(int i0 = 0; i0 < (15); ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.Vector3PubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      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.AtlasAuxiliaryRobotData data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.AtlasAuxiliaryRobotData data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getElectricJointTemperatures().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getElectricJointCurrents().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getElectricJointEnabledArray().size() * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += ((15) * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += ((15) * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getRawImuRates().length; ++i0)
      {
              current_alignment += geometry_msgs.msg.dds.Vector3PubSubType.getCdrSerializedSize(data.getRawImuRates()[i0], current_alignment);
      }
      for(int i0 = 0; i0 < data.getRawImuDeltas().length; ++i0)
      {
              current_alignment += geometry_msgs.msg.dds.Vector3PubSubType.getCdrSerializedSize(data.getRawImuDeltas()[i0], current_alignment);
      }
      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.AtlasAuxiliaryRobotData data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getElectricJointTemperatures().size() <= 6)
      cdr.write_type_e(data.getElectricJointTemperatures());else
          throw new RuntimeException("electric_joint_temperatures field exceeds the maximum length");

      if(data.getElectricJointCurrents().size() <= 6)
      cdr.write_type_e(data.getElectricJointCurrents());else
          throw new RuntimeException("electric_joint_currents field exceeds the maximum length");

      if(data.getElectricJointEnabledArray().size() <= 6)
      cdr.write_type_e(data.getElectricJointEnabledArray());else
          throw new RuntimeException("electric_joint_enabled_array field exceeds the maximum length");

      for(int i0 = 0; i0 < data.getRawImuTimestamps().length; ++i0)
      {
        	cdr.write_type_5(data.getRawImuTimestamps()[i0]);	
      }

      for(int i0 = 0; i0 < data.getRawImuPacketCounts().length; ++i0)
      {
        	cdr.write_type_5(data.getRawImuPacketCounts()[i0]);	
      }

      for(int i0 = 0; i0 < data.getRawImuRates().length; ++i0)
      {
        	geometry_msgs.msg.dds.Vector3PubSubType.write(data.getRawImuRates()[i0], cdr);		
      }

      for(int i0 = 0; i0 < data.getRawImuDeltas().length; ++i0)
      {
        	geometry_msgs.msg.dds.Vector3PubSubType.write(data.getRawImuDeltas()[i0], cdr);		
      }

      cdr.write_type_7(data.getBatteryCharging());

      cdr.write_type_5(data.getBatteryVoltage());

      cdr.write_type_5(data.getBatteryCurrent());

      cdr.write_type_5(data.getRemainingBatteryTime());

      cdr.write_type_5(data.getRemainingAmpHours());

      cdr.write_type_5(data.getRemainingChargePercentage());

      cdr.write_type_11(data.getBatteryCycleCount());

      cdr.write_type_5(data.getPumpInletPressure());

      cdr.write_type_5(data.getPumpSupplyPressure());

      cdr.write_type_5(data.getAirSumpPressure());

      cdr.write_type_5(data.getPumpSupplyTemperature());

      cdr.write_type_5(data.getPumpRpm());

      cdr.write_type_5(data.getMotorTemperature());

      cdr.write_type_5(data.getMotorDriverTemperature());

   }

   public static void read(controller_msgs.msg.dds.AtlasAuxiliaryRobotData data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_e(data.getElectricJointTemperatures());	
      cdr.read_type_e(data.getElectricJointCurrents());	
      cdr.read_type_e(data.getElectricJointEnabledArray());	
      for(int i0 = 0; i0 < data.getRawImuTimestamps().length; ++i0)
      {
        	data.getRawImuTimestamps()[i0] = cdr.read_type_5();
        	
      }
      	
      for(int i0 = 0; i0 < data.getRawImuPacketCounts().length; ++i0)
      {
        	data.getRawImuPacketCounts()[i0] = cdr.read_type_5();
        	
      }
      	
      for(int i0 = 0; i0 < data.getRawImuRates().length; ++i0)
      {
        	geometry_msgs.msg.dds.Vector3PubSubType.read(data.getRawImuRates()[i0], cdr);	
      }
      	
      for(int i0 = 0; i0 < data.getRawImuDeltas().length; ++i0)
      {
        	geometry_msgs.msg.dds.Vector3PubSubType.read(data.getRawImuDeltas()[i0], cdr);	
      }
      	
      data.setBatteryCharging(cdr.read_type_7());
      	
      data.setBatteryVoltage(cdr.read_type_5());
      	
      data.setBatteryCurrent(cdr.read_type_5());
      	
      data.setRemainingBatteryTime(cdr.read_type_5());
      	
      data.setRemainingAmpHours(cdr.read_type_5());
      	
      data.setRemainingChargePercentage(cdr.read_type_5());
      	
      data.setBatteryCycleCount(cdr.read_type_11());
      	
      data.setPumpInletPressure(cdr.read_type_5());
      	
      data.setPumpSupplyPressure(cdr.read_type_5());
      	
      data.setAirSumpPressure(cdr.read_type_5());
      	
      data.setPumpSupplyTemperature(cdr.read_type_5());
      	
      data.setPumpRpm(cdr.read_type_5());
      	
      data.setMotorTemperature(cdr.read_type_5());
      	
      data.setMotorDriverTemperature(cdr.read_type_5());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.AtlasAuxiliaryRobotData data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_e("electric_joint_temperatures", data.getElectricJointTemperatures());
      ser.write_type_e("electric_joint_currents", data.getElectricJointCurrents());
      ser.write_type_e("electric_joint_enabled_array", data.getElectricJointEnabledArray());
      ser.write_type_f("raw_imu_timestamps", data.getRawImuTimestamps());
      ser.write_type_f("raw_imu_packet_counts", data.getRawImuPacketCounts());
      ser.write_type_f("raw_imu_rates", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getRawImuRates());
      ser.write_type_f("raw_imu_deltas", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getRawImuDeltas());
      ser.write_type_7("battery_charging", data.getBatteryCharging());
      ser.write_type_5("battery_voltage", data.getBatteryVoltage());
      ser.write_type_5("battery_current", data.getBatteryCurrent());
      ser.write_type_5("remaining_battery_time", data.getRemainingBatteryTime());
      ser.write_type_5("remaining_amp_hours", data.getRemainingAmpHours());
      ser.write_type_5("remaining_charge_percentage", data.getRemainingChargePercentage());
      ser.write_type_11("battery_cycle_count", data.getBatteryCycleCount());
      ser.write_type_5("pump_inlet_pressure", data.getPumpInletPressure());
      ser.write_type_5("pump_supply_pressure", data.getPumpSupplyPressure());
      ser.write_type_5("air_sump_pressure", data.getAirSumpPressure());
      ser.write_type_5("pump_supply_temperature", data.getPumpSupplyTemperature());
      ser.write_type_5("pump_rpm", data.getPumpRpm());
      ser.write_type_5("motor_temperature", data.getMotorTemperature());
      ser.write_type_5("motor_driver_temperature", data.getMotorDriverTemperature());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.AtlasAuxiliaryRobotData data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_e("electric_joint_temperatures", data.getElectricJointTemperatures());
      ser.read_type_e("electric_joint_currents", data.getElectricJointCurrents());
      ser.read_type_e("electric_joint_enabled_array", data.getElectricJointEnabledArray());
      ser.read_type_f("raw_imu_timestamps", data.getRawImuTimestamps());
      ser.read_type_f("raw_imu_packet_counts", data.getRawImuPacketCounts());
      ser.read_type_f("raw_imu_rates", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getRawImuRates());
      ser.read_type_f("raw_imu_deltas", new geometry_msgs.msg.dds.Vector3PubSubType(), data.getRawImuDeltas());
      data.setBatteryCharging(ser.read_type_7("battery_charging"));
      data.setBatteryVoltage(ser.read_type_5("battery_voltage"));
      data.setBatteryCurrent(ser.read_type_5("battery_current"));
      data.setRemainingBatteryTime(ser.read_type_5("remaining_battery_time"));
      data.setRemainingAmpHours(ser.read_type_5("remaining_amp_hours"));
      data.setRemainingChargePercentage(ser.read_type_5("remaining_charge_percentage"));
      data.setBatteryCycleCount(ser.read_type_11("battery_cycle_count"));
      data.setPumpInletPressure(ser.read_type_5("pump_inlet_pressure"));
      data.setPumpSupplyPressure(ser.read_type_5("pump_supply_pressure"));
      data.setAirSumpPressure(ser.read_type_5("air_sump_pressure"));
      data.setPumpSupplyTemperature(ser.read_type_5("pump_supply_temperature"));
      data.setPumpRpm(ser.read_type_5("pump_rpm"));
      data.setMotorTemperature(ser.read_type_5("motor_temperature"));
      data.setMotorDriverTemperature(ser.read_type_5("motor_driver_temperature"));
   }

   public static void staticCopy(controller_msgs.msg.dds.AtlasAuxiliaryRobotData src, controller_msgs.msg.dds.AtlasAuxiliaryRobotData dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.AtlasAuxiliaryRobotData createData()
   {
      return new controller_msgs.msg.dds.AtlasAuxiliaryRobotData();
   }
   @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.AtlasAuxiliaryRobotData data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.AtlasAuxiliaryRobotData data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.AtlasAuxiliaryRobotData src, controller_msgs.msg.dds.AtlasAuxiliaryRobotData dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public AtlasAuxiliaryRobotDataPubSubType newInstance()
   {
      return new AtlasAuxiliaryRobotDataPubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/ScrewTheoryException.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

/**
 * {@code ScrewTheoryException} is used for handling exception thrown from the the screw theory
 * library.
 */
public class ScrewTheoryException extends RuntimeException
{
   private static final long serialVersionUID = -4504468232252668130L;

   public ScrewTheoryException(String message)
   {
      super(message);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/eventBasedStateMachine/FiniteStateMachineStateChangedListener.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.eventBasedStateMachine;

@Deprecated
public interface FiniteStateMachineStateChangedListener
{
   void stateHasChanged(Enum<?> oldState, Enum<?> newState);
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/controllerCore/RobotArmControllerCoreOptimizationSettings.java<|end_filename|>
package us.ihmc.exampleSimulations.controllerCore;

import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.ControllerCoreOptimizationSettings;
import us.ihmc.euclid.tuple2D.Vector2D;

public class RobotArmControllerCoreOptimizationSettings implements ControllerCoreOptimizationSettings
{
   private double jointAccelerationWeight = 0.005;
   private double jointJerkWeight = 2.5E-10;

   @Override
   public double getJointAccelerationWeight()
   {
      return jointAccelerationWeight;
   }

   @Override
   public double getJointJerkWeight()
   {
      return jointJerkWeight;
   }

   @Override
   public double getRhoWeight()
   {
      return 0;
   }

   @Override
   public double getRhoMin()
   {
      return 0;
   }

   @Override
   public double getRhoRateDefaultWeight()
   {
      return 0;
   }

   @Override
   public Vector2D getCoPWeight()
   {
      return new Vector2D();
   }

   @Override
   public Vector2D getCoPRateDefaultWeight()
   {
      return new Vector2D();
   }

   @Override
   public int getNumberOfBasisVectorsPerContactPoint()
   {
      return 0;
   }

   @Override
   public int getNumberOfContactPointsPerContactableBody()
   {
      return 0;
   }

   @Override
   public int getNumberOfContactableBodies()
   {
      return 0;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/PlanarRegionSegmentationMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import java.util.Arrays;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Point3D32;
import us.ihmc.euclid.tuple3D.Vector3D32;
import us.ihmc.jOctoMap.key.OcTreeKey;

public class PlanarRegionSegmentationMessage extends Packet<PlanarRegionSegmentationMessage>
{
   public int id;
   public Point3D32 origin;
   public Vector3D32 normal;
   public OcTreeKeyMessage[] nodeKeys;
   public Point3D32[] hitLocations;

   public PlanarRegionSegmentationMessage()
   {
   }

   @Override
   public void set(PlanarRegionSegmentationMessage other)
   {
      id = other.id;
      origin = new Point3D32(other.origin);
      normal = new Vector3D32(other.normal);
      nodeKeys = new OcTreeKeyMessage[other.nodeKeys.length];
      for (int i = 0; i < nodeKeys.length; i++)
      {
         nodeKeys[i] = new OcTreeKeyMessage();
         nodeKeys[i].set(other.nodeKeys[i]);
      }
      hitLocations = Arrays.stream(other.hitLocations).map(Point3D32::new).toArray(Point3D32[]::new);
      setPacketInformation(other);
   }

   public int getRegionId()
   {
      return id;
   }

   public Point3D32 getOrigin()
   {
      return origin;
   }

   public Vector3D32 getNormal()
   {
      return normal;
   }

   public int getNumberOfNodes()
   {
      return nodeKeys.length;
   }

   public OcTreeKeyMessage getNodeKey(int index)
   {
      return nodeKeys[index];
   }

   public Point3D32[] getHitLocations()
   {
      return hitLocations;
   }

   public Point3D32 getHitLocation(int index)
   {
      return hitLocations[index];
   }

   public void getNodeKey(int index, OcTreeKey nodeKeyToPack)
   {
      nodeKeyToPack.set(nodeKeys[index].k);
   }

   public void getHitLocation(int index, Point3D hitLocationToPack)
   {
      hitLocationToPack.set(hitLocations[index]);
   }

   @Override
   public boolean epsilonEquals(PlanarRegionSegmentationMessage other, double epsilon)
   {
      if (id != other.id)
         return false;
      if (!origin.epsilonEquals(other.origin, (float) epsilon))
         return false;
      if (!normal.epsilonEquals(other.normal, (float) epsilon))
         return false;
      if (!Arrays.equals(nodeKeys, other.nodeKeys))
         return false;
      if (hitLocations.length != other.hitLocations.length)
         return false;
      for (int i = 0; i < hitLocations.length; i++)
      {
         if (!hitLocations[i].epsilonEquals(other.hitLocations[i], (float) epsilon))
            return false;
      }
      return true;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/dataobjects/HandConfiguration.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.dataobjects;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;

/**
* @author twan
*         Date: 6/8/13
*/
public enum HandConfiguration
{
   @RosEnumValueDocumentation(documentation = "stops the fingers at their current position")
   STOP,
   @RosEnumValueDocumentation(documentation = "fully opens the fingers")
   OPEN,
   @RosEnumValueDocumentation(documentation = "fully closes the fingers")
   CLOSE,
   @RosEnumValueDocumentation(documentation = "fully closes the fingers applying maximum force")
   CRUSH,
   @RosEnumValueDocumentation(documentation = "closes all but one finger to create a hook")
   HOOK,
   @RosEnumValueDocumentation(documentation = "sets gripper to use a standard grasp")
   BASIC_GRIP,
   @RosEnumValueDocumentation(documentation = "sets gripper to use a pinch grasp where the thumb and fingers come together when closed")
   PINCH_GRIP,
   @RosEnumValueDocumentation(documentation = "sets gripper to use a wide-spread finger grasp")
   WIDE_GRIP,
   @RosEnumValueDocumentation(documentation = "sets gripper to use a scissor grasp where the index and middle finger come together when closed")
   SCISSOR_GRIP,
   @RosEnumValueDocumentation(documentation = "sets all fingers to their zero position")
   RESET,
   @RosEnumValueDocumentation(documentation = "fully open all fingers except the thumb")
   OPEN_FINGERS,
   @RosEnumValueDocumentation(documentation = "fully open the thumb only")
   OPEN_THUMB,
   @RosEnumValueDocumentation(documentation = "fully close all fingers except the thumb")
   CLOSE_FINGERS,
   @RosEnumValueDocumentation(documentation = "fully close the thumb only")
   CLOSE_THUMB,
   OPEN_INDEX,
   OPEN_MIDDLE,
   HALF_CLOSE,
   CONNECT,
   CRUSH_INDEX,
   CRUSH_MIDDLE,
   CRUSH_THUMB,
   INVERT_POWER,
   T_SPREAD,
   BEND_BACKWARD,
   CALIBRATE,
   FINGER_MANIPULATION,
   PRE_CREEPY_GRASP,
   PARTIAL_CREEPY_GRASP,
   CREEPY_GRASPING,
   CREEPY_GRASPING_HARD,
   SLOW_CLOSE;

   public final static HandConfiguration[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static HandConfiguration fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/WalkToLocationTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.humanoidBehaviors.behaviors.primitives.WalkToLocationBehavior;
import us.ihmc.humanoidBehaviors.behaviors.primitives.WalkToLocationBehavior.WalkingOrientation;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class WalkToLocationTask extends BehaviorAction
{
   private final FramePose2D targetPoseInWorld;
   private final WalkToLocationBehavior walkToLocationBehavior;

   private final double orientationRelativeToPathDirection;
   private final WalkingOrientation walkingOrientation;
   private double footstepLength;

   private double transferTime;
   private double swingTime;

   public WalkToLocationTask(FramePose2D targetPoseInWorld, WalkToLocationBehavior walkToLocationBehavior, double orientationRelativeToPathDirection,
                             double footstepLength, double swingTime, double transferTime)
   {
      super(walkToLocationBehavior);
      this.targetPoseInWorld = new FramePose2D(targetPoseInWorld);
      this.walkToLocationBehavior = walkToLocationBehavior;
      this.orientationRelativeToPathDirection = orientationRelativeToPathDirection;
      this.walkingOrientation = WalkingOrientation.CUSTOM;
      this.footstepLength = footstepLength;
      this.swingTime = swingTime;
      this.transferTime = transferTime;
   }

   public WalkToLocationTask(FramePose2D targetPoseInWorld, WalkToLocationBehavior walkToLocationBehavior, double orientationRelativeToPathDirection,
                             double footstepLength)
   {
      this(targetPoseInWorld, walkToLocationBehavior, orientationRelativeToPathDirection, footstepLength, Double.NaN, Double.NaN);
   }

   public WalkToLocationTask(FramePose2D targetPoseInWorld, WalkToLocationBehavior walkToLocationBehavior, WalkingOrientation walkingOrientation,
                             double footstepLength)
   {
      super(walkToLocationBehavior);
      this.targetPoseInWorld = new FramePose2D(targetPoseInWorld);
      this.walkToLocationBehavior = walkToLocationBehavior;
      this.orientationRelativeToPathDirection = 0.0;
      this.walkingOrientation = walkingOrientation;
      this.footstepLength = footstepLength;
   }

   @Override
   protected void setBehaviorInput()
   {
      if (walkingOrientation.equals(WalkingOrientation.CUSTOM))
      {
         walkToLocationBehavior.setWalkingOrientationRelativeToPathDirection(orientationRelativeToPathDirection);
      }
      walkToLocationBehavior.setFootstepLength(footstepLength);
      if (!Double.isNaN(swingTime))
      {
         walkToLocationBehavior.setSwingTime(swingTime);
      }
      if (!Double.isNaN(transferTime))
      {
         walkToLocationBehavior.setTransferTime(transferTime);
      }
      walkToLocationBehavior.setTarget(targetPoseInWorld, walkingOrientation);
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/subscriber/RosJointStateSubscriber.java<|end_filename|>
package us.ihmc.utilities.ros.subscriber;

import sensor_msgs.JointState;

public class RosJointStateSubscriber extends AbstractRosTopicSubscriber<sensor_msgs.JointState>
{
	public RosJointStateSubscriber()
	{
		super(sensor_msgs.JointState._TYPE);
	}

	@Override
	public void onNewMessage(JointState message)
	{
		
	}
	
	@Override
	public void connected()
	{
		
	}

}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasJointPrivilegedConfigurationParameters.java<|end_filename|>
package us.ihmc.atlas.parameters;

import us.ihmc.commonWalkingControlModules.configurations.JointPrivilegedConfigurationParameters;

public class AtlasJointPrivilegedConfigurationParameters extends JointPrivilegedConfigurationParameters
{
   private final boolean runningOnRealRobot;

   public AtlasJointPrivilegedConfigurationParameters(boolean runningOnRealRobot)
   {
      this.runningOnRealRobot = runningOnRealRobot;
   }

   /** {@inheritDoc} */
   @Override
   public double getDefaultConfigurationGain()
   {
      return 40.0;
   }

   /** {@inheritDoc} */
   @Override
   public double getDefaultVelocityGain()
   {
      return 6.0;
   }

   /** {@inheritDoc} */
   @Override
   public double getDefaultMaxVelocity()
   {
      return 2.0;
   }

   /** {@inheritDoc} */
   @Override
   public double getDefaultMaxAcceleration()
   {
      return Double.POSITIVE_INFINITY;
   }

   /** {@inheritDoc} */
   @Override
   public double getDefaultWeight()
   {
      return 5.0;
   }
}


<|start_filename|>robot-environment-awareness/src/visualizers/java/us/ihmc/robotEnvironmentAwareness/geometry/IntersectionPlaneBoxCalculatorVisualizer.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.geometry;

import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;

import javafx.application.Application;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.Box;
import javafx.scene.shape.MeshView;
import javafx.stage.Stage;
import us.ihmc.javaFXToolkit.scenes.View3DFactory;
import us.ihmc.javaFXToolkit.shapes.JavaFXMultiColorMeshBuilder;
import us.ihmc.javaFXToolkit.shapes.TextureColorPalette1D;

public class IntersectionPlaneBoxCalculatorVisualizer extends Application
{
   private final TextureColorPalette1D colorPalette = new TextureColorPalette1D();
   private final JavaFXMultiColorMeshBuilder colorMeshBuilder = new JavaFXMultiColorMeshBuilder(colorPalette);
   private final IntersectionPlaneBoxCalculator calculator = new IntersectionPlaneBoxCalculator();
   private final Box box;
//   private final Cylinder normalCylinder;

   public IntersectionPlaneBoxCalculatorVisualizer()
   {
      double lx = 0.1;
      double ly = 0.1;
      double lz = 0.1;

      Point3D cubeCenter = new Point3D(-0.25, -0.45, -0.05);
      Point3D pointOnPlane = new Point3D(-0.2242894023656845, -0.4647734463214874, -0.0023258039727807045);
      Vector3D planeNormal = new Vector3D(0.20791170661191224, 1.503689309739766E-8, 0.9781475973766547);

//
//      pointOnPlane.sub(cubeCenter);
//      cubeCenter.set(0.0, 0.0, 0.0);

      calculator.setBox(lx, ly, lz, cubeCenter);
      calculator.setPlane(pointOnPlane, planeNormal);

      colorPalette.setHueBased(0.9, 0.8);

      List<Point3D> intersections = calculator.computeIntersections();
      System.out.println(intersections);
      
      colorMeshBuilder.addPolyon(intersections, Color.DARKCYAN);
      for (int index = 0; index < intersections.size(); index++)
         colorMeshBuilder.addCube(0.01, intersections.get(index), Color.FIREBRICK);
      colorMeshBuilder.addCube(0.02, pointOnPlane, Color.SLATEGREY);
      box = new Box(lx, ly, lz);
      box.setTranslateX(cubeCenter.getX());
      box.setTranslateY(cubeCenter.getY());
      box.setTranslateZ(cubeCenter.getZ());

//      normalCylinder = new Cylinder(0.01, 0.3);
//      normalCylinder.setTranslateX(pointOnPlane.getX());
//      normalCylinder.setTranslateY(pointOnPlane.getY() + 0.5 * normalCylinder.getHeight());
//      normalCylinder.setTranslateZ(pointOnPlane.getZ());
//      AxisAngle4d axisAngle = new AxisAngle4d();
//      GeometryTools.getRotationBasedOnNormal(axisAngle, planeNormal, new Vector3d(0.0, 1.0, 0.0));
//      Affine affine = new Affine();
//      JavaFXTools.convertAxisAngleToAffine(axisAngle, affine);
//      normalCylinder.getTransforms().add(affine);
//      
      for (int i = 0; i < intersections.size(); i++)
      {

         Point3D intersection = intersections.get(i);
         Vector3D v0 = new Vector3D();
         Vector3D v1 = new Vector3D();
         Vector3D v3 = new Vector3D();
         Point3D nextIntersection = intersections.get((i + 1) % intersections.size());
         Point3D previousIntersection = intersections.get(i == 0 ? intersections.size() - 1 : i - 1);
         v0.sub(intersection, nextIntersection);
         v1.sub(intersection, previousIntersection);
         v3.cross(v0, v1);
         System.out.println(v3.dot(planeNormal) < 0.0);
      }
   }

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      View3DFactory view3dFactory = new View3DFactory(800, 600);
      view3dFactory.addCameraController();
      view3dFactory.addWorldCoordinateSystem(0.3);


      MeshView meshView = new MeshView();
      meshView.setMesh(colorMeshBuilder.generateMesh());
      meshView.setMaterial(colorMeshBuilder.generateMaterial());
      view3dFactory.addNodeToView(meshView);

      PhongMaterial material = new PhongMaterial(Color.DARKCYAN);
//      normalCylinder.setMaterial(material);
//      rootNode.getChildren().add(normalCylinder);
      material = new PhongMaterial();
      material.setDiffuseColor(new Color(1.0, 1.0, 0.0, 0.0));
      box.setMaterial(material);
      view3dFactory.addNodeToView(box);

      primaryStage.setScene(view3dFactory.getScene());
      primaryStage.show();
   }

   public static void main(String[] args)
   {
      Application.launch(args);
   }
}


<|start_filename|>ihmc-simulation-toolkit/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/SDFNoisySimulatedSensorReader.java<|end_filename|>
package us.ihmc.sensorProcessing.simulatedSensors;

import java.util.Random;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.sensorProcessing.frames.CommonHumanoidReferenceFrames;
import us.ihmc.simulationconstructionset.FloatingRootJointRobot;

public class SDFNoisySimulatedSensorReader extends SDFPerfectSimulatedSensorReader
{

   private final boolean addNoiseFiltering = true;
   private double noiseFilterAlpha = 1e-1;
   private double quaternionNoiseStd = 0.01;
   private double positionNoiseStd = 0.01;

   private final Random rand = new Random(124381L);
   private final Quaternion rotationError = new Quaternion();
   private final Vector3D positionError = new Vector3D();
   private final Quaternion rotationFilter = new Quaternion();
   private final Vector3D positionFilter = new Vector3D();

   public SDFNoisySimulatedSensorReader(FloatingRootJointRobot robot, FullRobotModel fullRobotModel, CommonHumanoidReferenceFrames referenceFrames)
   {
      super(robot, fullRobotModel, referenceFrames);
   }

   public void setNoiseFilterAlpha(double noiseFilterAlpha)
   {
      this.noiseFilterAlpha = noiseFilterAlpha;
   }

   public void setQuaternionNoiseStd(double quaternionNoiseStd)
   {
      this.quaternionNoiseStd = quaternionNoiseStd;
   }

   public void setPositionNoiseStd(double positionNoiseStd)
   {
      this.positionNoiseStd = positionNoiseStd;
   }

   @Override
   protected void packRootTransform(FloatingRootJointRobot robot, RigidBodyTransform transformToPack)
   {
      super.packRootTransform(robot, transformToPack);
      rotationError.set(rand.nextGaussian() * quaternionNoiseStd, rand.nextGaussian() * quaternionNoiseStd, rand.nextGaussian() * quaternionNoiseStd, 1);
      rotationError.normalize();

      positionError.setX(rand.nextGaussian() * positionNoiseStd);
      positionError.setY(rand.nextGaussian() * positionNoiseStd);
      positionError.setZ(rand.nextGaussian() * positionNoiseStd);

      RigidBodyTransform disturbanceTransform = new RigidBodyTransform();
      if (addNoiseFiltering)
      {
         double alpha = noiseFilterAlpha;
         // TODO reimplement me
//         rotationFilter.scale(1 - alpha);
//         rotationError.scale(alpha);
//         rotationFilter.add(rotationError);
//         rotationFilter.normalize();

         positionFilter.scale(1 - alpha);
         positionError.scale(alpha);
         positionFilter.add(positionError);
         disturbanceTransform.set(rotationFilter, positionFilter);
      }
      else
      {
         disturbanceTransform.set(rotationError, positionError);
      }

      transformToPack.multiply(disturbanceTransform);

   }

}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosOdometryPublisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import org.ros.message.Time;

import geometry_msgs.Point;
import geometry_msgs.Pose;
import geometry_msgs.PoseWithCovariance;
import geometry_msgs.Quaternion;
import geometry_msgs.Twist;
import geometry_msgs.TwistWithCovariance;
import geometry_msgs.Vector3;
import nav_msgs.Odometry;
import std_msgs.Header;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D32;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.utilities.ros.RosTools;

public class RosOdometryPublisher extends RosTopicPublisher<nav_msgs.Odometry>
{
   private int counter=0;
   public RosOdometryPublisher(boolean latched)
   {
      super(nav_msgs.Odometry._TYPE,latched);
   }
   
   public void publish(long timestamp, RigidBodyTransform transform, Vector3DReadOnly linearVelocity, Vector3DReadOnly angularVelocity, String childFrame, String frameId)
   {
      Odometry message = getMessage();

      Header header = createHeaderMsg(timestamp);
      header.setFrameId(frameId);
      message.setHeader(header);
      
      PoseWithCovariance poseWithCovariance = createPoseWithCovarianceMsg(transform);
      message.setPose(poseWithCovariance);
      
      TwistWithCovariance twistWithCovariance = createTwistWithCovariance(linearVelocity, angularVelocity);
      message.setTwist(twistWithCovariance);
      
      message.setChildFrameId(childFrame);
      
      publish(message);
   }

   private TwistWithCovariance createTwistWithCovariance(Vector3DReadOnly linearVelocity, Vector3DReadOnly angularVelocity)
   {
      TwistWithCovariance twistWithCovariance = newMessageFromType(TwistWithCovariance._TYPE);
      Twist twist = createTwistMsg(linearVelocity, angularVelocity);
      twistWithCovariance.setTwist(twist);
      return twistWithCovariance;
   }

   private Twist createTwistMsg(Vector3DReadOnly linearVelocity, Vector3DReadOnly angularVelocity)
   {
      Twist twist = newMessageFromType(Twist._TYPE);
      
      Vector3 angularVelocityAsRosVector = getVector3(angularVelocity);
      twist.setAngular(angularVelocityAsRosVector);
      
      Vector3 linearVelocityAsRosVector = getVector3(linearVelocity);
      twist.setLinear(linearVelocityAsRosVector);
      
      return twist;
   }

   private Vector3 getVector3(Vector3DReadOnly angularVelocity)
   {
      Vector3 rosVector3 = newMessageFromType(Vector3._TYPE);
      RosTools.packEuclidTuple3DToGeometry_msgsVector3(angularVelocity, rosVector3);
      return rosVector3;
   }

   private Header createHeaderMsg(long timestamp)
   {
      Header header = newMessageFromType(Header._TYPE);
      header.setStamp(Time.fromNano(timestamp));
      header.setSeq(counter);
      counter++;
      return header;
   }

   private PoseWithCovariance createPoseWithCovarianceMsg(RigidBodyTransform transform)
   {
      PoseWithCovariance poseWithCovariance = newMessageFromType(PoseWithCovariance._TYPE);
      Pose pose = createPoseMsg(transform);
      poseWithCovariance.setPose(pose);
      return poseWithCovariance;
   }

   private Pose createPoseMsg(RigidBodyTransform transform)
   {
      Pose pose = newMessageFromType(geometry_msgs.Pose._TYPE);
      Point point = newMessageFromType(Point._TYPE);
      Quaternion rotation = newMessageFromType(Quaternion._TYPE);
      pose.setPosition(point);
      pose.setOrientation(rotation);
      RosTools.packEuclidRigidBodyTransformToGeometry_msgsPose(transform, pose);
      return pose;
   }
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/activationFunction/PassThrough.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction;

/**
 * The output = the input 
 * Useful for the input Neurons
 */
public class PassThrough implements ActivationFunction
{

   @Override
   public double computeOutput(double input)
   {
      return input;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/RootsOfUnity.java<|end_filename|>
package us.ihmc.robotics.math;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.robotics.dataStructures.ComplexNumber;

/**
 * Garbage free implementation for generating the nth roots of unity 
 */
public class RootsOfUnity
{
   private static final ComplexNumber unity = new ComplexNumber(1.0, 0.0);
   private static List<ComplexNumber> rootsOfUnity = new ArrayList<>();
   public static List<ComplexNumber> getRootsOfUnity(int orderOfRoots)
   {      
      for(int i = 0; i < orderOfRoots; i++)
         rootsOfUnity.add(new ComplexNumber());
      unity.getRoots(rootsOfUnity, orderOfRoots);
      return rootsOfUnity;
   }
}


<|start_filename|>websitedocs/website/node_modules/docusaurus/.eslintrc.js<|end_filename|>
const OFF = 0;
const WARNING = 1;
const ERROR = 2;

module.exports = {
  extends: [
    'fbjs', // eslint-config-fbjs
    'prettier' // eslint-config-prettier
  ],
  overrides: [
    {
      files: ['lib/**/*.js'],
      rules: {
        // allow console
        'no-console': OFF,
        // require radix argument in parseInt
        'radix': ERROR,
        // disallow unused vars
        'no-unused-vars': ERROR,
        // almost certainly a bug
        'no-duplicate-case': ERROR,
      }
    },
    {
      files: ['examples/**/*.js'],
      rules: {
        'no-unused-vars': OFF
      }
    }
  ]
};

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/filters/ZeroLagLowPassFilter.java<|end_filename|>
package us.ihmc.robotics.filters;

import java.util.ArrayList;
import java.util.Arrays;

import org.apache.commons.lang3.ArrayUtils;

import us.ihmc.commons.MathTools;

public class ZeroLagLowPassFilter
{
   private static final int numberFactor = 3; //This is from the Matlab algorithm
   
   /**
    * This is a zero phase lag filter with the same performance and the Matlab filtfilt function
    * when the filter type is a first order low pass filter. This will extrapolate beginning and end of data sequence using a "reflection method".  
    * Slopes of original and extrapolated sequences match at the end points. This reduces end effects.
    * @param arrayToFilter
    * @param alpha this is [0, 1]. 0 = no filter, 1= all filter.
    * @return
    */
   
   public static ArrayList<Double> getFilteredArray(ArrayList<Double> arrayToFilter, double alpha)
   {
       double[] filteredArray = getFilteredArray(ArrayUtils.toPrimitive(arrayToFilter.toArray(new Double[arrayToFilter.size()])), alpha);
      return new ArrayList<Double>(Arrays.asList(ArrayUtils.toObject(filteredArray)));
      
   }
   
   public static double[] getFilteredArray(double[] arrayToFilter, double alpha)
   {
      if (arrayToFilter.length < 3)
         return Arrays.copyOf(arrayToFilter, arrayToFilter.length);
      
      alpha = MathTools.clamp(alpha, 0.0, 1.0);

      ArrayList<Double> arrayListToFilter = new ArrayList<>(Arrays.asList(ArrayUtils.toObject(arrayToFilter)));
      
      int originalArrayLength = arrayListToFilter.size();
   
      //Extrapolate at the ends
      preAndPostPendedToList(arrayListToFilter);
      
      double[] listToFilter = new double[arrayListToFilter.size()];
      
      for(int i=0; i<listToFilter.length; i++)
      {
         listToFilter[i] = arrayListToFilter.get(i);
      }
              
      double[] filteredList = new double[listToFilter.length];
      
      filteredList[0] = listToFilter[0];

      for (int i = 1; i < filteredList.length; i++)
      {
         filteredList[i] = alpha * filteredList[i - 1] + (1.0 - alpha) * listToFilter[i];
      }
 
      double[] reverseOrderFilteredList = getReserveredOrderedArrayCopy(filteredList);
      
      filteredList[0] = reverseOrderFilteredList[0];

      for (int i = 1; i < filteredList.length; i++)
      {
         filteredList[i] = alpha * filteredList[i - 1] + (1.0 - alpha) * reverseOrderFilteredList[i];
      }
      
      filteredList = getReserveredOrderedArrayCopy(filteredList);
   
      //trim beginning and end
      double[] ret = Arrays.copyOfRange(filteredList, numberFactor, originalArrayLength + numberFactor);
      
      return ret;

   }
   
   private static void preAndPostPendedToList(ArrayList<Double> list)
   {
      
     
      double startValue = list.get(0);
      for(int i= 0; i<numberFactor; i++)
      {
         double value = 2.0 * startValue - list.get(numberFactor);
         list.add(i, value);
      }
      
      double endValue = list.get(list.size()-1);
      int startingIndex = list.size() - 2;
      for(int i= 0; i<numberFactor; i++)
      {
         double value = 2.0 * endValue - list.get(startingIndex - i);
         list.add(value);
      }
   }
   
   public static void main(String[] args)
   {
      ArrayList<Double> list = new ArrayList<Double>();
      
      double[] newList = new double[10];
      for(int i=0; i<10; i++)
      {
         list.add((double)i + 1);
         newList[i]=i+ 1;
      }
      
//      System.out.println("Original List:");
//      ArrayTools.printArray(newList, System.out);
//      System.out.println("");
      
//      ZeroLagLowPassFilter.preAndPostPendedToList(list);
//      
//      ArrayTools.printArray(list, System.out);

      double[] filteredList = ZeroLagLowPassFilter.getFilteredArray(newList, 0.5);
      
      System.out.println(Arrays.toString(filteredList));
   }

      
   private static double[] getReserveredOrderedArrayCopy(double[] arrayToReverseAndCopy)
   {
      int length = arrayToReverseAndCopy.length;
      double[] ret = new double[length];

      for (int i = 0; i < length; i++)
      {
         ret[i] = arrayToReverseAndCopy[length - i - 1];
      }

      return ret;
   }


}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ICPControlGainsProvider.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.capturePoint;

import us.ihmc.yoVariables.providers.DoubleProvider;

public interface ICPControlGainsProvider extends ICPControlGainsReadOnly
{
   DoubleProvider getYoKpParallelToMotion();

   DoubleProvider getYoKpOrthogonalToMotion();

   DoubleProvider getYoKi();

   DoubleProvider getYoIntegralLeakRatio();

   DoubleProvider getYoMaxIntegralError();

   DoubleProvider getYoFeedbackPartMaxRate();

   DoubleProvider getYoFeedbackPartMaxValueParallelToMotion();

   DoubleProvider getYoFeedbackPartMaxValueOrthogonalToMotion();

   @Override
   default double getKpParallelToMotion()
   {
      return getYoKpParallelToMotion().getValue();
   }

   @Override
   default double getKpOrthogonalToMotion()
   {
      return getYoKpOrthogonalToMotion().getValue();
   }

   @Override
   default double getKi()
   {
      return getYoKi().getValue();
   }

   @Override
   default double getIntegralLeakRatio()
   {
      return getYoIntegralLeakRatio().getValue();
   }

   @Override
   default double getMaxIntegralError()
   {
      return getYoMaxIntegralError().getValue();
   }

   @Override
   default double getFeedbackPartMaxRate()
   {
      return getYoFeedbackPartMaxRate().getValue();
   }

   @Override
   default double getFeedbackPartMaxValueParallelToMotion()
   {
      return getYoFeedbackPartMaxValueParallelToMotion().getValue();
   }

   @Override
   default double getFeedbackPartMaxValueOrthogonalToMotion()
   {
      return getYoFeedbackPartMaxValueOrthogonalToMotion().getValue();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/FootstepAdjustment.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator;

import us.ihmc.euclid.referenceFrame.interfaces.FramePose2DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly;
import us.ihmc.robotics.robotSide.RobotSide;

/**
 * This interface is part of the API of {@link ContinuousStepGenerator}.
 * <p>
 * It is used to provide a protocol for adjusting generated footsteps.
 * </p>
 * 
 * @author <NAME>
 */
public interface FootstepAdjustment
{
   /**
    * Adjusts the footstep height, pitch, and roll.
    * 
    * @param footstepPose the generated footstep from the {@code ContinuousStepGenerator}. Height,
    *           pitch, and roll have to be computed.
    * @param footSide indicates for which foot the footstep is meant to be.
    * @return the adjusted footstep.
    */
   FramePose3DReadOnly adjustFootstep(FramePose2DReadOnly footstepPose, RobotSide footSide);
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/time/AlwaysZeroOffsetPPSTimestampOffsetProvider.java<|end_filename|>
package us.ihmc.ihmcPerception.time;

import controller_msgs.msg.dds.RobotConfigurationData;
import us.ihmc.humanoidRobotics.kryo.PPSTimestampOffsetProvider;

public class AlwaysZeroOffsetPPSTimestampOffsetProvider implements PPSTimestampOffsetProvider
{

   public AlwaysZeroOffsetPPSTimestampOffsetProvider()
   {
      super();
   }

   public long getCurrentTimestampOffset()
   {
      return 0;
   }

   public long adjustTimeStampToRobotClock(long timeStamp)
   {
      return timeStamp;
   }

   public boolean offsetIsDetermined()
   {
      return true;
   }

   @Override
   public long adjustRobotTimeStampToRosClock(long timeStamp)
   {
      return timeStamp;
   }

   @Override
   public void receivedPacket(RobotConfigurationData packet)
   {
      
   }

}

<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/CapturabilityBasedStatusListener.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import controller_msgs.msg.dds.CapturabilityBasedStatus;

public interface CapturabilityBasedStatusListener
{
   public abstract void updateStatusPacket(CapturabilityBasedStatus capturabilityBasedStatus);
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/cart/TriWheelCartSimulation.java<|end_filename|>
package us.ihmc.exampleSimulations.cart;

import java.util.ArrayList;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.exampleSimulations.newtonsCradle.GroundAsABoxRobot;
import us.ihmc.exampleSimulations.newtonsCradle.PileOfRandomObjectsRobot;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.SimulationConstructionSetParameters;
import us.ihmc.simulationconstructionset.physics.CollisionHandler;
import us.ihmc.simulationconstructionset.physics.collision.DefaultCollisionHandler;
import us.ihmc.simulationconstructionset.physics.collision.simple.CollisionManager;

public class TriWheelCartSimulation
{
   public TriWheelCartSimulation()
   {
      double dt = 0.001;

      TriWheelCartDescription robotDescription = new TriWheelCartDescription("triWheel");

      Vector3D startingPoint = new Vector3D(-2.0, 0.0, robotDescription.getInitialHeight());

      SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters();
      parameters.setCreateGUI(true);
      parameters.setDataBufferSize(8000);

      SimulationConstructionSet scs = new SimulationConstructionSet(parameters);

      Graphics3DObject staticLinkGraphics = new Graphics3DObject();
      staticLinkGraphics.addCoordinateSystem(0.1);
      scs.addStaticLinkGraphics(staticLinkGraphics);

      // create robot.
      Robot triWheelRobot = scs.addRobot(robotDescription);
      FloatingJoint floatingJoint = (FloatingJoint) triWheelRobot.getRootJoints().get(0);
      floatingJoint.setPosition(startingPoint);

      // robot controller.
      TriWheelCartController controller = new TriWheelCartController(triWheelRobot);
      triWheelRobot.setController(controller);

      // obstacles.
      PileOfRandomObjectsRobot pileOfRandomObjectsRobot = new PileOfRandomObjectsRobot();
      pileOfRandomObjectsRobot.setSizeScale(2.0);
      pileOfRandomObjectsRobot.setGroupAndCollisionMask(0xffff, 0xffff);
      pileOfRandomObjectsRobot.setNumberOfObjects(100);
      pileOfRandomObjectsRobot.setXYExtents(1.0, 1.0);
      pileOfRandomObjectsRobot.setZMinAndMax(1.0, 2.0);
      ArrayList<Robot> robots = pileOfRandomObjectsRobot.createAndGetRobots();
      for (Robot robot : robots)
         scs.addRobot(robot);

      // ground.
      int estimatedNumberOfContactPoints = 100;
      GroundAsABoxRobot groundAsABoxRobot = new GroundAsABoxRobot();
      groundAsABoxRobot.setFloorLength(15.0);
      groundAsABoxRobot.setFloorWidth(15.0);
      groundAsABoxRobot.setEstimatedNumberOfContactPoints(estimatedNumberOfContactPoints);
      groundAsABoxRobot.setAddWalls(false);
      groundAsABoxRobot.setCollisionMask(0xff);

      scs.addRobot(groundAsABoxRobot.createRobot());
      scs.setGroundVisible(false);

      // simulate.
      double coefficientOfRestitution = 0.2;
      double coefficientOfFriction = 0.7;
      CollisionHandler collisionHandler = new DefaultCollisionHandler(coefficientOfRestitution, coefficientOfFriction);
      CollisionManager collisionManager = new CollisionManager(null, collisionHandler);
      scs.initializeShapeCollision(collisionManager);

      scs.setDT(dt, 1);
      scs.setFastSimulate(true);

      scs.setCameraFix(0.0, 0.0, 0.8);
      scs.setCameraPosition(0.0, -8.0, 1.4);
      scs.startOnAThread();
   }

   public static void main(String[] args)
   {
      new TriWheelCartSimulation();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PilotAlarmPacket.java<|end_filename|>
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;

public class PilotAlarmPacket extends Packet<PilotAlarmPacket> implements Settable<PilotAlarmPacket>, EpsilonComparable<PilotAlarmPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public double beep_rate_;
   public boolean enable_tone_;

   public PilotAlarmPacket()
   {
   }

   public PilotAlarmPacket(PilotAlarmPacket other)
   {
      this();
      set(other);
   }

   public void set(PilotAlarmPacket other)
   {
      sequence_id_ = other.sequence_id_;

      beep_rate_ = other.beep_rate_;

      enable_tone_ = other.enable_tone_;

   }

   /**
            * 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 void setBeepRate(double beep_rate)
   {
      beep_rate_ = beep_rate;
   }
   public double getBeepRate()
   {
      return beep_rate_;
   }

   public void setEnableTone(boolean enable_tone)
   {
      enable_tone_ = enable_tone;
   }
   public boolean getEnableTone()
   {
      return enable_tone_;
   }


   public static Supplier<PilotAlarmPacketPubSubType> getPubSubType()
   {
      return PilotAlarmPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PilotAlarmPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PilotAlarmPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.beep_rate_, other.beep_rate_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_tone_, other.enable_tone_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PilotAlarmPacket)) return false;

      PilotAlarmPacket otherMyClass = (PilotAlarmPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.beep_rate_ != otherMyClass.beep_rate_) return false;

      if(this.enable_tone_ != otherMyClass.enable_tone_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PilotAlarmPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("beep_rate=");
      builder.append(this.beep_rate_);      builder.append(", ");
      builder.append("enable_tone=");
      builder.append(this.enable_tone_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/StateTransition.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.yoVariables.providers.DoubleProvider;

@Deprecated
public class StateTransition<E extends Enum<E>>
{
   private final E nextStateEnum;
   private final ArrayList<StateTransitionCondition> stateTransitionConditions = new ArrayList<StateTransitionCondition>();
   private ArrayList<DoubleProvider> timePassedYoVariables;
   private final ArrayList<StateTransitionAction> actions = new ArrayList<StateTransitionAction>();

   public StateTransition(E nextStateEnum, DoubleProvider timePassedYoVariable, StateTransitionCondition condition, StateTransitionAction action)
   {
      if (nextStateEnum == null)
         throw new RuntimeException("Cannot create StateTransition with null nextStateEnum!");

      this.nextStateEnum = nextStateEnum;
      if (condition != null)
         this.stateTransitionConditions.add(condition);
      if (action != null)
         this.actions.add(action);
      if (timePassedYoVariable != null)
         addTimePassedCondition(timePassedYoVariable);
   }

   public StateTransition(E nextStateEnum, StateTransitionCondition condition, StateTransitionAction action)
   {
      this(nextStateEnum, null, condition, action);
   }

   public StateTransition(E nextStateEnum, DoubleProvider timePassedYoVariable)
   {
      this(nextStateEnum, timePassedYoVariable, null, null);
   }

   public StateTransition(E nextStateEnum, StateTransitionCondition condition)
   {
      this(nextStateEnum, condition, (StateTransitionAction) null);
   }

   public StateTransition(E nextStateEnum, ArrayList<StateTransitionCondition> stateTransitionConditions, StateTransitionAction action)
   {
      this.nextStateEnum = nextStateEnum;
      this.stateTransitionConditions.addAll(stateTransitionConditions);
      if (action != null)
         this.actions.add(action);
   }

   public StateTransition(E nextStateEnum, StateTransitionCondition stateTransitionCondition, List<StateTransitionAction> actions)
   {
      this.nextStateEnum = nextStateEnum;
      if (stateTransitionCondition != null)
         this.stateTransitionConditions.add(stateTransitionCondition);
      this.actions.addAll(actions);
   }

   public StateTransition(E nextStateEnum, List<StateTransitionCondition> stateTransitionCondition, List<StateTransitionAction> actions)
   {
      this.nextStateEnum = nextStateEnum;
      this.stateTransitionConditions.addAll(stateTransitionCondition);
      this.actions.addAll(actions);
   }

   public StateTransition(E nextStateEnum, ArrayList<StateTransitionCondition> stateTransitionConditions)
   {
      this(nextStateEnum, stateTransitionConditions, (StateTransitionAction) null);
   }

   public void addStateTransitionCondition(StateTransitionCondition transitionCondition)
   {
      if (transitionCondition != null)
         stateTransitionConditions.add(transitionCondition);
   }

   public void addTimePassedCondition(DoubleProvider timePassedYoVariable)
   {
      if (timePassedYoVariables == null)
         timePassedYoVariables = new ArrayList<DoubleProvider>();
      if (timePassedYoVariable != null)
         timePassedYoVariables.add(timePassedYoVariable);
   }

   final E checkTransitionConditions(double timeInState)
   {
      for (int i = 0; i < stateTransitionConditions.size(); i++)
      {
         StateTransitionCondition stateTransitionCondition = stateTransitionConditions.get(i);
         if (!stateTransitionCondition.checkCondition())
         {
            return null;
         }
      }

      if (timePassedYoVariables == null)
         return nextStateEnum;

      for (DoubleProvider timePassedYoVariable : timePassedYoVariables)
      {
         if (timeInState < timePassedYoVariable.getValue())
         {
            return null;
         }
      }

      return nextStateEnum;
   }

   final void doAction()
   {
      for (int i = 0; i < actions.size(); i++)
      {
         actions.get(i).doTransitionAction();
      }
   }

   public E getNextStateEnum()
   {
      return nextStateEnum;
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/collidingArms/SingleBoxRobotDescription.java<|end_filename|>
package us.ihmc.exampleSimulations.collidingArms;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.robotics.robotDescription.CollisionMeshDescription;
import us.ihmc.robotics.robotDescription.FloatingJointDescription;
import us.ihmc.robotics.robotDescription.GroundContactPointDescription;
import us.ihmc.robotics.robotDescription.LinkDescription;
import us.ihmc.robotics.robotDescription.LinkGraphicsDescription;
import us.ihmc.robotics.robotDescription.RobotDescription;

public class SingleBoxRobotDescription
{
   private String name;
   private double mass = 0.6;
   private double xLength = 0.2;
   private double yWidth = 0.5;
   private double zHeight = 0.5;
   private double radiusOfGyrationPercent = 1.0;

   private int collisionGroup = 0xffff;
   private int collisionMask = 0xffff;
   private int estimatedNumberOfContactPoints = 6;

   private AppearanceDefinition appearance = YoAppearance.Aqua();

   public SingleBoxRobotDescription()
   {
   }

   public void setName(String name)
   {
      this.name = name;
   }

   public void setMass(double mass)
   {
      this.mass = mass;
   }

   public void setXLength(double xLength)
   {
      this.xLength = xLength;
   }

   public void setYWidth(double yWidth)
   {
      this.yWidth = yWidth;
   }

   public void setZHeight(double zHeight)
   {
      this.zHeight = zHeight;
   }

   public void setRadiusOfGyrationPercent(double radiusOfGyrationPercent)
   {
      this.radiusOfGyrationPercent = radiusOfGyrationPercent;
   }

   public void setCollisionGroup(int collisionGroup)
   {
      this.collisionGroup = collisionGroup;
   }

   public void setCollisionMask(int collisionMask)
   {
      this.collisionMask = collisionMask;
   }

   public AppearanceDefinition getAppearance()
   {
      return appearance;
   }

   public void setAppearance(AppearanceDefinition appearance)
   {
      this.appearance = appearance;
   }

   public RobotDescription createRobotDescription()
   {
      RobotDescription robotDescription = new RobotDescription(name);

      FloatingJointDescription rootJoint = new FloatingJointDescription("box", name);
      LinkDescription link = new LinkDescription("boxLink");
      link.setMassAndRadiiOfGyration(mass, radiusOfGyrationPercent * xLength, radiusOfGyrationPercent * yWidth, radiusOfGyrationPercent * zHeight);

      LinkGraphicsDescription linkGraphics = new LinkGraphicsDescription();
      linkGraphics.translate(0.0, 0.0, -zHeight / 2.0);
      linkGraphics.addCube(xLength, yWidth, zHeight, appearance);
      link.setLinkGraphics(linkGraphics);

      CollisionMeshDescription collisionMesh = new CollisionMeshDescription();
      collisionMesh.addCubeReferencedAtCenter(xLength, yWidth, zHeight);
      collisionMesh.setCollisionGroup(collisionGroup);
      collisionMesh.setCollisionMask(collisionMask);
      collisionMesh.setEstimatedNumberOfContactPoints(estimatedNumberOfContactPoints);
      link.addCollisionMesh(collisionMesh);

      for (int i = 0; i < 8; i++)
      {
         double xSign = 1.0;
         double ySign = 1.0;
         double zSign = 1.0;

         if (i % 2 == 0)
         {
            xSign = -1.0;
         }

         if ((i / 2) % 2 == 0)
         {
            ySign = -1.0;
         }

         if ((i / 4) % 2 == 0)
         {
            zSign = -1.0;
         }

         GroundContactPointDescription groundContactPoint = new GroundContactPointDescription("gc_"
               + i, new Vector3D(xSign * xLength / 2.0, ySign * yWidth / 2.0, zSign * zHeight / 2.0));
         rootJoint.addGroundContactPoint(groundContactPoint);
      }

      rootJoint.setLink(link);
      robotDescription.addRootJoint(rootJoint);

      return robotDescription;
   }

}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/collisionExample/CollisionExampleRobot.java<|end_filename|>
package us.ihmc.exampleSimulations.collisionExample;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Link;
import us.ihmc.simulationconstructionset.Robot;


public class CollisionExampleRobot extends Robot
{
   private static final long serialVersionUID = 6294679946351726669L;
   public static final double
      M1 = 1.0, R1 = 0.2, GYR1 = 0.6;
   public static final double
      Ixx1 = GYR1 * M1 * R1 * R1, Iyy1 = GYR1 * M1 * R1 * R1, Izz1 = GYR1 * M1 * R1 * R1;

   public static final double
      M2 = 2.0, R2 = 0.2, GYR2 = 0.6;
   public static final double
      Ixx2 = GYR2 * M2 * R2 * R2, Iyy2 = GYR2 * M2 * R2 * R2, Izz2 = GYR2 * M2 * R2 * R2;


   public CollisionExampleRobot()
   {
      super("CollisionExample");
      this.setGravity(0.0, 0.0, 0.0);

      FloatingJoint floatingJoint1 = new FloatingJoint("ball1", "ball1", new Vector3D(0.0, 0.0, 0.0), this);
      Link ball1 = ball1();
      floatingJoint1.setLink(ball1);

      this.addRootJoint(floatingJoint1);

      FloatingJoint floatingJoint2 = new FloatingJoint("ball2", "ball2", new Vector3D(0.0, 0.0, 0.0), this);
      Link ball2 = ball2();
      floatingJoint2.setLink(ball2);

      this.addRootJoint(floatingJoint2);
   }

   public Link ball1()
   {
      Link ret = new Link("ball1");
      ret.setMass(M1);
      ret.setMomentOfInertia(Ixx1, Iyy1, Izz1);

      Graphics3DObject linkGraphics = new Graphics3DObject();
      linkGraphics.addSphere(R1, YoAppearance.EarthTexture());
      ret.setLinkGraphics(linkGraphics);

      return ret;
   }

   public Link ball2()
   {
      Link ret = new Link("ball2");
      ret.setMass(M2);
      ret.setMomentOfInertia(Ixx2, Iyy2, Izz2);

      Graphics3DObject linkGraphics = new Graphics3DObject();
      linkGraphics.addSphere(R2, YoAppearance.EarthTexture());
      ret.setLinkGraphics(linkGraphics);

      return ret;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/implementations/PIDSE3Configuration.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains.implementations;

import us.ihmc.robotics.controllers.pidGains.GainCoupling;
import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly;
import us.ihmc.robotics.controllers.pidGains.PIDSE3GainsReadOnly;

public class PIDSE3Configuration
{
   private final PID3DConfiguration positionConfiguration;

   private final PID3DConfiguration orientationConfiguration;

   public PIDSE3Configuration(GainCoupling gainCoupling, boolean useIntegrator)
   {
      this(gainCoupling, useIntegrator, null, null);
   }

   public PIDSE3Configuration(GainCoupling gainCoupling, boolean useIntegrator, PIDSE3GainsReadOnly gains)
   {
      this(gainCoupling, useIntegrator, gains.getPositionGains(), gains.getOrientationGains());
   }

   public PIDSE3Configuration(GainCoupling gainCoupling, boolean useIntegrator, PID3DGainsReadOnly positionGains, PID3DGainsReadOnly orientationGains)
   {
      this.positionConfiguration = new PID3DConfiguration(gainCoupling, useIntegrator, positionGains);
      this.orientationConfiguration = new PID3DConfiguration(gainCoupling, useIntegrator, orientationGains);
   }

   public PID3DConfiguration getPositionConfiguration()
   {
      return positionConfiguration;
   }

   public PID3DConfiguration getOrientationConfiguration()
   {
      return orientationConfiguration;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/LeapOfFaithParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

public class LeapOfFaithParameters
{
   /**
    * Specifies whether or not to scale the weights in the optimization on the swing foot near the end of swing.
    */
   public boolean scaleFootWeight()
   {
      return false;
   }

   /**
    * The fraction of the way through the swing phase at which the pelvis starts to rotate down and in the
    * direction of the step
    */
   public double getFractionOfSwingToScaleFootWeight()
   {
      return 0.92;
   }

   /**
    * This is the rate factor that the foot weight is scaled by, via reduction, as a function of the time it's being scaled.
    */
   public double getHorizontalFootWeightScaleFactor()
   {
      return 0.0;
   }

   /**
    * This is the rate factor that the foot weight is scaled by, via increase, as a function of the time it's being scaled.
    */
   public double getVerticalFootWeightScaleFactor()
   {
      return 100.0;
   }

   /**
    * This is the minimum horizontal weight that will be used for the swing foot.
    */
   public double getMinimumHorizontalFootWeight()
   {
      return 2.5;
   }

   /**
    * Specifies whether or not to start rotating the pelvis to set the foot down.
    */
   public boolean usePelvisRotation()
   {
      return false;
   }

   /**
    * Gain to modify the desired pelvis yaw cause the pelvis to yaw towards the step to help set the foot down.
    * It is multiplied by the time past when we expected touchdown to compute the angle offset of the pelvis yaw.
    */
   public double getPelvisReachingYawGain()
   {
      return 1.0;
   }

   /**
    * Gain to modify the desired pelvis roll cause the pelvis to roll towards the step to help set the foot down.
    * It is multiplied by the time past when we expected touchdown to compute the angle offset of the pelvis roll.
    */
   public double getPelvisReachingRollGain()
   {
      return 1.0;
   }

   /**
    * The maximum angle that the pelvis will yaw in the direction of the step.
    */
   public double getPelvisReachingMaxYaw()
   {
      return 0.2;
   }

   /**
    * This maximum angle that the pelvis will roll down.
    */
   public double getPelvisReachingMaxRoll()
   {
      return 0.3;
   }

   /**
    * The fraction of the way through the swing phase at which the pelvis starts to rotate down and in the
    * direction of the step
    */
   public double getPelvisReachingFractionOfSwing()
   {
      return 0.90;
   }

   public boolean relaxPelvisControl()
   {
      return false;
   }

   public double getRelaxationRate()
   {
      return 2.0;
   }

   public double getMinimumPelvisWeight()
   {
      return 1.0;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KinematicsPlanningToolboxRigidBodyMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "KinematicsPlanningToolboxRigidBodyMessage" defined in "KinematicsPlanningToolboxRigidBodyMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from KinematicsPlanningToolboxRigidBodyMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit KinematicsPlanningToolboxRigidBodyMessage_.idl instead.
*
*/
public class KinematicsPlanningToolboxRigidBodyMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::KinematicsPlanningToolboxRigidBodyMessage_";

   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.KinematicsPlanningToolboxRigidBodyMessage 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.KinematicsPlanningToolboxRigidBodyMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 100; ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += geometry_msgs.msg.dds.QuaternionPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getKeyFrameTimes().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getKeyFramePoses().size(); ++i0)
      {
          current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getKeyFramePoses().get(i0), current_alignment);}

      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getCdrSerializedSize(data.getAngularSelectionMatrix(), current_alignment);

      current_alignment += controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.getCdrSerializedSize(data.getLinearSelectionMatrix(), current_alignment);

      current_alignment += controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.getCdrSerializedSize(data.getAngularWeightMatrix(), current_alignment);

      current_alignment += controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.getCdrSerializedSize(data.getLinearWeightMatrix(), current_alignment);

      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getControlFramePositionInEndEffector(), current_alignment);

      current_alignment += geometry_msgs.msg.dds.QuaternionPubSubType.getCdrSerializedSize(data.getControlFrameOrientationInEndEffector(), current_alignment);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getAllowablePositionDisplacement().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getAllowableOrientationDisplacement().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_2(data.getEndEffectorHashCode());

      if(data.getKeyFrameTimes().size() <= 100)
      cdr.write_type_e(data.getKeyFrameTimes());else
          throw new RuntimeException("key_frame_times field exceeds the maximum length");

      if(data.getKeyFramePoses().size() <= 100)
      cdr.write_type_e(data.getKeyFramePoses());else
          throw new RuntimeException("key_frame_poses field exceeds the maximum length");

      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.write(data.getAngularSelectionMatrix(), cdr);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.write(data.getLinearSelectionMatrix(), cdr);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.write(data.getAngularWeightMatrix(), cdr);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.write(data.getLinearWeightMatrix(), cdr);
      geometry_msgs.msg.dds.PointPubSubType.write(data.getControlFramePositionInEndEffector(), cdr);
      geometry_msgs.msg.dds.QuaternionPubSubType.write(data.getControlFrameOrientationInEndEffector(), cdr);
      if(data.getAllowablePositionDisplacement().size() <= 100)
      cdr.write_type_e(data.getAllowablePositionDisplacement());else
          throw new RuntimeException("allowable_position_displacement field exceeds the maximum length");

      if(data.getAllowableOrientationDisplacement().size() <= 100)
      cdr.write_type_e(data.getAllowableOrientationDisplacement());else
          throw new RuntimeException("allowable_orientation_displacement field exceeds the maximum length");

   }

   public static void read(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setEndEffectorHashCode(cdr.read_type_2());
      	
      cdr.read_type_e(data.getKeyFrameTimes());	
      cdr.read_type_e(data.getKeyFramePoses());	
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.read(data.getAngularSelectionMatrix(), cdr);	
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.read(data.getLinearSelectionMatrix(), cdr);	
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.read(data.getAngularWeightMatrix(), cdr);	
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.read(data.getLinearWeightMatrix(), cdr);	
      geometry_msgs.msg.dds.PointPubSubType.read(data.getControlFramePositionInEndEffector(), cdr);	
      geometry_msgs.msg.dds.QuaternionPubSubType.read(data.getControlFrameOrientationInEndEffector(), cdr);	
      cdr.read_type_e(data.getAllowablePositionDisplacement());	
      cdr.read_type_e(data.getAllowableOrientationDisplacement());	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_2("end_effector_hash_code", data.getEndEffectorHashCode());
      ser.write_type_e("key_frame_times", data.getKeyFrameTimes());
      ser.write_type_e("key_frame_poses", data.getKeyFramePoses());
      ser.write_type_a("angular_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getAngularSelectionMatrix());

      ser.write_type_a("linear_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getLinearSelectionMatrix());

      ser.write_type_a("angular_weight_matrix", new controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType(), data.getAngularWeightMatrix());

      ser.write_type_a("linear_weight_matrix", new controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType(), data.getLinearWeightMatrix());

      ser.write_type_a("control_frame_position_in_end_effector", new geometry_msgs.msg.dds.PointPubSubType(), data.getControlFramePositionInEndEffector());

      ser.write_type_a("control_frame_orientation_in_end_effector", new geometry_msgs.msg.dds.QuaternionPubSubType(), data.getControlFrameOrientationInEndEffector());

      ser.write_type_e("allowable_position_displacement", data.getAllowablePositionDisplacement());
      ser.write_type_e("allowable_orientation_displacement", data.getAllowableOrientationDisplacement());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setEndEffectorHashCode(ser.read_type_2("end_effector_hash_code"));
      ser.read_type_e("key_frame_times", data.getKeyFrameTimes());
      ser.read_type_e("key_frame_poses", data.getKeyFramePoses());
      ser.read_type_a("angular_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getAngularSelectionMatrix());

      ser.read_type_a("linear_selection_matrix", new controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType(), data.getLinearSelectionMatrix());

      ser.read_type_a("angular_weight_matrix", new controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType(), data.getAngularWeightMatrix());

      ser.read_type_a("linear_weight_matrix", new controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType(), data.getLinearWeightMatrix());

      ser.read_type_a("control_frame_position_in_end_effector", new geometry_msgs.msg.dds.PointPubSubType(), data.getControlFramePositionInEndEffector());

      ser.read_type_a("control_frame_orientation_in_end_effector", new geometry_msgs.msg.dds.QuaternionPubSubType(), data.getControlFrameOrientationInEndEffector());

      ser.read_type_e("allowable_position_displacement", data.getAllowablePositionDisplacement());
      ser.read_type_e("allowable_orientation_displacement", data.getAllowableOrientationDisplacement());
   }

   public static void staticCopy(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage src, controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage createData()
   {
      return new controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage();
   }
   @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.KinematicsPlanningToolboxRigidBodyMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage src, controller_msgs.msg.dds.KinematicsPlanningToolboxRigidBodyMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public KinematicsPlanningToolboxRigidBodyMessagePubSubType newInstance()
   {
      return new KinematicsPlanningToolboxRigidBodyMessagePubSubType();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/FootstepPlannerStatusMessage.java<|end_filename|>
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 footstep planner API.
       * The footstep planner sends this message to notify the user of the status of planning.
       */
public class FootstepPlannerStatusMessage extends Packet<FootstepPlannerStatusMessage> implements Settable<FootstepPlannerStatusMessage>, EpsilonComparable<FootstepPlannerStatusMessage>
{
   public static final byte FOOTSTEP_PLANNER_STATUS_IDLE = (byte) 0;
   public static final byte FOOTSTEP_PLANNER_STATUS_PLANNING_PATH = (byte) 1;
   public static final byte FOOTSTEP_PLANNER_STATUS_PLANNING_STEPS = (byte) 2;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte footstep_planner_status_ = (byte) 255;

   public FootstepPlannerStatusMessage()
   {
   }

   public FootstepPlannerStatusMessage(FootstepPlannerStatusMessage other)
   {
      this();
      set(other);
   }

   public void set(FootstepPlannerStatusMessage other)
   {
      sequence_id_ = other.sequence_id_;

      footstep_planner_status_ = other.footstep_planner_status_;

   }

   /**
            * 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 void setFootstepPlannerStatus(byte footstep_planner_status)
   {
      footstep_planner_status_ = footstep_planner_status;
   }
   public byte getFootstepPlannerStatus()
   {
      return footstep_planner_status_;
   }


   public static Supplier<FootstepPlannerStatusMessagePubSubType> getPubSubType()
   {
      return FootstepPlannerStatusMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return FootstepPlannerStatusMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(FootstepPlannerStatusMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.footstep_planner_status_, other.footstep_planner_status_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof FootstepPlannerStatusMessage)) return false;

      FootstepPlannerStatusMessage otherMyClass = (FootstepPlannerStatusMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.footstep_planner_status_ != otherMyClass.footstep_planner_status_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("FootstepPlannerStatusMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("footstep_planner_status=");
      builder.append(this.footstep_planner_status_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/gui/YoGUIMessageFrame.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.gui;


import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.text.DecimalFormat;
import java.util.ArrayList;

import javax.swing.JCheckBox;
import javax.swing.JFrame;
import javax.swing.JOptionPane;
import javax.swing.JTabbedPane;

import us.ihmc.yoVariables.variable.YoDouble;
import us.ihmc.simulationConstructionSetTools.util.globalParameters.BooleanGlobalParameter;
import us.ihmc.simulationConstructionSetTools.util.globalParameters.DoubleGlobalParameter;
import us.ihmc.simulationConstructionSetTools.util.globalParameters.EnumGlobalParameter;
import us.ihmc.simulationConstructionSetTools.util.globalParameters.GlobalParameter;
import us.ihmc.simulationConstructionSetTools.util.globalParameters.GlobalParameterChangedListener;
import us.ihmc.simulationConstructionSetTools.util.globalParameters.IntGlobalParameter;

public class YoGUIMessageFrame implements GlobalParameterChangedListener
{
   private static final DecimalFormat decimal3DPrintFormatter = new DecimalFormat("0.000");

   private static final int CHARACTERS_IN_VALUE_STRING = 12;
   public static boolean showMessagePanel = true;
   public static String messageWindowName = "GUI Message Frame";

   private static JFrame parentFrame;
   private static boolean initialized = false;

// private static final JFrame messageWindow = new JFrame("LittleDog Messages");
   private static JFrame messageWindow;
   private static final JTabbedPane jTabbedPane = new JTabbedPane();


// private static int trialMessagePanelIndex;
   private static int errorMessagePanelIndex;

// private static int terrainMessagePanelIndex;
   private int parameterMessagePanelIndex;
   private boolean createdParameterMessagePanel = false;

   private static JCheckBox lockBox = new JCheckBox("Lock Focus");
   private static boolean locked = false;
   private static int lastMessagePanel = 0;

   private static ArrayList<Integer> listOfPanels = new ArrayList<Integer>();

   private static YoGUIMessageFrame elvis;

   private YoDouble time;

   private YoGUIMessageFrame()
   {
   }

   public static YoGUIMessageFrame getInstance()
   {
      initialize();

      return elvis;
   }

   public static void setTimeVariable(YoDouble time)
   {
      initialize();

      elvis.time = time;
   }


   public void createChangeListener(String tabName)
   {
      if (createdParameterMessagePanel)
      {
         System.out.println("Already created change listener");

         return;
      }

      YoGUIMessagePanel messagePanel;
      messagePanel = new YoGUIMessagePanel(tabName);
      jTabbedPane.add(tabName, messagePanel);
      parameterMessagePanelIndex = jTabbedPane.indexOfTab(tabName);
      listOfPanels.add(parameterMessagePanelIndex);

      createdParameterMessagePanel = true;
   }


   private static void initialize()
   {
      if (initialized)
         return;

      if (elvis != null)
      {
         throw new RuntimeException("Should never get here!");
      }

      elvis = new YoGUIMessageFrame();

//    LittleDogOutput.parentFrame = parentFrame;

      messageWindow = new JFrame(messageWindowName);
      messageWindow.getContentPane().setLayout(new BorderLayout());
      messageWindow.getContentPane().add(jTabbedPane);    // ,BorderLayout.CENTER);
      messageWindow.getContentPane().add(lockBox, BorderLayout.SOUTH);

      // jTabbedPane.setSize(100, 100);
      lockBox.addActionListener(new ActionListener()
      {
         @Override
         public void actionPerformed(ActionEvent e)
         {
            if (locked)
            {
               locked = false;
               jTabbedPane.setSelectedIndex(lastMessagePanel);
            }
            else
            {
               locked = true;
            }

         }
      });

      YoGUIMessagePanel messagePanel;


//    String trialStatusString = "Trial Status";
//    messagePanel = new GUIMessagePanel(trialStatusString);
//    jTabbedPane.add(trialStatusString, messagePanel);
//    trialMessagePanelIndex = jTabbedPane.indexOfTab(trialStatusString);
//    listOfPanels.add(trialMessagePanelIndex);

      String errorStatusString = "Error";
      messagePanel = new YoGUIMessagePanel(errorStatusString);
      jTabbedPane.add(errorStatusString, messagePanel);
      errorMessagePanelIndex = jTabbedPane.indexOfTab(errorStatusString);
      listOfPanels.add(errorMessagePanelIndex);

//    String terrainStatusString = "Terrain";
//    messagePanel = new GUIMessagePanel(terrainStatusString);
//    jTabbedPane.add(terrainStatusString, messagePanel);
//    terrainMessagePanelIndex = jTabbedPane.indexOfTab(terrainStatusString);
//    listOfPanels.add(terrainMessagePanelIndex);




      messageWindow.setSize(new Dimension(900, 800));
      messageWindow.setLocation(250, 0);

      if (showMessagePanel)
      {
         messageWindow.setVisible(true);
      }
      else
         messageWindow.setVisible(false);

      initialized = true;
   }


// public static void appendTrialMessage(String message)
// {
//    if (!initialized)
//       initialize();
//
//    appendMessageToPanel(trialMessagePanelIndex, message);
// }
//
// public static void appendErrorMessage(String error)
// {
//    if (!initialized)
//       initialize();
//
//    appendMessageToPanel(errorMessagePanelIndex, error);
// }
//
// public static void appendTerrainMessage(String error)
// {
//    if (!initialized)
//       initialize();
//
//    appendMessageToPanel(terrainMessagePanelIndex, error);
// }
//
//
   private void appendParameterMessage(String message)
   {
      if (!createdParameterMessagePanel)
         createChangeListener("Parameter");

      appendMessageToPanel(parameterMessagePanelIndex, message);
   }


// public static void popupErrorMessage(String error)
// {
//    if (!initialized)
//       initialize();
//    GUIMessagePanel gUIMessagePanel = (GUIMessagePanel) jTabbedPane.getComponentAt(trialMessagePanelIndex);
//    gUIMessagePanel.appendMessage("Error: " + error);
//
//    JOptionPane.showMessageDialog(parentFrame, error, "Error", JOptionPane.ERROR_MESSAGE);
// }

   public static void popupWarningMessage(String warning)
   {
      if (!initialized)
         initialize();

      JOptionPane.showMessageDialog(parentFrame, warning, "Warning", JOptionPane.WARNING_MESSAGE);
   }

   public static void popupInformationMessage(String message)
   {
      if (!initialized)
         initialize();

      JOptionPane.showMessageDialog(parentFrame, message, "Message", JOptionPane.INFORMATION_MESSAGE);
   }


   public static void appendMessageToPanel(int panelIndex, String message)
   {
      appendMessageToPanel(panelIndex, message, Color.BLACK);
   }

   public static void appendMessageToPanel(int panelIndex, String message, Color color)
   {
      if (!initialized)
         initialize();

      // make sure that panel index is valid
      // IndexOutOfBoundsException
      YoGUIMessagePanel gUIMessagePanel = null;

//    String time;
//    if (LittleDogGlobalParameters.hasInstanceBeenCreated())
//       time = SpecialFunctions.getRobotTime();
//    else
//       time = "";
//    message = time + ": " + message;

      if (elvis.time != null)
      {
         message = decimal3DPrintFormatter.format(elvis.time.getDoubleValue()) + ": " + message;
      }

      try
      {
         gUIMessagePanel = (YoGUIMessagePanel) jTabbedPane.getComponentAt(panelIndex);
         gUIMessagePanel.appendMessage(message, color);
         if (!locked)
            jTabbedPane.setSelectedIndex(panelIndex);
         lastMessagePanel = panelIndex;
      }
      catch (IndexOutOfBoundsException indexOutOfBoundsException)
      {
         throw indexOutOfBoundsException;
      }
   }

   public static int createGUIMessagePanel(String name)
   {
      if (!initialized)
         initialize();

      YoGUIMessagePanel ret = new YoGUIMessagePanel(name);


      int checkIndex = jTabbedPane.indexOfTab(name);
      if (checkIndex != -1)
      {
         throw new RuntimeException("This name is already taken: " + name);
      }
      else
         jTabbedPane.addTab(name, ret);


      int integerValueToReturn = jTabbedPane.indexOfTab(name);

      listOfPanels.add(integerValueToReturn);

      return integerValueToReturn;
   }

   public static void reset()
   {
      for (Integer panelIndexValue : listOfPanels)
      {
         YoGUIMessagePanel gUIMessagePanel = null;

         gUIMessagePanel = (YoGUIMessagePanel) jTabbedPane.getComponentAt(panelIndexValue);

         gUIMessagePanel.clear();
      }
   }

   @Override
   public void globalParameterCreated(GlobalParameter globalParameter)
   {
      appendParameterMessage(globalParameter.getShortName() + "= " + padWithSpacesOrTrim(globalParameter.getValueInStringFormat(), CHARACTERS_IN_VALUE_STRING)
                             + " created");
   }


   @Override
   public void booleanValueChanged(GlobalParameter globalParameter, String comment, boolean previousValue, boolean newValue)
   {
      appendParameterMessage(globalParameter.getShortName() + " GlobalParameter changed from   "
                             + padWithSpacesOrTrim(Boolean.toString(previousValue), CHARACTERS_IN_VALUE_STRING) + " to   "
                             + padWithSpacesOrTrim(Boolean.toString(newValue), CHARACTERS_IN_VALUE_STRING) + " " + comment);
   }

   @Override
   public void doubleValueChanged(GlobalParameter globalParameter, String comment, double previousValue, double newValue)
   {
      appendParameterMessage(globalParameter.getShortName() + " GlobalParameter changed from   "
                             + padWithSpacesOrTrim(Double.toString(previousValue), CHARACTERS_IN_VALUE_STRING) + " to   "
                             + padWithSpacesOrTrim(Double.toString(newValue), CHARACTERS_IN_VALUE_STRING) + " " + comment);
   }

   @Override
   public void integerValueChanged(GlobalParameter globalParameter, String comment, int previousValue, int newValue)
   {
      appendParameterMessage(globalParameter.getShortName() + " GlobalParameter changed from   "
                             + padWithSpacesOrTrim(Integer.toString(previousValue), CHARACTERS_IN_VALUE_STRING) + " to   "
                             + padWithSpacesOrTrim(Integer.toString(newValue), CHARACTERS_IN_VALUE_STRING) + " " + comment);
   }

   @Override
   public void enumValueChanged(GlobalParameter globalParameter, String comment, Enum<?> previousValue, Enum<?> newValue)
   {
      appendParameterMessage(globalParameter.getShortName() + " GlobalParameter changed from   "
                             + padWithSpacesOrTrim(previousValue.toString(), CHARACTERS_IN_VALUE_STRING) + " to   "
                             + padWithSpacesOrTrim(newValue.toString(), CHARACTERS_IN_VALUE_STRING) + " " + comment);
   }


// CHARACTERS_IN_VALUE_STRING

   private String padWithSpacesOrTrim(String string, int sizeToBe)
   {
      if (string.length() == sizeToBe)
         return string;

      if (string.length() > sizeToBe)
         return string.substring(0, sizeToBe);

      StringBuffer padded = new StringBuffer(string);
      while (padded.length() < sizeToBe)
      {
         padded.append(" ");
      }

      return padded.toString();
   }


   public static void main(String[] args)
   {
      int sleepTime = 100;

      YoGUIMessageFrame yoGUIMessageFrame = YoGUIMessageFrame.getInstance();

//    YoGUIMessageFrame.appendMessageToPanel("01 This is an error");

      int junkPanelIndex = YoGUIMessageFrame.createGUIMessagePanel("Junk Panel");

      try
      {
         Thread.sleep(sleepTime);
      }
      catch (InterruptedException ex)
      {
      }


      YoGUIMessageFrame.appendMessageToPanel(junkPanelIndex, "01 new message");

      try
      {
         Thread.sleep(sleepTime);
      }
      catch (InterruptedException ex)
      {
      }

//    YoGUIMessageFrame.appendTrialMessage("02 This is an ");

      try
      {
         Thread.sleep(sleepTime);
      }
      catch (InterruptedException ex)
      {
      }

      for (int i = 0; i < 100; i++)
      {
         YoGUIMessageFrame.appendMessageToPanel(junkPanelIndex, i + " new message", Color.red);
      }



      GlobalParameter globalParameterDouble = new DoubleGlobalParameter("testParametwerwer wer werer wer werweerDouble", "", -10.0, yoGUIMessageFrame);
      GlobalParameter globalParameterBoolean = new BooleanGlobalParameter("testParameterBoolean", "", true, yoGUIMessageFrame);
      GlobalParameter globalParameterInteger = new IntGlobalParameter("testParameterInteger", "", 2, yoGUIMessageFrame);
      GlobalParameter globalParameterEnum = new EnumGlobalParameter("testParameterEnum", "", TestEnum.askdjaskdjasdjadajsdajsdjkasdlkj, yoGUIMessageFrame);

      System.out.println("testParameterDouble" + " max number for display = " + globalParameterDouble.getMaximumNumberOfCharactersInValue());
      System.out.println("testParameterBoolean" + " max number for display = " + globalParameterBoolean.getMaximumNumberOfCharactersInValue());
      System.out.println("testParameterInteger" + " max number for display = " + globalParameterInteger.getMaximumNumberOfCharactersInValue());
      System.out.println("testParameterEnum" + " max number for display = " + globalParameterEnum.getMaximumNumberOfCharactersInValue());

      for (int i = 0; i < 10; i++)
      {
         ((DoubleGlobalParameter) globalParameterDouble).set(i,
                 " changing, i=" + i + " a whole bunch of stuff to print here so that we go over the edge of the window.");
      }

      ((BooleanGlobalParameter) globalParameterBoolean).set(false, "hi");

//    try
//    {
//       Thread.sleep(10000);
//    }
//    catch (InterruptedException ex1)
//    {
//    }
//
//    System.out.println("Clearing");
//
//
//    YoGUIMessageFrame.reset();

   }

   private enum TestEnum {A, B, C, WEWER, askdjaskdjasdjadajsdajsdjkasdlkj}

}


<|start_filename|>ihmc-parameter-optimization/src/main/java/us/ihmc/utilities/parameterOptimization/BooleanYoVariableParameterToOptimize.java<|end_filename|>
package us.ihmc.utilities.parameterOptimization;

import us.ihmc.yoVariables.variable.YoBoolean;

public class BooleanYoVariableParameterToOptimize extends BooleanParameterToOptimize
{
   private final YoBoolean yoVariable;
   
   public BooleanYoVariableParameterToOptimize(YoBoolean yoVariable, ListOfParametersToOptimize listOfParametersToOptimize)
   {
      super(yoVariable.getName(), listOfParametersToOptimize);
      this.yoVariable = yoVariable;
   }
   
   public void setCurrentValueGivenZeroToOne(double zeroToOne)
   {
      super.setCurrentValueGivenZeroToOne(zeroToOne);
      yoVariable.set(this.getCurrentValue());
   }
   
   public void setCurrentValue(boolean newValue)
   {
      super.setCurrentValue(newValue);
      yoVariable.set(newValue);
   }

}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicReachability/SphereIntersectionTools.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.dynamicReachability;

public class SphereIntersectionTools
{
   /**
    * Computes the distance to the intersecting plane between two spheres. Sphere 1 is assumed to be the base sphere, and sphere 2 has it centered at a point
    * separated from the base sphere by {@param sphereSeparation}.
    *
    * @param sphereSeparation distance between the center of the spheres
    * @param radius1 radius of sphere 1
    * @param radius2 radius of sphere 2
    * @return distance from the center of sphere 1 to the intersecting plane between the two spheres
    */
   public static double computeDistanceToIntersectingPlane(double sphereSeparation, double radius1, double radius2)
   {
      return (Math.pow(sphereSeparation, 2.0) - Math.pow(radius2, 2.0) + Math.pow(radius1, 2.0)) / (2.0 * sphereSeparation);
   }

   /**
    * Computes the radius of the intersecting plane between two spheres. Sphere 1 is assumed to be the base sphere, and sphere 2 has it centered at a point
    * separated from the base sphere by {@param sphereSeparation}.
    *
    * @param sphereSeparation distance between the center of the spheres
    * @param radius1 radius of sphere 1
    * @param radius2 radius of sphere 2
    * @return radius of intersecting circular plane between the two spheres.
    */
   public static double computeRadiusOfIntersectingPlane(double sphereSeparation, double radius1, double radius2)
   {
      double distanceToPlane = computeDistanceToIntersectingPlane(sphereSeparation, radius1, radius2);
      return computeEllipseMajorAxisRadius(radius1, distanceToPlane);
   }

   /**
    * The intersection between two spheres is a plane. When this plane is projected onto the ground, it forms an ellipse.
    * This function returns the distance to the center of this ellipse along the vector connect the spheres' centers.
    * Sphere 1 is assumed to be the base sphere, centered at (0, 0). The distance to the center point of sphere 2 is defined
    * by {@param sphereSeparationX} and {@param sphereSeparationZ}.
    *
    * @param sphereSeparationX distance in X to the center of sphere 2 from sphere 1.
    * @param sphereSeparationZ distance in Z to the center of sphere 2 from sphere 1.
    * @param radius1 radius of sphere 1
    * @param radius2 radius of sphere 2
    *
    * @return horizontal distance to the ellipse that is the intersection region projected on the ground.
    */
   public static double computeDistanceToCenterOfIntersectionEllipse(double sphereSeparationX, double sphereSeparationZ, double radius1, double radius2)
   {
      double sphereSeparation = Math.sqrt(Math.pow(sphereSeparationX, 2.0) + Math.pow(sphereSeparationZ, 2.0));
      double angleOfSeparation = Math.atan(sphereSeparationZ / sphereSeparationX);

      return computeDistanceToCenterOfIntersectionEllipseInternal(sphereSeparation, angleOfSeparation, radius1, radius2);
   }

   /**
    * The intersection between two spheres is a plane. When this plane is projected onto the ground, it forms an ellipse.
    * This function returns the distance to the closest edge of this ellipse along the vector connect the spheres' centers.
    * Sphere 1 is assumed to be the base sphere, centered at (0, 0). The distance to the center point of sphere 2 is defined
    * by {@param sphereSeparationX} and {@param sphereSeparationZ}.
    *
    * @param sphereSeparationX distance in X to the center of sphere 2 from sphere 1.
    * @param sphereSeparationZ distance in Z to the center of sphere 2 from sphere 1.
    * @param radius1 radius of sphere 1
    * @param radius2 radius of sphere 2
    *
    * @return horizontal distance to the closest edge of the ellipse that is teh intersection region projected on the ground.
    */
   public static double computeDistanceToNearEdgeOfIntersectionEllipse(double sphereSeparationX, double sphereSeparationZ, double radius1, double radius2)
   {
      if (sphereSeparationZ == 0.0)
         return computeDistanceToIntersectingPlane(sphereSeparationX, radius1, radius2);

      double sphereSeparation = Math.sqrt(Math.pow(sphereSeparationX, 2.0) + Math.pow(sphereSeparationZ, 2.0));
      double angleOfSeparation = Math.atan(sphereSeparationZ / sphereSeparationX);

      double distanceToPlane = computeDistanceToIntersectingPlane(sphereSeparation, radius1, radius2);
      double horizontalDistanceToEllipse = computeDistanceToCenterOfIntersectionEllipseInternal(sphereSeparation, angleOfSeparation, radius1, radius2);

      double minorAxisRadius = computeEllipseMinorAxisRadius(radius1, distanceToPlane, angleOfSeparation);

      return horizontalDistanceToEllipse - minorAxisRadius;
   }

   /**
    * The intersection between two spheres is a plane. When this plane is projected onto the ground, it forms an ellipse.
    * This function returns the distance to farthest edge of this ellipse along the vector connect the spheres' centers.
    * Sphere 1 is assumed to be the base sphere, centered at (0, 0). The distance to the center point of sphere 2 is defined
    * by {@param sphereSeparationX} and {@param sphereSeparationZ}.
    *
    * @param sphereSeparationX distance in X to the center of sphere 2 from sphere 1.
    * @param sphereSeparationZ distance in Z to the center of sphere 2 from sphere 1.
    * @param radius1 radius of sphere 1
    * @param radius2 radius of sphere 2
    *
    * @return horizontal distance to the furthest edge of the ellipse that is teh intersection region projected on the ground.
    */
   public static double computeDistanceToFarEdgeOfIntersectionEllipse(double sphereSeparationX, double sphereSeparationZ, double radius1, double radius2)
   {
      if (sphereSeparationZ == 0.0)
         return computeDistanceToIntersectingPlane(sphereSeparationX, radius1, radius2);

      double sphereSeparation = Math.sqrt(Math.pow(sphereSeparationX, 2.0) + Math.pow(sphereSeparationZ, 2.0));
      double angleOfSeparation = Math.atan(sphereSeparationZ / sphereSeparationX);

      double distanceToPlane = computeDistanceToIntersectingPlane(sphereSeparation, radius1, radius2);
      double horizontalDistanceToEllipse = distanceToPlane * Math.cos(angleOfSeparation);

      double minorAxisRadius = computeEllipseMinorAxisRadius(radius1, distanceToPlane, angleOfSeparation);

      return horizontalDistanceToEllipse + minorAxisRadius;
   }

   static double computeDistanceToCenterOfIntersectionEllipseInternal(double sphereSeparation, double angleOfSeparation, double radius1, double radius2)
   {
      double distanceToPlane = computeDistanceToIntersectingPlane(sphereSeparation, radius1, radius2);
      return distanceToPlane * Math.cos(angleOfSeparation);
   }

   static double computeEllipseMajorAxisRadius(double sphereRadius1, double distanceToPlaneIntersection)
   {
      return Math.sqrt(Math.pow(sphereRadius1, 2.0) - Math.pow(distanceToPlaneIntersection, 2.0));
   }

   static double computeEllipseMinorAxisRadius(double sphereRadius1, double distanceToPlaneIntersection, double angleOfDistanceToHorizontal)
   {
      double majorAxisRadius = computeEllipseMajorAxisRadius(sphereRadius1, distanceToPlaneIntersection);

      return computeEllipseMinorAxisRadius(majorAxisRadius, angleOfDistanceToHorizontal);
   }

   static double computeEllipseMinorAxisRadius(double majorAxisRadius, double angleOfDistanceToHorizontal)
   {
      return majorAxisRadius * Math.sin(angleOfDistanceToHorizontal);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SelectionMatrix3DMessage.java<|end_filename|>
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 allows the user to precisely select what component in taskspace are to be achieved.
       */
public class SelectionMatrix3DMessage extends Packet<SelectionMatrix3DMessage> implements Settable<SelectionMatrix3DMessage>, EpsilonComparable<SelectionMatrix3DMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The ID of the reference frame defining the selection frame.
            * When selecting the axes of interest, these axes refer to the selection frame axes.
            * This frame is optional. It is preferable to provide it when possible, but when it is absent, i.e. equal to 0,
            * the selection matrix will then be generated regardless to what frame is it used in.
            */
   public long selection_frame_id_;
   /**
            * Specifies whether the x-axis of the selection frame is an axis of interest.
            */
   public boolean x_selected_ = true;
   /**
            * Specifies whether the y-axis of the selection frame is an axis of interest.
            */
   public boolean y_selected_ = true;
   /**
            * Specifies whether the z-axis of the selection frame is an axis of interest.
            */
   public boolean z_selected_ = true;

   public SelectionMatrix3DMessage()
   {
   }

   public SelectionMatrix3DMessage(SelectionMatrix3DMessage other)
   {
      this();
      set(other);
   }

   public void set(SelectionMatrix3DMessage other)
   {
      sequence_id_ = other.sequence_id_;

      selection_frame_id_ = other.selection_frame_id_;

      x_selected_ = other.x_selected_;

      y_selected_ = other.y_selected_;

      z_selected_ = other.z_selected_;

   }

   /**
            * 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_;
   }

   /**
            * The ID of the reference frame defining the selection frame.
            * When selecting the axes of interest, these axes refer to the selection frame axes.
            * This frame is optional. It is preferable to provide it when possible, but when it is absent, i.e. equal to 0,
            * the selection matrix will then be generated regardless to what frame is it used in.
            */
   public void setSelectionFrameId(long selection_frame_id)
   {
      selection_frame_id_ = selection_frame_id;
   }
   /**
            * The ID of the reference frame defining the selection frame.
            * When selecting the axes of interest, these axes refer to the selection frame axes.
            * This frame is optional. It is preferable to provide it when possible, but when it is absent, i.e. equal to 0,
            * the selection matrix will then be generated regardless to what frame is it used in.
            */
   public long getSelectionFrameId()
   {
      return selection_frame_id_;
   }

   /**
            * Specifies whether the x-axis of the selection frame is an axis of interest.
            */
   public void setXSelected(boolean x_selected)
   {
      x_selected_ = x_selected;
   }
   /**
            * Specifies whether the x-axis of the selection frame is an axis of interest.
            */
   public boolean getXSelected()
   {
      return x_selected_;
   }

   /**
            * Specifies whether the y-axis of the selection frame is an axis of interest.
            */
   public void setYSelected(boolean y_selected)
   {
      y_selected_ = y_selected;
   }
   /**
            * Specifies whether the y-axis of the selection frame is an axis of interest.
            */
   public boolean getYSelected()
   {
      return y_selected_;
   }

   /**
            * Specifies whether the z-axis of the selection frame is an axis of interest.
            */
   public void setZSelected(boolean z_selected)
   {
      z_selected_ = z_selected;
   }
   /**
            * Specifies whether the z-axis of the selection frame is an axis of interest.
            */
   public boolean getZSelected()
   {
      return z_selected_;
   }


   public static Supplier<SelectionMatrix3DMessagePubSubType> getPubSubType()
   {
      return SelectionMatrix3DMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SelectionMatrix3DMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SelectionMatrix3DMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.selection_frame_id_, other.selection_frame_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.x_selected_, other.x_selected_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.y_selected_, other.y_selected_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.z_selected_, other.z_selected_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SelectionMatrix3DMessage)) return false;

      SelectionMatrix3DMessage otherMyClass = (SelectionMatrix3DMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.selection_frame_id_ != otherMyClass.selection_frame_id_) return false;

      if(this.x_selected_ != otherMyClass.x_selected_) return false;

      if(this.y_selected_ != otherMyClass.y_selected_) return false;

      if(this.z_selected_ != otherMyClass.z_selected_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SelectionMatrix3DMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("selection_frame_id=");
      builder.append(this.selection_frame_id_);      builder.append(", ");
      builder.append("x_selected=");
      builder.append(this.x_selected_);      builder.append(", ");
      builder.append("y_selected=");
      builder.append(this.y_selected_);      builder.append(", ");
      builder.append("z_selected=");
      builder.append(this.z_selected_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/SliderboardDataReciever.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

import java.util.ArrayList;
import java.util.List;

import javax.sound.midi.MidiMessage;
import javax.sound.midi.Receiver;
import javax.sound.midi.ShortMessage;

import gnu.trove.map.TIntDoubleMap;
import gnu.trove.map.TIntObjectMap;
import gnu.trove.map.hash.TIntDoubleHashMap;
import gnu.trove.map.hash.TIntObjectHashMap;
import gnu.trove.procedure.TObjectProcedure;
import us.ihmc.commons.PrintTools;

public class SliderboardDataReciever implements Receiver
{
   private final MidiControlMap channelMapper;

   private final TIntObjectMap<List<SliderboardListener>> sliderListeners = new TIntObjectHashMap<>();
   private final TIntObjectMap<List<ButtonListener>> buttonListeners = new TIntObjectHashMap<>();

   private final TIntDoubleMap sliderValues = new TIntDoubleHashMap();

   public SliderboardDataReciever(MidiControlMap channelMapper)
   {
      this.channelMapper = channelMapper;
   }

   @Override
   public void send(MidiMessage message, long timeStamp)
   {
      if (!(message instanceof ShortMessage))
      {
         return;
      }

      ShortMessage shortMessage = (ShortMessage) message;
      if (shortMessage.getCommand() != SliderboardTools.STATUS)
      {
         return;
      }

      int midiChannel = shortMessage.getData1();
      if (channelMapper.getSliderIndex(midiChannel) != MidiControlMap.INVALID)
      {
         handleSliderInput(channelMapper.getSliderIndex(midiChannel), shortMessage.getData2());
      }
      else if (channelMapper.getButtonIndex(midiChannel) != MidiControlMap.INVALID)
      {
         handleButtonInput(channelMapper.getButtonIndex(midiChannel), shortMessage.getData2());
      }
      else if (channelMapper.getDelayVariationChannel() == midiChannel)
      {
         PrintTools.info("Got a 'Delay/Variation Send' message.");
      }
      else
      {
         PrintTools.info("Unknown controller: " + midiChannel + " - " + shortMessage.getData2());
         return;
      }
   }

   private void handleSliderInput(int sliderIndex, int data)
   {
      double value = SliderboardTools.toSliderPercent(data);

      List<SliderboardListener> listeners = sliderListeners.get(sliderIndex);
      double previousValue = sliderValues.containsKey(sliderIndex) ? sliderValues.get(sliderIndex) : -1.0;
      if (listeners != null && previousValue != value)
      {
         listeners.forEach(listener -> listener.sliderMoved(value));
      }
      sliderValues.put(sliderIndex, value);
   }

   private void handleButtonInput(int buttonIndex, int data)
   {
      boolean status = SliderboardTools.toButtonStatus(data);

      List<ButtonListener> listeners = buttonListeners.get(buttonIndex);
      if (listeners != null)
      {
         listeners.forEach(listener -> listener.buttonPressed(status));
      }
   }

   public boolean addListener(SliderboardListener sliderListener, int sliderIndex)
   {
      if (channelMapper.getSliderChannel(sliderIndex) == MidiControlMap.INVALID)
      {
         return false;
      }

      if (!sliderListeners.containsKey(sliderIndex))
      {
         sliderListeners.put(sliderIndex, new ArrayList<>());
      }
      sliderListeners.get(sliderIndex).add(sliderListener);
      return true;
   }

   public boolean addListener(ButtonListener buttonListener, int buttonIndex)
   {
      if (channelMapper.getButtonChannel(buttonIndex) == MidiControlMap.INVALID)
      {
         return false;
      }

      if (!buttonListeners.containsKey(buttonIndex))
      {
         buttonListeners.put(buttonIndex, new ArrayList<>());
      }
      buttonListeners.get(buttonIndex).add(buttonListener);
      return true;
   }

   public void clearListeners()
   {
      sliderListeners.forEachValue(new TObjectProcedure<List<SliderboardListener>>()
      {
         @Override
         public boolean execute(List<SliderboardListener> listenerList)
         {
            listenerList.clear();
            return true;
         }
      });

      buttonListeners.forEachValue(new TObjectProcedure<List<ButtonListener>>()
      {
         @Override
         public boolean execute(List<ButtonListener> listenerList)
         {
            listenerList.clear();
            return true;
         }
      });
   }

   public void removeListener(SliderboardListener sliderListener, int sliderIndex)
   {
      if (sliderListeners.containsKey(sliderIndex))
      {
         sliderListeners.get(sliderIndex).remove(sliderListener);
      }
   }

   public void removeListener(ButtonListener buttonListener, int buttonIndex)
   {
      if (buttonListeners.containsKey(buttonIndex))
      {
         buttonListeners.get(buttonIndex).remove(buttonListener);
      }
   }

   @Override
   public void close()
   {
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/quadTreeHeightMap/HeightQuadTreeMessageConverter.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.quadTreeHeightMap;

import java.util.ArrayList;
import java.util.List;

import controller_msgs.msg.dds.HeightQuadTreeLeafMessage;
import controller_msgs.msg.dds.HeightQuadTreeMessage;
import us.ihmc.commons.MathTools;
import us.ihmc.communication.packets.MessageTools;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.robotics.quadTree.Box;
import us.ihmc.robotics.quadTree.QuadTreeForGround;
import us.ihmc.robotics.quadTree.QuadTreeForGroundNode;

public class HeightQuadTreeMessageConverter
{
   public static HeightQuadTreeMessage convertQuadTreeForGround(QuadTreeForGround quadTreeToConvert)
   {
      return convertQuadTreeForGround(quadTreeToConvert, null, Double.POSITIVE_INFINITY);
   }

   public static HeightQuadTreeMessage convertQuadTreeForGround(QuadTreeForGround quadTreeToConvert, Point2D boundingCircleCenter, double boundingCircleRadius)
   {
      QuadTreeForGroundNode rootNode = quadTreeToConvert.getRootNode();

      List<HeightQuadTreeLeafMessage> leaves = new ArrayList<>();
      fullDepthCopy(rootNode, boundingCircleCenter, boundingCircleRadius, leaves);

      HeightQuadTreeMessage heightQuadTreeMessage = new HeightQuadTreeMessage();
      MessageTools.copyData(leaves, heightQuadTreeMessage.getLeaves());
      heightQuadTreeMessage.setDefaultHeight((float) rootNode.getDefaultHeightWhenNoPoints());
      heightQuadTreeMessage.setResolution((float) quadTreeToConvert.getQuadTreeParameters().getResolution());
      Box bounds = quadTreeToConvert.getRootNode().getBounds();
      heightQuadTreeMessage.setSizeX((float) (bounds.maxX - bounds.minX));
      heightQuadTreeMessage.setSizeY((float) (bounds.maxY - bounds.minY));
      return heightQuadTreeMessage;
   }

   private static void fullDepthCopy(QuadTreeForGroundNode original, Point2D boundingCircleCenter, double boundingCircleRadius,
                                     List<HeightQuadTreeLeafMessage> copyToPack)
   {
      boolean isLeaf = original.getLeaf() != null;

      if (isLeaf)
      {
         HeightQuadTreeLeafMessage leaf = new HeightQuadTreeLeafMessage();
         Box bounds = original.getBounds();
         leaf.setCenterX((float) bounds.centreX);
         leaf.setCenterY((float) bounds.centreY);
         leaf.setHeight((float) original.getLeaf().getAveragePoint().getZ());
         copyToPack.add(leaf);
         return;
      }

      if (original.hasChildren() && isAncestorOfAtLeastOneLeafInsideBoundingCircle(original, boundingCircleCenter, boundingCircleRadius))
      {
         ArrayList<QuadTreeForGroundNode> children = new ArrayList<>();
         original.getChildrenNodes(children);

         for (QuadTreeForGroundNode originalChild : children)
         {
            if (originalChild == null)
               continue;
            if (originalChild.isEmpty())
               continue;
            if (!isAncestorOfAtLeastOneLeafInsideBoundingCircle(originalChild, boundingCircleCenter, boundingCircleRadius))
               continue;

            fullDepthCopy(originalChild, boundingCircleCenter, boundingCircleRadius, copyToPack);
         }
      }
   }

   private static boolean isAncestorOfAtLeastOneLeafInsideBoundingCircle(QuadTreeForGroundNode node, Point2D boundingCircleCenter, double boundingCircleRadius)
   {
      if (node.getLeaf() != null)
         return isInsideBoundingCircle(node.getBounds().centreX, node.getBounds().centreY, boundingCircleCenter, boundingCircleRadius);

      if (node.isEmpty())
         return false;

      ArrayList<QuadTreeForGroundNode> children = new ArrayList<>();
      node.getChildrenNodes(children);

      for (QuadTreeForGroundNode child : children)
      {
         if (isAncestorOfAtLeastOneLeafInsideBoundingCircle(child, boundingCircleCenter, boundingCircleRadius))
            return true;
      }

      return false;
   }

   private static boolean isInsideBoundingCircle(double x, double y, Point2D boundingCircleCenter, double boundingCircleRadius)
   {
      double dx = x - boundingCircleCenter.getX();
      double dy = y - boundingCircleCenter.getY();
      return dx * dx + dy * dy < boundingCircleRadius * boundingCircleRadius;
   }

   public static HeightQuadTree convertMessage(HeightQuadTreeMessage messageToConvert)
   {
      HeightQuadTree heightQuadTree = new HeightQuadTree();
      heightQuadTree.setDefaultHeight(messageToConvert.getDefaultHeight());
      heightQuadTree.setResolution(messageToConvert.getResolution());
      heightQuadTree.setSizeX(messageToConvert.getSizeX());
      heightQuadTree.setSizeY(messageToConvert.getSizeY());

      if (messageToConvert.getLeaves().isEmpty())
         return heightQuadTree;

      HeightQuadTreeNode root = new HeightQuadTreeNode();
      root.setCenterX(0.0f);
      root.setCenterY(0.0f);
      root.setSizeX(messageToConvert.getSizeX());
      root.setSizeY(messageToConvert.getSizeY());

      for (int i = 0; i < messageToConvert.getLeaves().size(); i++)
         insertLeafRecursive(root, messageToConvert.getLeaves().get(i));

      heightQuadTree.setRoot(root);

      return heightQuadTree;
   }

   private static void insertLeafRecursive(HeightQuadTreeNode node, HeightQuadTreeLeafMessage leaf)
   {
      double epsilon = 1.0e-3;
      if (MathTools.epsilonEquals(node.getCenterX(), leaf.getCenterX(), epsilon) && MathTools.epsilonEquals(node.getCenterY(), leaf.getCenterY(), epsilon))
      {
         node.setHeight(leaf.getHeight());
         return;
      }

      if (!node.hasChildrenArray())
         node.assignChildrenArray();

      // Computing the morton code to make sure that the indexing is correct.
      int mortonCode = 0;
      if (leaf.getCenterX() > node.getCenterX())
         mortonCode |= 1;
      if (leaf.getCenterY() > node.getCenterY())
         mortonCode |= 2;

      HeightQuadTreeNode child = node.getChild(mortonCode);

      if (child == null)
      {
         child = new HeightQuadTreeNode();
         child.setSizeX(0.5f * node.getSizeX());
         child.setSizeY(0.5f * node.getSizeY());

         if ((mortonCode & 1) != 0)
            child.setCenterX(node.getCenterX() + 0.5f * child.getSizeX());
         else
            child.setCenterX(node.getCenterX() - 0.5f * child.getSizeX());

         if ((mortonCode & 2) != 0)
            child.setCenterY(node.getCenterY() + 0.5f * child.getSizeY());
         else
            child.setCenterY(node.getCenterY() - 0.5f * child.getSizeY());

         node.setChild(mortonCode, child);
      }

      insertLeafRecursive(child, leaf);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/State.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

import java.util.ArrayList;

@Deprecated
public abstract class State<E extends Enum<E>>
{
   private final E stateEnum;
   private ArrayList<StateTransition<E>> stateTransitions = new ArrayList<StateTransition<E>>();
   private StateTransition<E> defaultNextStateTransition;
   private boolean gotoDefaultNextState;
   private TimeInCurrentStateProvider timeInCurrentStateProvider;
   private PreviousStateProvider<E, State<E>> previousStateProvider;

   public State(E stateEnum)
   {
      this.stateEnum = stateEnum;
   }

   // These are abstract methods that a state defines.
   public abstract void doAction();

   public abstract void doTransitionIntoAction();

   public abstract void doTransitionOutOfAction();

   public final void addStateTransition(E nextStateEnum, StateTransitionCondition stateTransitionCondition)
   {
      StateTransition<E> stateTransition = new StateTransition<E>(nextStateEnum, stateTransitionCondition, (StateTransitionAction) null);
      this.addStateTransition(stateTransition);
   }

   public final void addStateTransition(StateTransition<E> stateTransition)
   {
      stateTransitions.add(stateTransition);
   }

   public final void addDelayBasedStateTransition(final E nextStateEnum, final double delay)
   {
      StateTransitionCondition stateTransitionCondition = new StateTransitionCondition()
      {
         @Override
         public boolean checkCondition()
         {
            return (getTimeInCurrentState() > delay);
         }
      };

      stateTransitions.add(new StateTransition<E>(nextStateEnum, stateTransitionCondition));
   }

   public final void addImmediateStateTransition(final E nextStateEnum)
   {
      StateTransitionCondition stateTransitionCondition = new StateTransitionCondition()
      {
         @Override
         public boolean checkCondition()
         {
            return true;
         }
      };

      stateTransitions.add(new StateTransition<E>(nextStateEnum, stateTransitionCondition));
   }

   public final void setDefaultNextState(E nextStateEnum)
   {
      if (defaultNextStateTransition != null)
         throw new RuntimeException("Have already set default next state for " + stateEnum);
      defaultNextStateTransition = new StateTransition<E>(nextStateEnum, (StateTransitionCondition) null);
   }

   public final StateTransition<E> getDefaultNextStateTransition()
   {
      return defaultNextStateTransition;
   }

   public final void transitionToDefaultNextState()
   {
      gotoDefaultNextState = true;
   }

   public final double getTimeInCurrentState()
   {
      return timeInCurrentStateProvider.timeInCurrentState();
   }

   public final ArrayList<StateTransition<E>> getStateTransitions()
   {
      return stateTransitions;
   }

   public final E getStateEnum()
   {
      if(stateEnum == null)
         throw new RuntimeException("A state has not had its state enume set stateEnum is null for this state.");
      return stateEnum;
   }

   public final State<E> getPreviousState()
   {
      return previousStateProvider.getPreviousState();
   }

   public String toString()
   {
      StringBuffer stringBuffer = new StringBuffer();

      stringBuffer.append(stateEnum + ": (");

      boolean firstTime = true;
      for (StateTransition<E> stateTransition : stateTransitions)
      {
         if (!firstTime)
            stringBuffer.append(", ");
         stringBuffer.append(stateTransition.getNextStateEnum());
         firstTime = false;
      }

      stringBuffer.append(")");

      return stringBuffer.toString();
   }

   // Default methods. User should not use these. Only executor of a state machine should use these.
   final boolean getTransitionToDefaultNextState()
   {
      return gotoDefaultNextState;
   }

   final void setTimeInCurrentStateProvider(TimeInCurrentStateProvider timeInCurrentStateProvider)
   {
      this.timeInCurrentStateProvider = timeInCurrentStateProvider;
   }

   final void setPreviousStateProvider(PreviousStateProvider<E, State<E>> provider)
   {
      this.previousStateProvider = provider;
   }

   final void clearTransitionToDefaultNextState()
   {
      gotoDefaultNextState = false;
   }

   StateTransition<E> checkTransitionConditions(double timeInState)
   {
      for (int i = 0; i < stateTransitions.size(); i++)
      {
         StateTransition<E> stateTransition = stateTransitions.get(i);

         E nextStateEnum = stateTransition.checkTransitionConditions(timeInState);
         if (nextStateEnum != null)
         {
            return stateTransition;
         }
      }

      return null;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AdjustFootstepMessage.java<|end_filename|>
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.
       * The intent of this message is to adjust a footstep when the robot is executing it
       * (a foot is currently swinging to reach the footstep to be adjusted).
       */
public class AdjustFootstepMessage extends Packet<AdjustFootstepMessage> implements Settable<AdjustFootstepMessage>, EpsilonComparable<AdjustFootstepMessage>
{
   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 which foot is expected to be executing the footstep to be adjusted.
            */
   public byte robot_side_ = (byte) 255;
   /**
            * Specifies the adjusted position of the footstep. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple3D.Point3D location_;
   /**
            * Specifies the adjusted orientation of the footstep. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple4D.Quaternion orientation_;
   /**
            * Predicted contact points represent the vertices of the expected contact polygon between the foot and the world.
            * An empty list will request the controller to use the default foot support polygon.
            * Contact points  are expressed in sole frame. The ordering does not matter.
            * For example: to tell the controller to use the entire foot, the predicted contact points would be:
            * - x: 0.5 * foot_length, y: -0.5 * toe_width
            * - x: 0.5 * foot_length, y: 0.5 * toe_width
            * - x: -0.5 * foot_length, y: -0.5 * heel_width
            * - x: -0.5 * foot_length, y: 0.5 * heel_width
            * Note: The z coordinate of each point is ignored.
            */
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  predicted_contact_points_2d_;
   /**
            * The time to delay this command on the controller side before being executed.
            */
   public double execution_delay_time_;

   public AdjustFootstepMessage()
   {
      location_ = new us.ihmc.euclid.tuple3D.Point3D();
      orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
      predicted_contact_points_2d_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D> (100, new geometry_msgs.msg.dds.PointPubSubType());

   }

   public AdjustFootstepMessage(AdjustFootstepMessage other)
   {
      this();
      set(other);
   }

   public void set(AdjustFootstepMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.location_, location_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.orientation_, orientation_);
      predicted_contact_points_2d_.set(other.predicted_contact_points_2d_);
      execution_delay_time_ = other.execution_delay_time_;

   }

   /**
            * 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_;
   }

   /**
            * Specifies which foot is expected to be executing the footstep to be adjusted.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * Specifies which foot is expected to be executing the footstep to be adjusted.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }


   /**
            * Specifies the adjusted position of the footstep. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple3D.Point3D getLocation()
   {
      return location_;
   }


   /**
            * Specifies the adjusted orientation of the footstep. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple4D.Quaternion getOrientation()
   {
      return orientation_;
   }


   /**
            * Predicted contact points represent the vertices of the expected contact polygon between the foot and the world.
            * An empty list will request the controller to use the default foot support polygon.
            * Contact points  are expressed in sole frame. The ordering does not matter.
            * For example: to tell the controller to use the entire foot, the predicted contact points would be:
            * - x: 0.5 * foot_length, y: -0.5 * toe_width
            * - x: 0.5 * foot_length, y: 0.5 * toe_width
            * - x: -0.5 * foot_length, y: -0.5 * heel_width
            * - x: -0.5 * foot_length, y: 0.5 * heel_width
            * Note: The z coordinate of each point is ignored.
            */
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  getPredictedContactPoints2d()
   {
      return predicted_contact_points_2d_;
   }

   /**
            * The time to delay this command on the controller side before being executed.
            */
   public void setExecutionDelayTime(double execution_delay_time)
   {
      execution_delay_time_ = execution_delay_time;
   }
   /**
            * The time to delay this command on the controller side before being executed.
            */
   public double getExecutionDelayTime()
   {
      return execution_delay_time_;
   }


   public static Supplier<AdjustFootstepMessagePubSubType> getPubSubType()
   {
      return AdjustFootstepMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AdjustFootstepMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AdjustFootstepMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!this.location_.epsilonEquals(other.location_, epsilon)) return false;
      if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false;
      if (this.predicted_contact_points_2d_.size() != other.predicted_contact_points_2d_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.predicted_contact_points_2d_.size(); i++)
         {  if (!this.predicted_contact_points_2d_.get(i).epsilonEquals(other.predicted_contact_points_2d_.get(i), epsilon)) return false; }
      }

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_delay_time_, other.execution_delay_time_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AdjustFootstepMessage)) return false;

      AdjustFootstepMessage otherMyClass = (AdjustFootstepMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if (!this.location_.equals(otherMyClass.location_)) return false;
      if (!this.orientation_.equals(otherMyClass.orientation_)) return false;
      if (!this.predicted_contact_points_2d_.equals(otherMyClass.predicted_contact_points_2d_)) return false;
      if(this.execution_delay_time_ != otherMyClass.execution_delay_time_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AdjustFootstepMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("location=");
      builder.append(this.location_);      builder.append(", ");
      builder.append("orientation=");
      builder.append(this.orientation_);      builder.append(", ");
      builder.append("predicted_contact_points_2d=");
      builder.append(this.predicted_contact_points_2d_);      builder.append(", ");
      builder.append("execution_delay_time=");
      builder.append(this.execution_delay_time_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisOrientationControlState.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controlModules.pelvis;

import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.FeedbackControlCommand;
import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.InverseDynamicsCommand;
import us.ihmc.robotics.stateMachine.core.State;

public interface PelvisOrientationControlState extends State
{
   @Override
   default boolean isDone(double timeInState)
   {
      return true;
   }

   default InverseDynamicsCommand<?> getInverseDynamicsCommand()
   {
      return null;
   }

   @Override
   default void onEntry()
   {
   }

   @Override
   default void onExit()
   {
   }

   public abstract void goToHomeFromCurrentDesired(double trajectoryTime);

   public abstract FeedbackControlCommand<?> getFeedbackControlCommand();
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosImuPublisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import org.ros.message.Time;

import controller_msgs.msg.dds.IMUPacket;
import geometry_msgs.Quaternion;
import geometry_msgs.Vector3;
import std_msgs.Header;
import us.ihmc.euclid.tuple3D.Vector3D32;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.euclid.tuple4D.Quaternion32;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;

public class RosImuPublisher extends RosTopicPublisher<sensor_msgs.Imu>
{
   int counter=0;
   public RosImuPublisher(boolean latched)
   {
      super(sensor_msgs.Imu._TYPE,latched);
   }
   
   public void publish(long timestamp, Vector3DReadOnly linearAcceleration, QuaternionReadOnly orientation, Vector3DReadOnly angularVelocity, String frameId)
   {
      sensor_msgs.Imu message = getMessage();

      Vector3  localLinearAcceleration = newMessageFromType(Vector3._TYPE);
      localLinearAcceleration.setX(linearAcceleration.getX());
      localLinearAcceleration.setY(linearAcceleration.getY());
      localLinearAcceleration.setZ(linearAcceleration.getZ());
      
      Quaternion localOrientation = newMessageFromType(Quaternion._TYPE);
      localOrientation.setW(orientation.getS());
      localOrientation.setX(orientation.getX());
      localOrientation.setY(orientation.getY());
      localOrientation.setZ(orientation.getZ());
      
      Vector3 localAngularVelocity = newMessageFromType(Vector3._TYPE);
      localAngularVelocity.setX(angularVelocity.getX());
      localAngularVelocity.setY(angularVelocity.getY());
      localAngularVelocity.setZ(angularVelocity.getZ());
      
      Header header = newMessageFromType(Header._TYPE);
      header.setFrameId(frameId);
      header.setStamp(Time.fromNano(timestamp));
      header.setSeq(counter);
      counter++;
            
      message.setHeader(header);
      message.setLinearAcceleration(localLinearAcceleration);
      message.setOrientation(localOrientation);
      message.setAngularVelocity(localAngularVelocity);
      
      publish(message);
   }

   public void publish(long timeStamp, IMUPacket imuPacketForSensor, String frameId)
   {
      publish(timeStamp, imuPacketForSensor.getLinearAcceleration(), imuPacketForSensor.getOrientation(),
            imuPacketForSensor.getAngularVelocity(), frameId);
   }
}

<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/io/ProjectDirectoryTools.java<|end_filename|>
package us.ihmc.tools.io;

import java.net.URI;
import java.net.URISyntaxException;
import java.net.URL;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;

/**
 * Class with hacks to find the project folder of a running program
 * 
 * This only works if you run from source. The heuristics used are
 * 
 * - Your compiled class files are in the project folder
 * - Your current working directory is the project folder
 * - Your current working directory is the top level directory of your project folder
 * 
 * Requirements
 * - A build.gradle file is expected in the folder
 * - Standard maven structure is used
 *  
 * @author jesper
 *
 */
public class ProjectDirectoryTools
{
   private final static String buildFile = "build.gradle";
   private final static String sourceDirectory = "src";
   private final static String groupDirectory = "main";
   private final static String resourceDirectory = "resources";
         
   
   public static Path getProjectDirectory(Class<?> classInProject, String projectName)
   {
      // Step 1: Try super directories of the compiled class file
      URL myURL = classInProject.getResource(classInProject.getSimpleName() + ".class");
      try
      {
         URI myURI = myURL.toURI();
         String scheme = myURI.getScheme();
         if(scheme != null && scheme.equalsIgnoreCase("file"))
         {
            Path myPath = Paths.get(myURI);
            
            Path currentPath = myPath;
            while((currentPath = currentPath.getParent()) != null)
            {
               if(isProjectDirectory(projectName, currentPath))
               {
                  return currentPath;
               }
            }
            
         }
      }
      catch (URISyntaxException | IllegalArgumentException e)
      {
         // Ignore, try other heuristics
      }
      
      // Step 2: Try if working directory is current path
      Path workingDirectory = Paths.get(".").toAbsolutePath().normalize();
      if(workingDirectory.getFileName() != null)
      {  
         if(projectName.equals(workingDirectory.getFileName().toString()))
         {
            if(isProjectDirectory(projectName, workingDirectory))
            {
               return workingDirectory;
            }
         }
         
      }
      
      // Step 3: Try if the project is in the current working directory
      Path projectPath = Paths.get(projectName);
      if(isProjectDirectory(projectName, projectPath))
      {
         return projectPath;
      }
      
      return null;

   }
   
   /**
    * Try to get the main resource directory.
    * 
    *  This function is heuristic based. Use only to pre-populate a load/save dialog and don't automatically write
    * 
    * @param classInProject A class that is in the project you want the resource folder off
    * @param projectName Name of the project to match to
    * @return File with the resource directory or null if not known
    */
   public static Path getMainResourceDirectory(Class<?> classInProject, String projectName)
   {
      Path projectDirectory = getProjectDirectory(classInProject, projectName);
      if(projectDirectory != null)
      {
         Path resourceDirectoryPath = projectDirectory.resolve(Paths.get(sourceDirectory, groupDirectory, resourceDirectory));
         if(Files.exists(resourceDirectoryPath))
         {
            return resourceDirectoryPath;
         }
      }
      
      return null;
      
   }

   private static boolean isProjectDirectory(String projectName, Path currentPath)
   {
      Path file = currentPath.getFileName();
      if(file != null)
      {
         String name = currentPath.getFileName().toString();
         if(projectName.equals(name))
         {
            if(Files.exists(currentPath.resolve(buildFile)))
            {
               return true;
               
            }
         }
      }
      return false;
   }
   
   public static void main(String[] args)
   {
      System.out.println(getMainResourceDirectory(ProjectDirectoryTools.class, "ihmc-java-toolkit"));
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ExoskeletonBehaviorStatePacket.java<|end_filename|>
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 used to switch the control scheme between force and position control.
       * WARNING: When in position control, the IHMC balance algorithms will be disabled and it is up to the user to ensure stability.
       */
public class ExoskeletonBehaviorStatePacket extends Packet<ExoskeletonBehaviorStatePacket> implements Settable<ExoskeletonBehaviorStatePacket>, EpsilonComparable<ExoskeletonBehaviorStatePacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte exoskeleton_behavior_state_;

   public ExoskeletonBehaviorStatePacket()
   {
   }

   public ExoskeletonBehaviorStatePacket(ExoskeletonBehaviorStatePacket other)
   {
      this();
      set(other);
   }

   public void set(ExoskeletonBehaviorStatePacket other)
   {
      sequence_id_ = other.sequence_id_;

      exoskeleton_behavior_state_ = other.exoskeleton_behavior_state_;

   }

   /**
            * 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 void setExoskeletonBehaviorState(byte exoskeleton_behavior_state)
   {
      exoskeleton_behavior_state_ = exoskeleton_behavior_state;
   }
   public byte getExoskeletonBehaviorState()
   {
      return exoskeleton_behavior_state_;
   }


   public static Supplier<ExoskeletonBehaviorStatePacketPubSubType> getPubSubType()
   {
      return ExoskeletonBehaviorStatePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ExoskeletonBehaviorStatePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ExoskeletonBehaviorStatePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.exoskeleton_behavior_state_, other.exoskeleton_behavior_state_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ExoskeletonBehaviorStatePacket)) return false;

      ExoskeletonBehaviorStatePacket otherMyClass = (ExoskeletonBehaviorStatePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.exoskeleton_behavior_state_ != otherMyClass.exoskeleton_behavior_state_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ExoskeletonBehaviorStatePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("exoskeleton_behavior_state=");
      builder.append(this.exoskeleton_behavior_state_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/InputDevice.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;


public interface InputDevice
{
   public String getName();

   public String getDescription();


}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/Layer.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork;

import java.util.ArrayList;

/**
 * A single layer of a Neural Network
 */
public class Layer
{
   ArrayList<Neuron> neurons = new ArrayList<>();

   public void addNeuron(Neuron neuron)
   {
      neurons.add(neuron);
   }

   public void compute()
   {
      for (int i = 0; i < neurons.size(); i++)
      {
         Neuron neuron = neurons.get(i);
         neuron.compute();
      }
   }

   public ArrayList<Neuron> getNeurons()
   {
      return neurons;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SpineTrajectoryMessage.java<|end_filename|>
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 commands the controller to move the spine in jointspace to the desired joint angles while going through the specified trajectory points.
       * A third order polynomial function is used to interpolate between trajectory points.
       */
public class SpineTrajectoryMessage extends Packet<SpineTrajectoryMessage> implements Settable<SpineTrajectoryMessage>, EpsilonComparable<SpineTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The trajectories for each joint in order from the one closest to the pelvis to the one the closest to the chest.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage jointspace_trajectory_;

   public SpineTrajectoryMessage()
   {
      jointspace_trajectory_ = new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
   }

   public SpineTrajectoryMessage(SpineTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(SpineTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.staticCopy(other.jointspace_trajectory_, jointspace_trajectory_);
   }

   /**
            * 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_;
   }


   /**
            * The trajectories for each joint in order from the one closest to the pelvis to the one the closest to the chest.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage getJointspaceTrajectory()
   {
      return jointspace_trajectory_;
   }


   public static Supplier<SpineTrajectoryMessagePubSubType> getPubSubType()
   {
      return SpineTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SpineTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SpineTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.jointspace_trajectory_.epsilonEquals(other.jointspace_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SpineTrajectoryMessage)) return false;

      SpineTrajectoryMessage otherMyClass = (SpineTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.jointspace_trajectory_.equals(otherMyClass.jointspace_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SpineTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("jointspace_trajectory=");
      builder.append(this.jointspace_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/BehaviorVisualizer.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger;

import com.martiansoftware.jsap.JSAP;
import com.martiansoftware.jsap.JSAPException;
import com.martiansoftware.jsap.JSAPResult;
import com.martiansoftware.jsap.Switch;

import us.ihmc.robotDataLogger.YoVariableClient;
import us.ihmc.robotDataVisualizer.visualizer.SCSVisualizer;

public class BehaviorVisualizer extends SCSVisualizer
{
   public BehaviorVisualizer(int bufferSize)
   {
      super(bufferSize, true, false);
      setShowOverheadView(false);

      YoVariableClient client = new YoVariableClient(this);
      client.start();
   }

   public static void main(String[] args)
   {
      JSAP jsap = new JSAP();
      Switch runningOnRealRobotSwitch = new Switch("runningOnRealRobot").setLongFlag("realRobot");

      try
      {
         jsap.registerParameter(runningOnRealRobotSwitch);

         JSAPResult config = jsap.parse(args);

         if (config.success())
         {
            new BehaviorVisualizer(1024 * 32);
         }
      }
      catch (JSAPException e)
      {
         e.printStackTrace();
      }
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/frames/CommonReferenceFrameIds.java<|end_filename|>
package us.ihmc.sensorProcessing.frames;

public enum CommonReferenceFrameIds
{
   NONE(-1),
   MIDFEET_ZUP_FRAME(-100),
   PELVIS_ZUP_FRAME(-101),
   PELVIS_FRAME(-102),
   CHEST_FRAME(-103),
   CENTER_OF_MASS_FRAME(-104),
   LEFT_SOLE_FRAME(-105),
   RIGHT_SOLE_FRAME(-106);

   private final long hashId;

   CommonReferenceFrameIds(long hashId)
   {
      this.hashId = hashId;
   }

   public long getHashId()
   {
      return hashId;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/hyperCubeTree/LineSegmentSearchVolume.java<|end_filename|>
package us.ihmc.robotics.hyperCubeTree;

import us.ihmc.euclid.geometry.LineSegment3D;
import us.ihmc.euclid.tuple2D.Point2D;

public class LineSegmentSearchVolume extends HyperVolume
{
   final int dim;
   final private double[] pointA;
   final private double[] pointB;
   final private double[] scale;
   public LineSegmentSearchVolume(LineSegment3D lineSegment)
   {
      super(3, computeOuterBounds(lineSegment), null, true);
      dim = 3;
      pointA = new double[dim];
      pointB = new double[dim];
      scale = new double[dim];
      lineSegment.getFirstEndpoint().get(pointA);
      lineSegment.getSecondEndpoint().get(pointB);
      for (int i=0;i<dim;i++)
      {
         scale[i]=1.0/(pointB[i]-pointA[i]);
      }  
   }
   private static OneDimensionalBounds[] computeOuterBounds(LineSegment3D lineSegment)
   {
      double[] aVals = new double[3];
      lineSegment.getFirstEndpoint().get(aVals);
      double[] bVals = new double[3];
      lineSegment.getSecondEndpoint().get(bVals);
      OneDimensionalBounds[] ret = new OneDimensionalBounds[3];
      for (int i = 0; i < 3; i++)
      {
         ret[i] = new OneDimensionalBounds(Math.min(aVals[i], bVals[i]), Math.max(aVals[i], bVals[i]));
      }
      return ret;
   }
   public LineSegmentSearchVolume(Point2D point1, Point2D point2)
   {
      this(new double[]{point1.getX(),point1.getY()},new double[]{point2.getX(),point2.getY()});
   }
   
   public LineSegmentSearchVolume(double[] point1, double[] point2)
   {
      super(point1.length,calculateBounds(point1, point2), null,true);
      dim = point1.length;
      pointA=point1;
      pointB=point2;
      scale = new double[dim];
      for (int i=0;i<dim;i++)
      {
         scale[i]=1.0/(pointB[i]-pointA[i]);
      }  
   }
   private static OneDimensionalBounds[] calculateBounds(double[] point1, double[] point2)
   {
      if (point1.length != point2.length)
         throw new DimensionalityMismatchException();
      int dim = point1.length;
      OneDimensionalBounds[] bounds = new OneDimensionalBounds[dim];
      for (int i=0;i<dim;i++)
         bounds[i]=new OneDimensionalBounds(point1[i], point2[i]);
      return bounds;
   }

   
   protected boolean complexBoundsIntersect(OneDimensionalBounds[] bounds)
   {
      OneDimensionalBounds boundaryRepresentedInLineParameterSpace;
      OneDimensionalBounds accumulatorBounds = new OneDimensionalBounds(0.0,1.0);
      for (int i=0;i<dim;i++)
      {
         boundaryRepresentedInLineParameterSpace = transformOneDimensionalBoundsToLineParameterSpace(bounds, i);
         accumulatorBounds=accumulatorBounds.intersectionWith(boundaryRepresentedInLineParameterSpace);
         if (null == accumulatorBounds)
            return false;
      }
      return true;
   }

   private OneDimensionalBounds transformOneDimensionalBoundsToLineParameterSpace(OneDimensionalBounds[] bounds, int i)
   {
      double scale = this.scale[i];
      double translation = pointA[i];
      if (Double.isInfinite(scale))
         return OneDimensionalBounds.unbounded();
      return new OneDimensionalBounds(((bounds[i].min()-translation)*scale),((bounds[i].max()-translation)*scale));
   }

   protected boolean containsBoundsIfWithinOuterBounds(OneDimensionalBounds[] bounds)
   {
      return false;
   }

   protected boolean containsPointIfWithinOuterBounds(double[] point)
   {
      return false;
   }


   @Override
   protected double[] pointWithin(OneDimensionalBounds[] bounds)
   {
      OneDimensionalBounds sBounds = sBoundsWithin(bounds);
      double sValue = sBounds.midpoint();
      double[] ret = new double[dim];
      for (int i = 0; i<dim ; i++ )
      {
         ret[i]=pointA[i]*(1-sValue)+pointB[i]*sValue;
      }
      return ret;
   }
   
   private OneDimensionalBounds sBoundsWithin(OneDimensionalBounds[] bounds)
   {
      OneDimensionalBounds boundaryRepresentedInLineParameterSpace;
      OneDimensionalBounds accumulatorBounds = new OneDimensionalBounds(0.0,1.0);
      for (int i=0;i<dim;i++)
      {
         boundaryRepresentedInLineParameterSpace = transformOneDimensionalBoundsToLineParameterSpace(bounds, i);
         accumulatorBounds=accumulatorBounds.intersectionWith(boundaryRepresentedInLineParameterSpace);
         if (null == accumulatorBounds)
            return null;
      }
      return accumulatorBounds;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/RawIMUSensorsInterface.java<|end_filename|>
package us.ihmc.robotics.sensors;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.tuple3D.Vector3D;

public interface RawIMUSensorsInterface
{
   public abstract void setOrientation(RotationMatrix orientation, int imuIndex);

   public abstract void setAcceleration(Vector3D acceleration, int imuIndex);

   public abstract void setAngularVelocity(Vector3D angularVelocity, int imuIndex);

   public abstract void setCompass(Vector3D compass, int imuIndex);

   public abstract void getOrientation(RotationMatrix orientationToPack, int imuIndex);

   public abstract void getAcceleration(Vector3D accelerationToPack, int imuIndex);

   public abstract void getAngularVelocity(Vector3D angularVelocityToPack, int imuIndex);

   public abstract void getCompass(Vector3D compassToPack, int imuIndex);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/AbstractInverseDynamicsCopier.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.ArrayList;

import org.apache.commons.lang3.tuple.ImmutablePair;

import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.tools.MultiBodySystemTools;

public abstract class AbstractInverseDynamicsCopier
{  
   private final ArrayList<ImmutablePair<JointBasics, JointBasics>> jointPairs = new ArrayList<ImmutablePair<JointBasics,JointBasics>>();
   
   
   public AbstractInverseDynamicsCopier(RigidBodyBasics originalBody, RigidBodyBasics targetBody)
   {
      setRigidBodies(originalBody, targetBody);
   }

   public void setRigidBodies(RigidBodyBasics originalBody, RigidBodyBasics targetBody)
   {
      jointPairs.clear();
      JointBasics[] originalJoints = MultiBodySystemTools.collectSubtreeJoints(originalBody);
      JointBasics[] targetJoints = MultiBodySystemTools.collectSubtreeJoints(targetBody);
      
      for(int i = 0; i < originalJoints.length; i++)
      {      
         JointBasics originalJoint = originalJoints[i];
         JointBasics targetJoint = targetJoints[i];
         
         areJointsTheSame(originalJoint, targetJoint);
         
         ImmutablePair<JointBasics, JointBasics> jointPair = new ImmutablePair<JointBasics, JointBasics>(originalJoint, targetJoint);
         
         jointPairs.add(jointPair);
      }
   }
   
   
   public final void copy()
   {
      for(int i = 0; i <  jointPairs.size(); i++)
      {
         ImmutablePair<JointBasics,JointBasics> jointPair = jointPairs.get(i);
         JointBasics originalJoint = jointPair.getLeft();
         JointBasics targetJoint = jointPair.getRight();
         
         copyJoint(originalJoint, targetJoint);
         
      }
   }
   
   protected abstract void copyJoint(JointBasics originalJoint, JointBasics targetJoint);
   
   
   private static final void areJointsTheSame(JointBasics originalJoint, JointBasics targetJoint)
   {
      if(!(originalJoint.getClass().equals(targetJoint.getClass()) && 
            originalJoint.getName().equals(targetJoint.getName())))
      {
         throw new RuntimeException(originalJoint.getName() + " differs from " + targetJoint);
      }
      
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/RosCapturabilityBasedStatusPublisher.java<|end_filename|>
package us.ihmc.avatar.ros;

import java.util.concurrent.ArrayBlockingQueue;

import controller_msgs.msg.dds.CapturabilityBasedStatus;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.publisher.RosBoolPublisher;
import us.ihmc.utilities.ros.publisher.RosPoint2dPublisher;
import us.ihmc.utilities.ros.publisher.RosPoint32Publisher;

/**
 *
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class RosCapturabilityBasedStatusPublisher implements PacketConsumer<CapturabilityBasedStatus>, Runnable
{
   private final ArrayBlockingQueue<CapturabilityBasedStatus> availableCapturabilityStatus = new ArrayBlockingQueue<>(30);

   private final RosPoint2dPublisher capturePointPublisher = new RosPoint2dPublisher(false);
   private final RosPoint2dPublisher desiredCapturePointPublisher = new RosPoint2dPublisher(false);
   private final RosPoint32Publisher centerOfMassPublisher = new RosPoint32Publisher(false);
   
   private final RosBoolPublisher isInDoubleSupportPublisher = new RosBoolPublisher(false);

   private final SideDependentList<RosSupportPolygonPublisher> supportPolygonPublishers = new SideDependentList<>();
   private final RosMainNode rosMainNode;

   public RosCapturabilityBasedStatusPublisher(RosMainNode rosMainNode, String rosNameSpace)
   {
      this.rosMainNode = rosMainNode;

      for (RobotSide value : RobotSide.values)
      {
         RosSupportPolygonPublisher rosSupportPolygonPublisher = new RosSupportPolygonPublisher(false);
         supportPolygonPublishers.put(value, rosSupportPolygonPublisher);
         this.rosMainNode
               .attachPublisher(rosNameSpace + "/output/capturability/" + value.getCamelCaseNameForStartOfExpression() + "_foot_support_polygon", rosSupportPolygonPublisher);
      }

      this.rosMainNode.attachPublisher(rosNameSpace + "/output/capturability/capture_point", capturePointPublisher);
      this.rosMainNode.attachPublisher(rosNameSpace + "/output/capturability/desired_capture_point", desiredCapturePointPublisher);
      this.rosMainNode.attachPublisher(rosNameSpace + "/output/capturability/center_of_mass", centerOfMassPublisher);
      this.rosMainNode.attachPublisher(rosNameSpace + "/output/capturability/is_in_double_support", isInDoubleSupportPublisher);

      new Thread(this, getClass().getName()).start();
   }

   @Override
   public void receivedPacket(CapturabilityBasedStatus packet)
   {
      if (!availableCapturabilityStatus.offer(packet))
      {
         availableCapturabilityStatus.clear();
      }
   }

   @Override
   public void run()
   {
      while (true)
      {
         CapturabilityBasedStatus capturabilityBasedStatus;
         try
         {
            capturabilityBasedStatus = availableCapturabilityStatus.take();
         }
         catch (InterruptedException e)
         {
            continue;
         }

         if (rosMainNode.isStarted())
         {
            capturePointPublisher.publish(new Point2D(capturabilityBasedStatus.getCapturePoint2d()));
            desiredCapturePointPublisher.publish(new Point2D(capturabilityBasedStatus.getDesiredCapturePoint2d()));
            centerOfMassPublisher.publish(capturabilityBasedStatus.getCenterOfMass3d());
            isInDoubleSupportPublisher.publish(HumanoidMessageTools.unpackIsInDoubleSupport(capturabilityBasedStatus));

//            for (RobotSide value : RobotSide.values)
//            {
//               RosSupportPolygonPublisher rosSupportPolygonPublisher = supportPolygonPublishers.get(value);
//               switch (value)
//               {
//               case LEFT:
//                  rosSupportPolygonPublisher.publish(capturabilityBasedStatus.leftFootSupportPolygonStore, capturabilityBasedStatus.leftFootSupportPolygonLength);
//                  break;
//               case RIGHT:
//                  rosSupportPolygonPublisher.publish(capturabilityBasedStatus.rightFootSupportPolygonStore, capturabilityBasedStatus.rightFootSupportPolygonLength);
//                  break;
//               }
//            }
         }
      }
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/CTTaskNode.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import java.util.ArrayList;

import controller_msgs.msg.dds.KinematicsToolboxOutputStatus;
import us.ihmc.commons.PrintTools;

public class CTTaskNode
{
   protected NodeData nodeData;
   protected NodeData normalizedNodeData;

   private ArrayList<CTTaskNode> childNodes;
   private CTTaskNode parentNode;

   protected boolean validity = true;

   protected KinematicsToolboxOutputStatus configuration;

   public CTTaskNode(CTTaskNode node)
   {
      this.nodeData = new NodeData(node.nodeData);
      this.normalizedNodeData = new NodeData(node.normalizedNodeData);
      this.configuration = node.configuration;

      this.childNodes = node.childNodes;
      this.parentNode = node.parentNode;

      this.validity = node.validity;
   }

   public CTTaskNode(double[] rootData)
   {
      this.nodeData = new NodeData(rootData.length);
      this.nodeData.q = rootData;
      this.childNodes = new ArrayList<CTTaskNode>();
      this.normalizedNodeData = new NodeData(rootData.length);

   }

   public CTTaskNode(int dimensionOfData)
   {
      this.nodeData = new NodeData(dimensionOfData);
      this.childNodes = new ArrayList<CTTaskNode>();
      this.normalizedNodeData = new NodeData(dimensionOfData);
   }

   public final int getDimensionOfNodeData()
   {
      return nodeData.getDimension();
   }

   public final double getNodeData(int index)
   {
      return nodeData.getQ(index);
   }

   public final double getNormalizedNodeData(int index)
   {
      return normalizedNodeData.getQ(index);
   }

   public final double getDistance(CTTaskNode targetNode)
   {
      return nodeData.distance(targetNode.nodeData);
   }

   public final double getNormailzedDistance(CTTaskNode targetNode)
   {
      return normalizedNodeData.distance(targetNode.normalizedNodeData);
   }

   public final double getTimeGap(CTTaskNode targetNode)
   {
      return targetNode.getNodeData(0) - getNodeData(0);
   }

   public final double getNormalizedTimeGap(CTTaskNode targetNode)
   {
      return targetNode.getNormalizedNodeData(0) - getNormalizedNodeData(0);
   }

   public final void setNodeData(int index, double data)
   {
      nodeData.setQ(index, data);
   }

   public final void setNormalizedNodeData(int index, double data)
   {
      normalizedNodeData.setQ(index, data);
   }
   
   public final void setTimeData(double time)
   {
      nodeData.setQ(0, time);
   }

   public final void addChildNode(CTTaskNode node)
   {
      childNodes.add(node);
      node.setParentNode(this);
   }

   public final CTTaskNode getChildNode(int indexOfChild)
   {
      return childNodes.get(indexOfChild);
   }

   public final int getNumberOfChild()
   {
      return childNodes.size();
   }

   public final void setParentNode(CTTaskNode node)
   {
      parentNode = node;
   }

   public final void clearParentNode()
   {
      parentNode = null;
   }

   public final CTTaskNode getParentNode()
   {
      return parentNode;
   }

   public final void printNodeData()
   {
      for (int i = 0; i < getDimensionOfNodeData(); i++)
         PrintTools.info("" + i + " " + getNodeData(i));
   }

   public final double getTime()
   {
      return getNodeData(0);
   }

   public final void convertDataToNormalizedData(TaskRegion nodeRegion)
   {
      normalizedNodeData = new NodeData(getDimensionOfNodeData());
      for (int i = 0; i < getDimensionOfNodeData(); i++)
      {
         double normalizedValue = 0;
         if (i == 0)
         {
            normalizedValue = (getNodeData(i) - nodeRegion.getLowerLimit(i)) / nodeRegion.getTrajectoryTime();
         }
         else
         {
            if (nodeRegion.isEnable(i))
               normalizedValue = (getNodeData(i) - nodeRegion.getLowerLimit(i)) / nodeRegion.sizeOfRegion(i);
            else
               normalizedValue = 0;
         }
         normalizedNodeData.setQ(i, normalizedValue);
      }
   }

   public final void convertNormalizedDataToData(TaskRegion nodeRegion)
   {
      nodeData = new NodeData(getDimensionOfNodeData());
      for (int i = 0; i < getDimensionOfNodeData(); i++)
      {
         double value = 0;
         if (i == 0)
         {
            value = getNormalizedNodeData(i) * nodeRegion.getTrajectoryTime() + nodeRegion.getLowerLimit(i);
         }
         else
         {
            value = getNormalizedNodeData(i) * nodeRegion.sizeOfRegion(i) + nodeRegion.getLowerLimit(i);
         }
         nodeData.setQ(i, value);
      }
   }

   /**
    * alpha is within 0 ~ 1 and a ratio from this to towardNode.
    * if i, the createdNewNode would be towardNode.
    * if 0, the createdNewNode would be this.
    */
   public final CTTaskNode createNewNodeTowardNode(CTTaskNode towardNode, double alpha)
   {
      /*
       * clamping alpha first.
       */
      if (alpha > 1)
         alpha = 1;
      else if (alpha < 0)
         alpha = 0;
      else
         ;

      CTTaskNode createdNewNode = new CTTaskNode(this);

      for (int i = 0; i < createdNewNode.getDimensionOfNodeData(); i++)
      {
         double stepToward;
         stepToward = (towardNode.getNodeData(i) - this.getNodeData(i)) * alpha;
         createdNewNode.setNodeData(i, this.getNodeData(i) + stepToward);

         stepToward = (towardNode.getNormalizedNodeData(i) - this.getNormalizedNodeData(i)) * alpha;
         createdNewNode.setNormalizedNodeData(i, this.getNormalizedNodeData(i) + stepToward);
      }
      createdNewNode.setParentNode(this);
      return createdNewNode;
   }

   public final void setConfigurationJoints(KinematicsToolboxOutputStatus outputStatus)
   {
      this.configuration = new KinematicsToolboxOutputStatus(outputStatus);
   }

   public KinematicsToolboxOutputStatus getConfiguration()
   {
      return configuration;
   }

   public void setValidity(boolean value)
   {
      validity = value;
   }

   public boolean getValidity()
   {
      return validity;
   }

}

<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/scripts/engine/MessageTransformer.java<|end_filename|>
package us.ihmc.humanoidBehaviors.behaviors.scripts.engine;

import java.lang.reflect.Field;
import java.util.HashMap;
import java.util.Map;

import controller_msgs.msg.dds.AdjustFootstepMessage;
import controller_msgs.msg.dds.EuclideanTrajectoryMessage;
import controller_msgs.msg.dds.FootstepDataMessage;
import controller_msgs.msg.dds.SE3TrajectoryMessage;
import controller_msgs.msg.dds.SO3TrajectoryMessage;
import controller_msgs.msg.dds.WrenchTrajectoryMessage;
import geometry_msgs.msg.dds.Wrench;
import us.ihmc.commons.lists.PreallocatedList;
import us.ihmc.euclid.interfaces.Transformable;
import us.ihmc.euclid.transform.RigidBodyTransform;

/**
 * This is an highly inefficient way of enabling the transformation of messages using a
 * {@code RigidBodyTransform}.
 * <p>
 * This class was created to remove custom features implemented in messages that were including a
 * transformation method. This class should be used as little as possible and eventually deleted.
 * </p>
 */
@SuppressWarnings("rawtypes")
public final class MessageTransformer
{
   private static final Map<Class, CustomTransformer> customTransformers = createCustomTransformers();

   private MessageTransformer()
   {
   }

   /**
    * Recursively transforms the fields, including sub-fields, that implement {@code Transformable}
    * of the given {@code message}.
    * <p>
    * Some types are handling in a specific manner as {@code AdjustFootstepMessage} and
    * {@code FootstepDataMessage}.
    * </p>
    * 
    * @param message the object to be transformed. Modified.
    * @param rigidBodyTransformToApply the transform to apply on the given message. Not modified.
    */
   @SuppressWarnings({"unchecked"})
   public static void transform(Object message, RigidBodyTransform rigidBodyTransformToApply)
   {
      if (message == null || isPrimitive(message.getClass()))
         return;

      if (customTransformers.containsKey(message.getClass()))
      {
         customTransformers.get(message.getClass()).transform(message, rigidBodyTransformToApply);
      }
      else if (message instanceof PreallocatedList)
      {
         PreallocatedList list = (PreallocatedList) message;
         for (int i = 0; i < list.size(); i++)
            transform(list.get(i), rigidBodyTransformToApply);
      }
      else if (message instanceof Iterable)
      {
         for (Object element : (Iterable) message)
            transform(element, rigidBodyTransformToApply);
      }
      else if (message.getClass().isArray())
      {
         for (Object element : (Object[]) message)
            transform(element, rigidBodyTransformToApply);
      }
      else
      {
         if (message instanceof Transformable)
         {
            ((Transformable) message).applyTransform(rigidBodyTransformToApply);
            return;
         }

         if (Enum.class.isAssignableFrom(message.getClass()))
            return;

         Field[] fields = message.getClass().getFields();

         for (Field field : fields)
         {
            try
            {
               transform(field.get(message), rigidBodyTransformToApply);
            }
            catch (IllegalArgumentException | IllegalAccessException e)
            {
               e.printStackTrace();
               return;
            }
         }
      }
   }

   private static Map<Class, CustomTransformer> createCustomTransformers()
   {
      Map<Class, CustomTransformer> customTransformers = new HashMap<>();

      customTransformers.put(AdjustFootstepMessage.class, new CustomTransformer<AdjustFootstepMessage>()
      {
         @Override
         public void transform(AdjustFootstepMessage object, RigidBodyTransform rigidBodyTransformToApply)
         {
            object.getLocation().applyTransform(rigidBodyTransformToApply);
            object.getOrientation().applyTransform(rigidBodyTransformToApply);
         }
      });

      customTransformers.put(FootstepDataMessage.class, new CustomTransformer<FootstepDataMessage>()
      {
         @Override
         public void transform(FootstepDataMessage message, RigidBodyTransform rigidBodyTransformToApply)
         {
            if (message.getLocation() != null)
               message.getLocation().applyTransform(rigidBodyTransformToApply);
            if (message.getOrientation() != null)
               message.getOrientation().applyTransform(rigidBodyTransformToApply);

            if (message.getCustomPositionWaypoints() != null)
            {
               for (int i = 0; i < message.getCustomPositionWaypoints().size(); i++)
                  message.getCustomPositionWaypoints().get(i).applyTransform(rigidBodyTransformToApply);
            }
         }
      });

      customTransformers.put(SE3TrajectoryMessage.class, new CustomTransformer<SE3TrajectoryMessage>()
      {
         @Override
         public void transform(SE3TrajectoryMessage object, RigidBodyTransform rigidBodyTransformToApply)
         {
            MessageTransformer.transform(object.getTaskspaceTrajectoryPoints(), rigidBodyTransformToApply);
         }
      });

      customTransformers.put(SO3TrajectoryMessage.class, new CustomTransformer<SO3TrajectoryMessage>()
      {
         @Override
         public void transform(SO3TrajectoryMessage object, RigidBodyTransform rigidBodyTransformToApply)
         {
            MessageTransformer.transform(object.getTaskspaceTrajectoryPoints(), rigidBodyTransformToApply);
         }
      });

      customTransformers.put(EuclideanTrajectoryMessage.class, new CustomTransformer<EuclideanTrajectoryMessage>()
      {
         @Override
         public void transform(EuclideanTrajectoryMessage object, RigidBodyTransform rigidBodyTransformToApply)
         {
            MessageTransformer.transform(object.getTaskspaceTrajectoryPoints(), rigidBodyTransformToApply);
         }
      });

      customTransformers.put(WrenchTrajectoryMessage.class, new CustomTransformer<WrenchTrajectoryMessage>()
      {
         @Override
         public void transform(WrenchTrajectoryMessage object, RigidBodyTransform rigidBodyTransformToApply)
         {
            MessageTransformer.transform(object.getWrenchTrajectoryPoints(), rigidBodyTransformToApply);
         }
      });

      customTransformers.put(Wrench.class, new CustomTransformer<Wrench>()
      {
         @Override
         public void transform(Wrench object, RigidBodyTransform rigidBodyTransformToApply)
         {
            object.getForce().applyTransform(rigidBodyTransformToApply);
            object.getTorque().applyTransform(rigidBodyTransformToApply);
         }
      });

      return customTransformers;
   }

   private static boolean isPrimitive(Class<?> clazz)
   {
      if (clazz.equals(boolean.class) || clazz.equals(boolean[].class) || clazz.equals(Boolean.class))
         return true;
      if (clazz.equals(byte.class) || clazz.equals(byte[].class) || clazz.equals(Byte.class))
         return true;
      if (clazz.equals(char.class) || clazz.equals(char[].class) || clazz.equals(Character.class))
         return true;
      if (clazz.equals(short.class) || clazz.equals(short[].class) || clazz.equals(Short.class))
         return true;
      if (clazz.equals(int.class) || clazz.equals(int[].class) || clazz.equals(Integer.class))
         return true;
      if (clazz.equals(long.class) || clazz.equals(long[].class) || clazz.equals(Long.class))
         return true;
      if (clazz.equals(float.class) || clazz.equals(float[].class) || clazz.equals(Float.class))
         return true;
      if (clazz.equals(double.class) || clazz.equals(double[].class) || clazz.equals(Double.class))
         return true;
      return false;
   }

   private static interface CustomTransformer<T>
   {
      void transform(T object, RigidBodyTransform rigidBodyTransformToApply);
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/multisenseBlobExperiments/BlobDetectionPointCloudReceiver.java<|end_filename|>
package us.ihmc.atlas.multisenseBlobExperiments;

import java.util.ArrayList;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.ihmcPerception.depthData.PointCloudDataReceiverInterface;
import us.ihmc.ihmcPerception.depthData.PointCloudSource;

public class BlobDetectionPointCloudReceiver implements PointCloudDataReceiverInterface
{
   @Override
   public void receivedPointCloudData(ReferenceFrame scanFrame, ReferenceFrame lidarFrame, long[] timestamps, ArrayList<Point3D> points,
         PointCloudSource... sources)
   {
      System.out.println(getClass().getSimpleName() + " - receiving point cloud");
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/activationFunction/ActivationFunction.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction;

/**
 * The "transfer function" used to process the output of a neuron
 *
 */
public interface ActivationFunction
{
   public double computeOutput(double input);
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BlackFlyParameterPacket.java<|end_filename|>
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;

/**
       * Atlas specific message
       */
public class BlackFlyParameterPacket extends Packet<BlackFlyParameterPacket> implements Settable<BlackFlyParameterPacket>, EpsilonComparable<BlackFlyParameterPacket>
{
   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_;
   public boolean auto_exposure_;
   public boolean auto_gain_;
   public boolean auto_shutter_;
   public double exposure_;
   public double frame_rate_;
   public boolean from_ui_;
   public double gain_;
   public double shutter_;
   public byte robot_side_ = (byte) 255;

   public BlackFlyParameterPacket()
   {
   }

   public BlackFlyParameterPacket(BlackFlyParameterPacket other)
   {
      this();
      set(other);
   }

   public void set(BlackFlyParameterPacket other)
   {
      sequence_id_ = other.sequence_id_;

      auto_exposure_ = other.auto_exposure_;

      auto_gain_ = other.auto_gain_;

      auto_shutter_ = other.auto_shutter_;

      exposure_ = other.exposure_;

      frame_rate_ = other.frame_rate_;

      from_ui_ = other.from_ui_;

      gain_ = other.gain_;

      shutter_ = other.shutter_;

      robot_side_ = other.robot_side_;

   }

   /**
            * 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 void setAutoExposure(boolean auto_exposure)
   {
      auto_exposure_ = auto_exposure;
   }
   public boolean getAutoExposure()
   {
      return auto_exposure_;
   }

   public void setAutoGain(boolean auto_gain)
   {
      auto_gain_ = auto_gain;
   }
   public boolean getAutoGain()
   {
      return auto_gain_;
   }

   public void setAutoShutter(boolean auto_shutter)
   {
      auto_shutter_ = auto_shutter;
   }
   public boolean getAutoShutter()
   {
      return auto_shutter_;
   }

   public void setExposure(double exposure)
   {
      exposure_ = exposure;
   }
   public double getExposure()
   {
      return exposure_;
   }

   public void setFrameRate(double frame_rate)
   {
      frame_rate_ = frame_rate;
   }
   public double getFrameRate()
   {
      return frame_rate_;
   }

   public void setFromUi(boolean from_ui)
   {
      from_ui_ = from_ui;
   }
   public boolean getFromUi()
   {
      return from_ui_;
   }

   public void setGain(double gain)
   {
      gain_ = gain;
   }
   public double getGain()
   {
      return gain_;
   }

   public void setShutter(double shutter)
   {
      shutter_ = shutter;
   }
   public double getShutter()
   {
      return shutter_;
   }

   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   public byte getRobotSide()
   {
      return robot_side_;
   }


   public static Supplier<BlackFlyParameterPacketPubSubType> getPubSubType()
   {
      return BlackFlyParameterPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return BlackFlyParameterPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(BlackFlyParameterPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.auto_exposure_, other.auto_exposure_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.auto_gain_, other.auto_gain_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.auto_shutter_, other.auto_shutter_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.exposure_, other.exposure_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.frame_rate_, other.frame_rate_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.from_ui_, other.from_ui_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.gain_, other.gain_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.shutter_, other.shutter_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof BlackFlyParameterPacket)) return false;

      BlackFlyParameterPacket otherMyClass = (BlackFlyParameterPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.auto_exposure_ != otherMyClass.auto_exposure_) return false;

      if(this.auto_gain_ != otherMyClass.auto_gain_) return false;

      if(this.auto_shutter_ != otherMyClass.auto_shutter_) return false;

      if(this.exposure_ != otherMyClass.exposure_) return false;

      if(this.frame_rate_ != otherMyClass.frame_rate_) return false;

      if(this.from_ui_ != otherMyClass.from_ui_) return false;

      if(this.gain_ != otherMyClass.gain_) return false;

      if(this.shutter_ != otherMyClass.shutter_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("BlackFlyParameterPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("auto_exposure=");
      builder.append(this.auto_exposure_);      builder.append(", ");
      builder.append("auto_gain=");
      builder.append(this.auto_gain_);      builder.append(", ");
      builder.append("auto_shutter=");
      builder.append(this.auto_shutter_);      builder.append(", ");
      builder.append("exposure=");
      builder.append(this.exposure_);      builder.append(", ");
      builder.append("frame_rate=");
      builder.append(this.frame_rate_);      builder.append(", ");
      builder.append("from_ui=");
      builder.append(this.from_ui_);      builder.append(", ");
      builder.append("gain=");
      builder.append(this.gain_);      builder.append(", ");
      builder.append("shutter=");
      builder.append(this.shutter_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/agileHexapod/InverseFunction.java<|end_filename|>
package us.ihmc.exampleSimulations.agileHexapod;

import us.ihmc.yoVariables.variable.YoDouble;

public class InverseFunction
{
   private static YoDouble DET = new YoDouble("DET", null);


   /*
    * InverseA()
    * takes the generalized force on the body...
    * ... and calculates the Minimum Force Set
    * input: generalized forces(6v.)
    * & joints angles(18v.)
    * & pointers to the minimum forces(6v.)
    * output: minimum force set values(6v.)
    */

   public static void InverseA(double pos_x1, double pos_x2, double pos_x3, double pos_y1, double pos_y2, double pos_y3, double fX, double fY, double fZ,
                               double tX, double tY, double tZ, double h11, double h21, double k1, double h12, double h22, double k2, double h13, double h23,
                               double k3, YoDouble fx1, YoDouble fz1, YoDouble fx2, YoDouble fy2, YoDouble fz2, YoDouble fz3)

   {
      double s42, s44, s45, s46, s51, s52;
      double s53, s55, s56, s61, s63, s64;

      double t11, t12, t16;
      double t21, t22, t23, t24, t25, t26;
      double t31, t32, t36;
      double t42;
      double t51, t52, t53, t54, t55, t56;
      double t61, t62, t63, t64, t65, t66;

      double cd;

      s42 = (pos_y1 + AgileHexapodRobot.LEN * Math.cos(h21) * (Math.sin(h11 + k1) + Math.sin(h11)));
      s44 = (-2.0 * AgileHexapodRobot.LEN * Math.sin(h11) * Math.sin(k1) - AgileHexapodRobot.LEN * Math.sin(h12) * Math.sin(k2)
             - AgileHexapodRobot.LEN * Math.sin(h13) * Math.sin(k3) + AgileHexapodRobot.LEN * Math.cos(h12) * (Math.cos(k2) + 1)
             + AgileHexapodRobot.LEN * Math.cos(h13) * (Math.cos(k3) + 1) + 2.0 * AgileHexapodRobot.LEN * Math.cos(h11) * (Math.cos(k1) + 1));
      s45 = (pos_y2 + AgileHexapodRobot.LEN * Math.cos(h22) * (Math.sin(h12 + k2) + Math.sin(h12)));
      s46 = (pos_y3 + AgileHexapodRobot.LEN * Math.cos(h23) * (Math.sin(h13 + k3) + Math.sin(h13)));
      s51 = (-AgileHexapodRobot.LEN * Math.cos(h11 + k1) - AgileHexapodRobot.LEN * Math.cos(h11));
      s52 = (-pos_x1 + AgileHexapodRobot.LEN * Math.sin(h21) * (Math.sin(h11 + k1) + Math.sin(h11)));
      s53 = (AgileHexapodRobot.LEN * Math.cos(h12) * (-Math.cos(k2) - 1) + AgileHexapodRobot.LEN * Math.cos(h13) * (-Math.cos(k3) - 1)
             + AgileHexapodRobot.LEN * Math.sin(h12) * Math.sin(k2) + AgileHexapodRobot.LEN * Math.sin(h13) * Math.sin(k3));
      s55 = (-pos_x2 + AgileHexapodRobot.LEN * Math.sin(h22) * (Math.sin(h12) * (Math.cos(k2) + 1) + Math.cos(h12) * Math.sin(k2)));
      s56 = (-pos_x3 + AgileHexapodRobot.LEN * Math.sin(h23) * (Math.sin(h13 + k3) + Math.sin(h13)));
      s61 = (-pos_y1 - AgileHexapodRobot.LEN * Math.cos(h21) * (Math.sin(h11 + k1) + Math.sin(h11)));
      s63 = (-pos_y2 - pos_y3 - AgileHexapodRobot.LEN * Math.cos(h22) * (Math.sin(h12 + k2) + Math.sin(h12))
             - AgileHexapodRobot.LEN * Math.cos(h23) * (Math.sin(h13 + k3) + Math.sin(h13)));
      s64 = (2.0 * pos_x1 + pos_x2 + pos_x3 - 2.0 * AgileHexapodRobot.LEN * Math.sin(h21) * (Math.sin(h11 + k1) + Math.sin(h11))
             - AgileHexapodRobot.LEN * Math.sin(h22) * (Math.sin(h12 + k2) + Math.sin(h12))
             - AgileHexapodRobot.LEN * Math.sin(h23) * (Math.sin(h13 + k3) + Math.sin(h13)));

      DET.set((4.0 * s63 - 8.0 * s61) * (s42 * (s55 - s56) + s45 * (-s52 + s56) + s46 * (s52 - s55)));

      if ((DET.getDoubleValue() > -0.001) && (DET.getDoubleValue() < 0.0))
         DET.set(-0.001);
      if ((DET.getDoubleValue() < 0.001) && (DET.getDoubleValue() > 0.0))
         DET.set(0.001);

      cd = 4.0 * s63 - 8.0 * s61;

      t11 = (4.0 * s63 * DET.getDoubleValue()) / (cd);
      t12 = (2.0 * s64 * DET.getDoubleValue()) / (cd);
      t16 = (-8.0 * DET.getDoubleValue()) / (cd);

      t21 = 4.0 * (s53 * s61 - s51 * s63) * (s46 - s45);
      t22 = s51 * s64 * (-2.0 * s46 + 2.0 * s45) + s53 * s64 * (-s45 + s46) + s55 * s44 * (-s63 + 2.0 * s61) + s56 * s44 * (-2.0 * s61 + s63);
      t23 = cd * (s45 * s56 - s46 * s55);
      t24 = cd * (s55 - s56);
      t25 = cd * (-s45 + s46);
      t26 = (4.0 * s53 - 8.0 * s51) * (s45 - s46);

      t31 = (-4.0 * s61 * DET.getDoubleValue()) / (cd);
      t32 = (-s64 * DET.getDoubleValue()) / (cd);
      t36 = (4.0 * DET.getDoubleValue()) / (cd);

      t42 = DET.getDoubleValue() / 4.0;

      t51 = 4.0 * (s46 - s42) * (s51 * s63 - s61 * s53);
      t52 = (2.0 * s51 - s53) * (-s42 * s64 + s46 * s64) + (2.0 * s61 - s63) * (s56 * s44 - s52 * s44);
      t53 = cd * (-s42 * s56 + s46 * s52);
      t54 = cd * (s56 - s52);
      t55 = cd * (-s46 + s42);
      t56 = (4.0 * s53 - 8.0 * s51) * (s46 - s42);

      t61 = 4.0 * (s63 * s51 - s61 * s53) * (s42 - s45);
      t62 = (2.0 * s51 - s53) * (-s45 * s64 + s42 * s64) + (2.0 * s61 - s63) * (s52 * s44 - s55 * s44);
      t63 = cd * (s42 * s55 - s45 * s52);
      t64 = cd * (s52 - s55);
      t65 = cd * (s45 - s42);
      t66 = (8.0 * s51 - 4.0 * s53) * (s45 - s42);


      fx1.set((1.0 / DET.getDoubleValue()) * (t11 * fX + t12 * fY + t16 * tZ));
      fz1.set((1.0 / DET.getDoubleValue()) * (t21 * fX + t22 * fY + t23 * fZ + t24 * tX + t25 * tY + t26 * tZ));
      fx2.set((1.0 / DET.getDoubleValue()) * (t31 * fX + t32 * fY + t36 * tZ));
      fy2.set((1.0 / DET.getDoubleValue()) * (t42 * fY));
      fz2.set((1.0 / DET.getDoubleValue()) * (t51 * fX + t52 * fY + t53 * fZ + t54 * tX + t55 * tY + t56 * tZ));
      fz3.set((1.0 / DET.getDoubleValue()) * (t61 * fX + t62 * fY + t63 * fZ + t64 * tX + t65 * tY + t66 * tZ));


      /* test it */

      /*
       *   ls.sminv_err_1 = (*fx1) + 2.0* (*fx2) - fX;
       *   ls.sminv_err_2 = 4.0 * (*fy2) - fY;
       *   ls.sminv_err_3 = (*fz1) + (*fz2) + (*fz3) - fZ;
       *
       *   ls.sminv_err_4 = s42*(*fz1) + s44*(*fy2) + s45*(*fz2) + s46*(*fz3) - tX;
       *   ls.sminv_err_5 = s51*(*fx1) + s52*(*fz1) + s53*(*fx2) + s55*(*fz2) + s56*(*fz3) - tY;
       *   ls.sminv_err_6 = s61*(*fx1) + s63*(*fx2) + s64*(*fy2) - tZ;
       */

   }


}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/activationFunction/Sigmoid.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction;

public class Sigmoid implements ActivationFunction
{

   @Override
   public double computeOutput(double input)
   {
      
      return 1.0 / (1.0 + Math.exp(-input));
   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/ros/generators/RosCustomGenerator.java<|end_filename|>
package us.ihmc.communication.ros.generators;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */

public interface RosCustomGenerator
{
   String getRosTopic();

   String getRosPackage();

   RosFieldDefinition[] getFields();

   String getTypeDocumentation();

   String getMessageName();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasLowLevelControlModeMessage.java<|end_filename|>
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;

/**
       * Atlas specific message.
       */
public class AtlasLowLevelControlModeMessage extends Packet<AtlasLowLevelControlModeMessage> implements Settable<AtlasLowLevelControlModeMessage>, EpsilonComparable<AtlasLowLevelControlModeMessage>
{
   public static final byte ATLAS_LOW_LEVEL_CONTROL_MODE_STAND_PREP = (byte) 0;
   public static final byte ATLAS_LOW_LEVEL_CONTROL_MODE_FREEZE = (byte) 1;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte requested_atlas_low_level_control_mode_ = (byte) 255;

   public AtlasLowLevelControlModeMessage()
   {
   }

   public AtlasLowLevelControlModeMessage(AtlasLowLevelControlModeMessage other)
   {
      this();
      set(other);
   }

   public void set(AtlasLowLevelControlModeMessage other)
   {
      sequence_id_ = other.sequence_id_;

      requested_atlas_low_level_control_mode_ = other.requested_atlas_low_level_control_mode_;

   }

   /**
            * 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 void setRequestedAtlasLowLevelControlMode(byte requested_atlas_low_level_control_mode)
   {
      requested_atlas_low_level_control_mode_ = requested_atlas_low_level_control_mode;
   }
   public byte getRequestedAtlasLowLevelControlMode()
   {
      return requested_atlas_low_level_control_mode_;
   }


   public static Supplier<AtlasLowLevelControlModeMessagePubSubType> getPubSubType()
   {
      return AtlasLowLevelControlModeMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AtlasLowLevelControlModeMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AtlasLowLevelControlModeMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.requested_atlas_low_level_control_mode_, other.requested_atlas_low_level_control_mode_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AtlasLowLevelControlModeMessage)) return false;

      AtlasLowLevelControlModeMessage otherMyClass = (AtlasLowLevelControlModeMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.requested_atlas_low_level_control_mode_ != otherMyClass.requested_atlas_low_level_control_mode_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AtlasLowLevelControlModeMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("requested_atlas_low_level_control_mode=");
      builder.append(this.requested_atlas_low_level_control_mode_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/MomentumTrajectoryMessage.java<|end_filename|>
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 can be used to send a predefined angular momentum trajectory to the controller.
       * This trajectory will be used for ICP planning.
       */
public class MomentumTrajectoryMessage extends Packet<MomentumTrajectoryMessage> implements Settable<MomentumTrajectoryMessage>, EpsilonComparable<MomentumTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * List of angular momentum trajectory waypoints.
            * Each waypoint contains the angular momentum and the angular momentum rate at a given time.
            * NOTE: As the angular momentum can't be encoded using the SO3TrajectoryMessage, the EuclideanTrajectoryMessage is used.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage angular_momentum_trajectory_;

   public MomentumTrajectoryMessage()
   {
      angular_momentum_trajectory_ = new controller_msgs.msg.dds.EuclideanTrajectoryMessage();
   }

   public MomentumTrajectoryMessage(MomentumTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(MomentumTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.staticCopy(other.angular_momentum_trajectory_, angular_momentum_trajectory_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * List of angular momentum trajectory waypoints.
            * Each waypoint contains the angular momentum and the angular momentum rate at a given time.
            * NOTE: As the angular momentum can't be encoded using the SO3TrajectoryMessage, the EuclideanTrajectoryMessage is used.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage getAngularMomentumTrajectory()
   {
      return angular_momentum_trajectory_;
   }


   public static Supplier<MomentumTrajectoryMessagePubSubType> getPubSubType()
   {
      return MomentumTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return MomentumTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(MomentumTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.angular_momentum_trajectory_.epsilonEquals(other.angular_momentum_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof MomentumTrajectoryMessage)) return false;

      MomentumTrajectoryMessage otherMyClass = (MomentumTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.angular_momentum_trajectory_.equals(otherMyClass.angular_momentum_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("MomentumTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("angular_momentum_trajectory=");
      builder.append(this.angular_momentum_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/DispatchDoneListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;

public interface DispatchDoneListener
{
   public void dispatchDone(SimulationToDispatch dispatchSim, double[] finalState);
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WalkToGoalBehaviorPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "WalkToGoalBehaviorPacket" defined in "WalkToGoalBehaviorPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from WalkToGoalBehaviorPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit WalkToGoalBehaviorPacket_.idl instead.
*
*/
public class WalkToGoalBehaviorPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.WalkToGoalBehaviorPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::WalkToGoalBehaviorPacket_";

   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.WalkToGoalBehaviorPacket 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.WalkToGoalBehaviorPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 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;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WalkToGoalBehaviorPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WalkToGoalBehaviorPacket data, int current_alignment)
   {
      int initial_alignment = 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;
   }

   public static void write(controller_msgs.msg.dds.WalkToGoalBehaviorPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_9(data.getWalkToGoalAction());

      cdr.write_type_6(data.getXGoal());

      cdr.write_type_6(data.getYGoal());

      cdr.write_type_6(data.getThetaGoal());

      cdr.write_type_9(data.getGoalRobotSide());

   }

   public static void read(controller_msgs.msg.dds.WalkToGoalBehaviorPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setWalkToGoalAction(cdr.read_type_9());
      	
      data.setXGoal(cdr.read_type_6());
      	
      data.setYGoal(cdr.read_type_6());
      	
      data.setThetaGoal(cdr.read_type_6());
      	
      data.setGoalRobotSide(cdr.read_type_9());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.WalkToGoalBehaviorPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_9("walk_to_goal_action", data.getWalkToGoalAction());
      ser.write_type_6("x_goal", data.getXGoal());
      ser.write_type_6("y_goal", data.getYGoal());
      ser.write_type_6("theta_goal", data.getThetaGoal());
      ser.write_type_9("goal_robot_side", data.getGoalRobotSide());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.WalkToGoalBehaviorPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setWalkToGoalAction(ser.read_type_9("walk_to_goal_action"));
      data.setXGoal(ser.read_type_6("x_goal"));
      data.setYGoal(ser.read_type_6("y_goal"));
      data.setThetaGoal(ser.read_type_6("theta_goal"));
      data.setGoalRobotSide(ser.read_type_9("goal_robot_side"));
   }

   public static void staticCopy(controller_msgs.msg.dds.WalkToGoalBehaviorPacket src, controller_msgs.msg.dds.WalkToGoalBehaviorPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.WalkToGoalBehaviorPacket createData()
   {
      return new controller_msgs.msg.dds.WalkToGoalBehaviorPacket();
   }
   @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.WalkToGoalBehaviorPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.WalkToGoalBehaviorPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.WalkToGoalBehaviorPacket src, controller_msgs.msg.dds.WalkToGoalBehaviorPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public WalkToGoalBehaviorPacketPubSubType newInstance()
   {
      return new WalkToGoalBehaviorPacketPubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/quadTree/QuadTreeForGroundListener.java<|end_filename|>
package us.ihmc.robotics.quadTree;

import us.ihmc.euclid.tuple3D.Point3D32;

public interface QuadTreeForGroundListener
{
   public void nodeAdded(String id, Box bounds, float x, float y, float height);

   public void nodeRemoved(String id);

   public void RawPointAdded(float x, float y, float z);
   
   public void PopToOctree(Point3D32 location);
   public void PopToOctree(Point3D32 location, Point3D32 LidarHeadLocation);
   
   
//   public void metaDataUpdated(String id, OneDimensionalBounds[] bounds, float height);
//
//   public void treeCleared();

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/partNames/LeggedJointNameMap.java<|end_filename|>
package us.ihmc.robotics.partNames;

import org.apache.commons.lang3.tuple.ImmutablePair;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.robotics.robotSide.RobotSegment;

public interface LeggedJointNameMap<E extends Enum<E> & RobotSegment<E>> extends JointNameMap<E>
{
   ImmutablePair<E, LegJointName> getLegJointName(String jointName);

   ImmutablePair<E, LimbName> getLimbName(String limbName);

   String getJointBeforeFootName(E robotSegment);

   RigidBodyTransform getSoleToParentFrameTransform(E robotSegment);
}


<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/visibilityGraphs/tools/VisibilityGraphsGeometryTools.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.tools;

import us.ihmc.euclid.geometry.tools.EuclidGeometryTools;
import us.ihmc.euclid.tools.EuclidCoreTools;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;

/**
 * Sets of geometry tools to be moved to Euclid.
 */
public class VisibilityGraphsGeometryTools
{

   public static boolean doRay2DAndLineSegment2DIntersect(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DReadOnly lineSegmentStart,
                                                          Point2DReadOnly lineSegmentEnd)
   {
      return doRay2DAndLineSegment2DIntersect(rayOrigin.getX(), rayOrigin.getY(), rayDirection.getX(), rayDirection.getY(), lineSegmentStart.getX(),
                                              lineSegmentStart.getY(), lineSegmentEnd.getX(), lineSegmentEnd.getY());
   }

   public static boolean doRay2DAndLineSegment2DIntersect(double rayOriginX, double rayOriginY, double rayDirectionX, double rayDirectionY,
                                                          double lineSegmentStartX, double lineSegmentStartY, double lineSegmentEndX, double lineSegmentEndY)
   {
      return intersectionBetweenRay2DAndLineSegment2D(rayOriginX, rayOriginY, rayDirectionX, rayDirectionY, lineSegmentStartX, lineSegmentStartY,
                                                      lineSegmentEndX, lineSegmentEndY, null);
   }

   public static int intersectionBetweenRay2DAndCircle2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DReadOnly circleCenter,
                                                         double circleRadius, Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack)
   {
      return intersectionBetweenRay2DAndCircle2D(rayOrigin.getX(), rayOrigin.getY(), rayDirection.getX(), rayDirection.getY(), circleCenter.getX(),
                                                 circleCenter.getY(), circleRadius, firstIntersectionToPack, secondIntersectionToPack);
   }

   /**
    * Implementation inspired from:
    * {@link EuclidGeometryTools#intersectionBetweenRay3DAndCylinder3D(double, double, double, us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly, us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly, us.ihmc.euclid.tuple3D.interfaces.Point3DBasics, us.ihmc.euclid.tuple3D.interfaces.Point3DBasics)}
    * 
    * @param rayOriginX
    * @param rayOriginY
    * @param rayDirectionX
    * @param rayDirectionY
    * @param circleCenterX
    * @param circleCenterY
    * @param circleRadius
    * @param firstIntersectionToPack
    * @param secondIntersectionToPack
    * @return
    */
   public static int intersectionBetweenRay2DAndCircle2D(double rayOriginX, double rayOriginY, double rayDirectionX, double rayDirectionY, double circleCenterX,
                                                         double circleCenterY, double circleRadius, Point2DBasics firstIntersectionToPack,
                                                         Point2DBasics secondIntersectionToPack)
   {
      if (circleRadius < 0.0)
         throw new IllegalArgumentException("The circle radius has to be positive");

      double epsilon = 1.0e-12;

      if (firstIntersectionToPack != null)
         firstIntersectionToPack.setToNaN();
      if (secondIntersectionToPack != null)
         secondIntersectionToPack.setToNaN();

      double A = EuclidCoreTools.normSquared(rayDirectionX, rayDirectionY);

      if (A < epsilon)
         return 0;

      double startX = rayOriginX - circleCenterX;
      double startY = rayOriginY - circleCenterY;

      double B = 2.0 * (startX * rayDirectionX + startY * rayDirectionY);
      double C = EuclidCoreTools.normSquared(startX, startY) - circleRadius * circleRadius;
      double deltaSquared = B * B - 4 * A * C;

      if (deltaSquared < -epsilon)
         return 0;

      double oneOverTwoA = 0.5 / A;

      if (deltaSquared < epsilon)
      {
         double dCircle = -B / oneOverTwoA;

         if (dCircle < 0.0)
            return 0;

         if (firstIntersectionToPack != null)
         {
            firstIntersectionToPack.set(rayDirectionX, rayDirectionY);
            firstIntersectionToPack.scale(dCircle);
            firstIntersectionToPack.add(startX, startY);
         }
         return 1;
      }
      else
      {
         double delta = Math.sqrt(deltaSquared);
         double dCircle1 = (-B + delta) * oneOverTwoA;
         double dCircle2 = (-B - delta) * oneOverTwoA;

         if (dCircle2 < 0.0)
         {
            dCircle2 = Double.NaN;
         }

         if (dCircle1 < 0.0)
         {
            dCircle1 = dCircle2;
            dCircle2 = Double.NaN;
         }

         if (dCircle2 < dCircle1)
         {
            double temp = dCircle1;
            dCircle1 = dCircle2;
            dCircle2 = temp;
         }

         if (Double.isNaN(dCircle1))
            return 0;

         if (firstIntersectionToPack != null)
         {
            firstIntersectionToPack.set(rayDirectionX, rayDirectionY);
            firstIntersectionToPack.scale(dCircle1);
            firstIntersectionToPack.add(rayOriginX, rayOriginY);
         }

         if (Double.isNaN(dCircle2))
            return 1;

         if (secondIntersectionToPack != null)
         {
            secondIntersectionToPack.set(rayDirectionX, rayDirectionY);
            secondIntersectionToPack.scale(dCircle2);
            secondIntersectionToPack.add(rayOriginX, rayOriginY);
         }

         return 2;
      }
   }

   public static Point2D intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly rayOrigin, Vector2D rayDirection, Point2DReadOnly lineSegmentStart,
                                                                  Point2DReadOnly lineSegmentEnd)
   {
      Point2D intersection = new Point2D();
      boolean success = intersectionBetweenRay2DAndLineSegment2D(rayOrigin.getX(), rayOrigin.getY(), rayDirection.getX(), rayDirection.getY(),
                                                                 lineSegmentStart.getX(), lineSegmentStart.getY(), lineSegmentEnd.getX(), lineSegmentEnd.getY(),
                                                                 intersection);
      if (success)
         return intersection;
      else
         return null;
   }

   public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DReadOnly lineSegmentStart,
                                                                  Point2DReadOnly lineSegmentEnd, Point2DBasics intersectionToPack)
   {
      return intersectionBetweenRay2DAndLineSegment2D(rayOrigin.getX(), rayOrigin.getY(), rayDirection.getX(), rayDirection.getY(), lineSegmentStart.getX(),
                                                      lineSegmentStart.getY(), lineSegmentEnd.getX(), lineSegmentEnd.getY(), intersectionToPack);
   }

   public static boolean intersectionBetweenRay2DAndLineSegment2D(double rayOriginX, double rayOriginY, double rayDirectionX, double rayDirectionY,
                                                                  double lineSegmentStartX, double lineSegmentStartY, double lineSegmentEndX,
                                                                  double lineSegmentEndY, Point2DBasics intersectionToPack)
   {
      double epsilon = 1.0e-7;

      double lineSegmentDirectionX = lineSegmentEndX - lineSegmentStartX;
      double lineSegmentDirectionY = lineSegmentEndY - lineSegmentStartY;

      double determinant = -rayDirectionX * lineSegmentDirectionY + rayDirectionY * lineSegmentDirectionX;

      double dx = lineSegmentStartX - rayOriginX;
      double dy = lineSegmentStartY - rayOriginY;

      if (Math.abs(determinant) < epsilon)
      { // The ray and line segment are parallel
        // Check if they are collinear
         double cross = dx * rayDirectionY - dy * rayDirectionX;
         if (Math.abs(cross) < epsilon)
         {
            if (EuclidGeometryTools.isPoint2DInFrontOfRay2D(lineSegmentStartX, lineSegmentStartY, rayOriginX, rayOriginY, rayDirectionX, rayDirectionY))
            {
               if (intersectionToPack != null)
                  intersectionToPack.set(lineSegmentStartX, lineSegmentStartY);
               return true;
            }

            if (EuclidGeometryTools.isPoint2DInFrontOfRay2D(lineSegmentEndX, lineSegmentEndY, rayOriginX, rayOriginY, rayDirectionX, rayDirectionY))
            {
               if (intersectionToPack != null)
                  intersectionToPack.set(lineSegmentEndX, lineSegmentEndY);
               return true;
            }

            return false;
         }
         // The ray and line segment are parallel but are not collinear, they do not intersect
         else
         {
            return false;
         }
      }

      double oneOverDeterminant = 1.0 / determinant;
      double AInverse00 = -lineSegmentDirectionY;
      double AInverse01 = lineSegmentDirectionX;
      double AInverse10 = -rayDirectionY;
      double AInverse11 = rayDirectionX;

      double alpha = oneOverDeterminant * (AInverse00 * dx + AInverse01 * dy);
      double beta = oneOverDeterminant * (AInverse10 * dx + AInverse11 * dy);

      boolean areIntersecting = alpha > 0.0 - epsilon && 0.0 - epsilon < beta && beta < 1.0 + epsilon;

      if (areIntersecting && intersectionToPack != null)
      {
         intersectionToPack.setX(rayOriginX + alpha * rayDirectionX);
         intersectionToPack.setY(rayOriginY + alpha * rayDirectionY);
      }

      return areIntersecting;
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/VideoPacket.java<|end_filename|>
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;

public class VideoPacket extends Packet<VideoPacket> implements Settable<VideoPacket>, EpsilonComparable<VideoPacket>
{
   public static final byte VIDEO_SOURCE_MULTISENSE_LEFT_EYE = (byte) 0;
   public static final byte VIDEO_SOURCE_MULTISENSE_RIGHT_EYE = (byte) 1;
   public static final byte VIDEO_SOURCE_FISHEYE_LEFT = (byte) 2;
   public static final byte VIDEO_SOURCE_FISHEYE_RIGHT = (byte) 3;
   public static final byte VIDEO_SOURCE_CV_THRESHOLD = (byte) 4;
   public static final byte VIDEO_SOURCE_IMAGE_PROCESSING_BEHAVIOR = (byte) 5;
   public static final byte VIDEO_SOURCE_AWARE_FACE_TRACKER = (byte) 6;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte video_source_ = (byte) 255;
   public long timestamp_;
   public us.ihmc.idl.IDLSequence.Byte  data_;
   public us.ihmc.euclid.tuple3D.Point3D position_;
   public us.ihmc.euclid.tuple4D.Quaternion orientation_;
   public controller_msgs.msg.dds.IntrinsicParametersMessage intrinsic_parameters_;

   public VideoPacket()
   {
      data_ = new us.ihmc.idl.IDLSequence.Byte (262144, "type_9");

      position_ = new us.ihmc.euclid.tuple3D.Point3D();
      orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
      intrinsic_parameters_ = new controller_msgs.msg.dds.IntrinsicParametersMessage();
   }

   public VideoPacket(VideoPacket other)
   {
      this();
      set(other);
   }

   public void set(VideoPacket other)
   {
      sequence_id_ = other.sequence_id_;

      video_source_ = other.video_source_;

      timestamp_ = other.timestamp_;

      data_.set(other.data_);
      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.position_, position_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.orientation_, orientation_);
      controller_msgs.msg.dds.IntrinsicParametersMessagePubSubType.staticCopy(other.intrinsic_parameters_, intrinsic_parameters_);
   }

   /**
            * 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 void setVideoSource(byte video_source)
   {
      video_source_ = video_source;
   }
   public byte getVideoSource()
   {
      return video_source_;
   }

   public void setTimestamp(long timestamp)
   {
      timestamp_ = timestamp;
   }
   public long getTimestamp()
   {
      return timestamp_;
   }


   public us.ihmc.idl.IDLSequence.Byte  getData()
   {
      return data_;
   }


   public us.ihmc.euclid.tuple3D.Point3D getPosition()
   {
      return position_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getOrientation()
   {
      return orientation_;
   }


   public controller_msgs.msg.dds.IntrinsicParametersMessage getIntrinsicParameters()
   {
      return intrinsic_parameters_;
   }


   public static Supplier<VideoPacketPubSubType> getPubSubType()
   {
      return VideoPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return VideoPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(VideoPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.video_source_, other.video_source_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.timestamp_, other.timestamp_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.data_, other.data_, epsilon)) return false;

      if (!this.position_.epsilonEquals(other.position_, epsilon)) return false;
      if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false;
      if (!this.intrinsic_parameters_.epsilonEquals(other.intrinsic_parameters_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof VideoPacket)) return false;

      VideoPacket otherMyClass = (VideoPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.video_source_ != otherMyClass.video_source_) return false;

      if(this.timestamp_ != otherMyClass.timestamp_) return false;

      if (!this.data_.equals(otherMyClass.data_)) return false;
      if (!this.position_.equals(otherMyClass.position_)) return false;
      if (!this.orientation_.equals(otherMyClass.orientation_)) return false;
      if (!this.intrinsic_parameters_.equals(otherMyClass.intrinsic_parameters_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("VideoPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("video_source=");
      builder.append(this.video_source_);      builder.append(", ");
      builder.append("timestamp=");
      builder.append(this.timestamp_);      builder.append(", ");
      builder.append("data=");
      builder.append(this.data_);      builder.append(", ");
      builder.append("position=");
      builder.append(this.position_);      builder.append(", ");
      builder.append("orientation=");
      builder.append(this.orientation_);      builder.append(", ");
      builder.append("intrinsic_parameters=");
      builder.append(this.intrinsic_parameters_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/main/ParameterFileSavedListener.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.main;

import java.io.File;

public interface ParameterFileSavedListener
{
   public abstract void parameterFileSaved(File file);
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/IMUPacket.java<|end_filename|>
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;

/**
       * General purpose packet to carry IMU data. (need to be replace with the ROS IMUData)
       */
public class IMUPacket extends Packet<IMUPacket> implements Settable<IMUPacket>, EpsilonComparable<IMUPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.tuple4D.Quaternion orientation_;
   public us.ihmc.euclid.tuple3D.Vector3D angular_velocity_;
   public us.ihmc.euclid.tuple3D.Vector3D linear_acceleration_;
   public double time_;

   public IMUPacket()
   {
      orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
      angular_velocity_ = new us.ihmc.euclid.tuple3D.Vector3D();
      linear_acceleration_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public IMUPacket(IMUPacket other)
   {
      this();
      set(other);
   }

   public void set(IMUPacket other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.orientation_, orientation_);
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.angular_velocity_, angular_velocity_);
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.linear_acceleration_, linear_acceleration_);
      time_ = other.time_;

   }

   /**
            * 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 us.ihmc.euclid.tuple4D.Quaternion getOrientation()
   {
      return orientation_;
   }


   public us.ihmc.euclid.tuple3D.Vector3D getAngularVelocity()
   {
      return angular_velocity_;
   }


   public us.ihmc.euclid.tuple3D.Vector3D getLinearAcceleration()
   {
      return linear_acceleration_;
   }

   public void setTime(double time)
   {
      time_ = time;
   }
   public double getTime()
   {
      return time_;
   }


   public static Supplier<IMUPacketPubSubType> getPubSubType()
   {
      return IMUPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return IMUPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(IMUPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false;
      if (!this.angular_velocity_.epsilonEquals(other.angular_velocity_, epsilon)) return false;
      if (!this.linear_acceleration_.epsilonEquals(other.linear_acceleration_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof IMUPacket)) return false;

      IMUPacket otherMyClass = (IMUPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.orientation_.equals(otherMyClass.orientation_)) return false;
      if (!this.angular_velocity_.equals(otherMyClass.angular_velocity_)) return false;
      if (!this.linear_acceleration_.equals(otherMyClass.linear_acceleration_)) return false;
      if(this.time_ != otherMyClass.time_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("IMUPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("orientation=");
      builder.append(this.orientation_);      builder.append(", ");
      builder.append("angular_velocity=");
      builder.append(this.angular_velocity_);      builder.append(", ");
      builder.append("linear_acceleration=");
      builder.append(this.linear_acceleration_);      builder.append(", ");
      builder.append("time=");
      builder.append(this.time_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/mapping/XBoxOneMapping.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick.mapping;

import java.util.ArrayList;
import java.util.List;

import org.apache.commons.lang3.SystemUtils;

import com.google.common.collect.BiMap;
import com.google.common.collect.HashBiMap;

import net.java.games.input.Component;
import net.java.games.input.Component.Identifier;
import net.java.games.input.Event;
import us.ihmc.tools.inputDevices.joystick.JoystickCompatibilityFilter;

/**
 * OS X Mappings based on the following driver: https://github.com/360Controller/360Controller
 * using the default bindings/mappings
 */
public enum XBoxOneMapping implements JoystickMapping
{
   A,
   B,
   X,
   Y,
   LEFT_BUMPER,
   RIGHT_BUMPER,
   SELECT,
   START,
   XBOX_BUTTON,
   LEFT_TRIGGER,
   RIGHT_TRIGGER,
   LEFT_STICK_X,
   LEFT_STICK_Y,
   RIGHT_STICK_X,
   RIGHT_STICK_Y,
   LEFT_STICK_BUTTON,
   RIGHT_STICK_BUTTON,
   /** @deprecated Only works on Windows and Linux */
   DPAD,
   /** @deprecated Only works on Mac */
   DPAD_UP,
   /** @deprecated Only works on Mac */
   DPAD_DOWN,
   /** @deprecated Only works on Mac */
   DPAD_LEFT,
   /** @deprecated Only works on Mac */
   DPAD_RIGHT;

   public static final XBoxOneMapping[] values = values();

   private static final BiMap<Identifier, XBoxOneMapping> windowsBiMap = HashBiMap.create(values.length);
   private static final BiMap<Identifier, XBoxOneMapping> macBiMap = HashBiMap.create(values.length);
   private static final BiMap<Identifier, XBoxOneMapping> linuxBiMap = HashBiMap.create(values.length);

   static
   {
      mapValues(A, Identifier.Button._0, Identifier.Button._0, Identifier.Button.A);
      mapValues(B, Identifier.Button._1, Identifier.Button._1, Identifier.Button.B);
      mapValues(X, Identifier.Button._2, Identifier.Button._2, Identifier.Button.X);
      mapValues(Y, Identifier.Button._3, Identifier.Button._3, Identifier.Button.Y);
      mapValues(LEFT_BUMPER, Identifier.Button._4, Identifier.Button._4, Identifier.Button.LEFT_THUMB);
      mapValues(RIGHT_BUMPER, Identifier.Button._5, Identifier.Button._5, Identifier.Button.RIGHT_THUMB);
      mapValues(SELECT, Identifier.Button._6, Identifier.Button._9, Identifier.Button.SELECT);
      mapValues(START, Identifier.Button._7, Identifier.Button._8, Identifier.Button.START);
      mapValues(XBOX_BUTTON, Identifier.Button._10, Identifier.Button._10, Identifier.Button.MODE);
      mapValues(LEFT_TRIGGER, Identifier.Axis.Z, Identifier.Axis.Z, Identifier.Axis.Z);
      mapValues(RIGHT_TRIGGER, Identifier.Axis.RZ, Identifier.Axis.RZ, Identifier.Axis.RZ);
      mapValues(LEFT_STICK_X, Identifier.Axis.X, Identifier.Axis.X, Identifier.Axis.X);
      mapValues(LEFT_STICK_Y, Identifier.Axis.Y, Identifier.Axis.Y, Identifier.Axis.Y);
      mapValues(RIGHT_STICK_X, Identifier.Axis.RX, Identifier.Axis.RX, Identifier.Axis.RX);
      mapValues(RIGHT_STICK_Y, Identifier.Axis.RY, Identifier.Axis.RY, Identifier.Axis.RY);
      mapValues(LEFT_STICK_BUTTON, Identifier.Button._8, Identifier.Button._6, Identifier.Button.LEFT_THUMB3);
      mapValues(RIGHT_STICK_BUTTON, Identifier.Button._9, Identifier.Button._7, Identifier.Button.RIGHT_THUMB3);
      mapValues(DPAD, Identifier.Axis.POV, Identifier.Button.UNKNOWN, Identifier.Axis.POV);
      mapValues(DPAD_UP, Identifier.Button.UNKNOWN, Identifier.Button._11, Identifier.Button.UNKNOWN);
      mapValues(DPAD_DOWN, Identifier.Button.UNKNOWN, Identifier.Button._12, Identifier.Button.UNKNOWN);
      mapValues(DPAD_LEFT, Identifier.Button.UNKNOWN, Identifier.Button._13, Identifier.Button.UNKNOWN);
      mapValues(DPAD_RIGHT, Identifier.Button.UNKNOWN, Identifier.Button._14, Identifier.Button.UNKNOWN);
   }

   private static void mapValues(XBoxOneMapping mapping, Identifier windowsIdentifier, Identifier macIdentifier, Identifier linuxIdentifier)
   {
      windowsBiMap.put(windowsIdentifier, mapping);
      macBiMap.put(macIdentifier, mapping);
      linuxBiMap.put(linuxIdentifier, mapping);
   }

   public static List<JoystickCompatibilityFilter> getCompatibilityFilters()
   {
      ArrayList<JoystickCompatibilityFilter> compatibilityFilters = new ArrayList<>();
      
      if (SystemUtils.IS_OS_LINUX)
      {
         compatibilityFilters.add(new JoystickCompatibilityFilter(LEFT_TRIGGER, false, 1.0, 0.5));
         compatibilityFilters.add(new JoystickCompatibilityFilter(RIGHT_TRIGGER, false, 1.0, 0.5));
      }
      
      return compatibilityFilters;
   }

   @Override
   public Identifier getIdentifier()
   {
      if (SystemUtils.IS_OS_WINDOWS)
      {
         return windowsBiMap.inverse().get(this);
      }
      else if (SystemUtils.IS_OS_MAC)
      {
         return macBiMap.inverse().get(this);
      }
      else if (SystemUtils.IS_OS_LINUX)
      {
         return linuxBiMap.inverse().get(this);
      }
      else
      {
         return null;
      }
   }
   
   public static XBoxOneMapping getMapping(Identifier identifier)
   {
      if (SystemUtils.IS_OS_WINDOWS)
      {
         return windowsBiMap.get(identifier);
      }
      else if (SystemUtils.IS_OS_MAC)
      {
         return macBiMap.get(identifier);
      }
      else if (SystemUtils.IS_OS_LINUX)
      {
         return linuxBiMap.get(identifier);
      }
      else
      {
         return null;
      }
   }
   
   private static XBoxOneMapping getMapping(Component component)
   {
      return getMapping(component.getIdentifier());
   }

   public static XBoxOneMapping getMapping(Event event)
   {
      return getMapping(event.getComponent());
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/StateMachinesJPanel.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Font;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Line2D;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;
import java.util.Map;

import javax.swing.JPanel;

import org.jgraph.JGraph;
import org.jgraph.graph.DefaultCellViewFactory;
import org.jgraph.graph.DefaultEdge;
import org.jgraph.graph.DefaultGraphCell;
import org.jgraph.graph.DefaultGraphModel;
import org.jgraph.graph.GraphConstants;
import org.jgraph.graph.GraphLayoutCache;

import com.jgraph.layout.JGraphFacade;
import com.jgraph.layout.tree.JGraphTreeLayout;

import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;

@Deprecated
public class StateMachinesJPanel<E extends Enum<E>> extends JPanel implements StateChangedListener<E>
{
   private static final long serialVersionUID = 2453853798153829891L;
   private final StateMachine<E> stateMachine;
   private boolean oldStateDiagram;
   private DefaultGraphCell[] stateCells;
   private JGraph graph;
   private int numberOfStates;
   private Enum<?> currentState = null;


   public StateMachinesJPanel(StateMachine<E> stateMachine)
   {
      this(stateMachine, false);

//    System.out.println("state machine window created");
   }

   public StateMachinesJPanel(final StateMachine<E> stateMachine, boolean OldStateDiagram)
   {
      this.stateMachine = stateMachine;
      this.numberOfStates = stateMachine.states.size();
      this.oldStateDiagram = OldStateDiagram;
      this.currentState = stateMachine.getStateYoVariable().getEnumValue();

      if (!OldStateDiagram)
      {
         graph = new JGraph();
         initializeJPanel();
      }


      Thread repaintWhenStateChange = new Thread(new Runnable()
      {
         public void run()
         {
            while (true)
            {
               // System.out.println("painting");
               try
               {
                  Thread.sleep(100);
               }
               catch (InterruptedException e)
               {
                  e.printStackTrace();
               }

               if (!currentState.name().equals(stateMachine.getStateYoVariable().getEnumValue().name()))
               {
                  currentState = stateMachine.getStateYoVariable().getEnumValue();
                  updateStateMachine();
               }
            }
         }
      }, "IHMC-StateMachinesPanel");
      repaintWhenStateChange.start();

   }

   public void updateStateMachine()
   {
      State<E> currentState = stateMachine.getCurrentState();
      if (oldStateDiagram)
      {
         this.repaint();
      }
      else
      {
         int currentIndex = indexOfStateInStateCells(currentState);
         GraphConstants.setBackground(graph.getAttributes(stateCells[currentIndex]), Color.RED);
         stateCells[currentIndex].setAttributes(graph.getAttributes(stateCells[currentIndex]));

         for (int i = 0; i < stateCells.length; i++)
         {
            if (i != currentIndex)
            {
               GraphConstants.setBackground(graph.getAttributes(stateCells[i]), Color.CYAN);
               stateCells[i].setAttributes(graph.getAttributes(stateCells[i]));
            }
         }

         graph.repaint();
         graph.refresh();

      }
   }

   public void stateChanged(State<E> oldState, State<E> newState, double time)
   {
      if (oldStateDiagram)
      {
         this.repaint();
      }
      else
      {
         int newIndex = indexOfStateInStateCells(newState);
         int oldIndex = indexOfStateInStateCells(oldState);

         GraphConstants.setBackground(graph.getAttributes(stateCells[oldIndex]), Color.CYAN);
         GraphConstants.setBackground(graph.getAttributes(stateCells[newIndex]), Color.RED);

         stateCells[oldIndex].setAttributes(graph.getAttributes(stateCells[oldIndex]));
         stateCells[newIndex].setAttributes(graph.getAttributes(stateCells[newIndex]));

         graph.repaint();
         this.repaint();
         graph.refresh();
      }
   }

   private int[] sourceArrows = new int[100];
   private int[] targetArrows = new int[100];
   private int sourceArrowIndex = 0;
   private int targetArrowIndex = 0;

   private void createArrow(int source, int target)
   {
      if (!arrowAlreadyExist(source, target))
      {
         DefaultGraphCell[] arrowCell = new DefaultGraphCell[1];
         DefaultEdge edge = new DefaultEdge();
         edge.setSource(stateCells[source].getChildAt(0));
         edge.setTarget(stateCells[target].getChildAt(0));
         arrowCell[0] = edge;
         int arrow = GraphConstants.ARROW_TECHNICAL;
         GraphConstants.setLineEnd(edge.getAttributes(), arrow);
         GraphConstants.setEndFill(edge.getAttributes(), true);
         graph.getGraphLayoutCache().insert(arrowCell);

         sourceArrows[sourceArrowIndex] = source;
         sourceArrowIndex++;
         targetArrows[targetArrowIndex] = target;
         targetArrowIndex++;
      }
   }


   private boolean arrowAlreadyExist(int source, int target)
   {
      for (int i = 0; i < sourceArrows.length; i++)
      {
         if ((sourceArrows[i] == source) && (targetArrows[i] == target))
         {
            return true;
         }
      }

      return false;
   }

   private DefaultGraphCell createCell(String name, Point2D placement, int index)
   {
//      System.out.println(name + " " + placement + " " + index);
      stateCells[index] = new DefaultGraphCell(new String(name));
      Font f = new Font("Arial", Font.PLAIN, 12);
      GraphConstants.setFont(stateCells[index].getAttributes(), f);
      GraphConstants.setAutoSize(stateCells[index].getAttributes(), true);
      GraphConstants.setOpaque(stateCells[index].getAttributes(), true);

      Color color = colorStateCell(stateMachine.states.get(indexOfStateinStateMachine(name)));
      GraphConstants.setBackground(stateCells[index].getAttributes(), color);
      GraphConstants.setBounds(stateCells[index].getAttributes(), new Rectangle2D.Double(placement.getX(), placement.getY(), 0, 0));
      stateCells[index].addPort();

      return stateCells[index];
   }

   private int indexOfStateinStateMachine(String state)
   {
      for (int i = 0; i < stateMachine.states.size(); i++)
      {
         if (stateMachine.states.get(i).getStateEnum().toString().equals(state))
         {
            return i;
         }
      }

      return -1;
   }

   private int indexOfStateInStateCells(State<E> state)
   {
      for (int i = 0; i < stateCells.length; i++)
      {
         if (stateCells[i].toString().equals(state.getStateEnum().toString()))
         {
            return i;
         }
      }

      return -1;
   }

   private State<E> getParentState()
   {
      State<E> parentState = null;
      int max = 0;
      boolean needParentForDiagram = false;
      for (int i = 0; i < numberOfStates; i++)
      {
         State<E> state = stateMachine.states.get(i);
         if (state.getStateTransitions().size() == max)
         {
            needParentForDiagram = true;
         }

         if (state.getStateTransitions().size() > max)
         {
            max = state.getStateTransitions().size();
            needParentForDiagram = false;
            parentState = state;
         }
      }

      if (needParentForDiagram)
      {
         parentState = stateMachine.states.get(0);
      }

      return parentState;
   }

   private boolean isStateEmpty(State<E> checkstate)
   {
      boolean isEmpty = true;

      // check each state
      for (int i = 0; i < stateMachine.states.size(); i++)
      {
         State<E> state = stateMachine.states.get(i);

         // if it has a default state
         StateTransition<E> defaultNextStateTransition = state.getDefaultNextStateTransition();
         if (defaultNextStateTransition != null)
         {
            // and that default state if the one we are checking, then the one we are checking is not empty.
            if ((defaultNextStateTransition.getNextStateEnum() == checkstate.getStateEnum()))
            {
               isEmpty = false;
            }
         }

         // if this state has other state transitions
         for (int j = 0; j < state.getStateTransitions().size(); j++)
         {
            // and one of those state transitions are the state we are checking,
            // then the state we are checking is not empty
            if (state.getStateTransitions().get(j).getNextStateEnum() == checkstate.getStateEnum())
            {
               isEmpty = false;
            }

         }
      }

      // is empty if there are no states going to the check state.
      return isEmpty;
   }

   private boolean doesCellAlreadyExist(String state)
   {
      for (int i = 0; i < stateCells.length; i++)
      {
         if (stateCells[i] != null)
         {
            if (stateCells[i].toString().equals(state))
            {
               return true;
            }
         }
      }

      return false;
   }

   private int stateCellIndex = 0;

   private void createStateMachineDiagram(State<E> parent)
   {
      // if (!isStateEmpty(parent))
      {
         for (int i = 0; i < parent.getStateTransitions().size(); i++)
         {
            String state = parent.getStateTransitions().get(i).getNextStateEnum().toString();

            if (!doesCellAlreadyExist(state))
            {
               stateCells[stateCellIndex] = createCell(state, new Point2D(0, 0), stateCellIndex);
               createArrow(indexOfStateInStateCells(parent), stateCellIndex);
               stateCellIndex++;

               if (stateMachine.states.get(indexOfStateinStateMachine(state)).getStateTransitions().size() != 0)
               {
                  createStateMachineDiagram(stateMachine.states.get(indexOfStateinStateMachine(state)));
               }
               else
               {
                  try
                  {
                     for (int j = indexOfStateinStateMachine(state) + 1;
                             j < indexOfStateinStateMachine(parent.getStateTransitions().get(i + 1).getNextStateEnum().toString()); j++)
                     {
                        State<E> checkState = stateMachine.states.get(j);

                        if (!doesCellAlreadyExist(checkState.getStateEnum().toString()))
                        {
                           stateCells[stateCellIndex] = createCell(checkState.getStateEnum().toString(), new Point2D(0, 0), stateCellIndex);
                           createArrow(stateCellIndex - 1, stateCellIndex);
                           stateCellIndex++;
                        }
                     }
                  }
                  catch (Exception noAdditionalStateTransitions)
                  {
                     for (int j = indexOfStateinStateMachine(state) + 1; j < stateCells.length; j++)
                     {
                        State<E> checkState = stateMachine.states.get(j);

                        if (!doesCellAlreadyExist(checkState.getStateEnum().toString()))
                        {
                           stateCells[stateCellIndex] = createCell(checkState.getStateEnum().toString(), new Point2D(0, 0), stateCellIndex);
                           createArrow(stateCellIndex - 1, stateCellIndex);
                           stateCellIndex++;
                        }
                     }
                  }
               }
            }
         }
      }
   }

   private void createArrowsForStateTransitions()
   {
      for (int i = 0; i < stateCells.length; i++)
      {
         State<E> state = stateMachine.states.get(indexOfStateinStateMachine(stateCells[i].toString()));

         for (int z = 0; z < state.getStateTransitions().size(); z++)
         {
            int source = indexOfStateInStateCells(state);
            int target =
               indexOfStateInStateCells(stateMachine.states.get(indexOfStateinStateMachine(state.getStateTransitions().get(z).getNextStateEnum().toString())));
            createArrow(source, target);
         }
      }
   }

   private void createDefaultArrows()
   {
      for (int i = 0; i < stateMachine.states.size(); i++)
      {
         State<E> state = stateMachine.states.get(i);
         StateTransition<E> defaultNextStateTransition = state.getDefaultNextStateTransition();
         if (defaultNextStateTransition != null)
         {
            int source = indexOfStateInStateCells(state);
            int target = indexOfStateInStateCells(
                             stateMachine.states.get(
                                indexOfStateinStateMachine(defaultNextStateTransition.getNextStateEnum().toString())));
            createArrow(source, target);
         }
      }
   }

   private Color colorStateCell(State<E> state)
   {
      State<E> currentState = stateMachine.getCurrentState();

      if (state == currentState)
      {
         return Color.RED;
      }
      else
      {
         return Color.CYAN;
      }
   }

   private void initializeJPanel()
   {
      initiate = true;
      graph.setGraphLayoutCache(new GraphLayoutCache(new DefaultGraphModel(), new DefaultCellViewFactory(), true));
      graph.setEditable(false);

      for (int i = 0; i < numberOfStates; i++)
      {
         isStateEmpty(stateMachine.states.get(i));
      }

      stateCells = new DefaultGraphCell[numberOfStates];


      State<E> parent = getParentState();

      stateCells[stateCellIndex] = createCell(parent.getStateEnum().toString(), new Point2D(0, 0), stateCellIndex);
      stateCellIndex++;

      createStateMachineDiagram(parent);

      graph.getGraphLayoutCache().insert(stateCells);



      JGraphFacade facade = new JGraphFacade(graph.getGraphLayoutCache());
      facade.setEdgePromotion(false);
      JGraphTreeLayout layout = new JGraphTreeLayout();
      layout.run(facade);
      Map<?, ?> map = facade.createNestedMap(true, true);
      graph.getGraphLayoutCache().edit(map);
      createDefaultArrows();
      createArrowsForStateTransitions();
   }

   private Point2D[] stateCenters;
   private Vector2D tempVector2d = new Vector2D();

   private Dimension dimension = new Dimension();
   final static BasicStroke wideStroke = new BasicStroke(3.0f);

   private void OldStateMachineDiagram(Graphics g, double width, double height)
   {
      if (stateCenters == null)
      {
         stateCenters = new Point2D[numberOfStates];

         for (int i = 0; i < numberOfStates; i++)
         {
            stateCenters[i] = new Point2D();
         }
      }

      Graphics2D g2 = (Graphics2D) g;
      g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);

      g2.setStroke(wideStroke);

      // Determine the Ellipse Radii and Circumference:
      double Ry = 0.25 * height;
      double Rx = 0.25 * width;
      double circumference = approximateElipseCircumference(Rx, Ry);

      // Determine the Circle Radius:
      int circleRadiusBasedOnSpacing = ((int) (circumference / (2.5 * numberOfStates)));
      int circleRadiusBasedOnPanelSize = ((int) (0.9 * Math.min(Rx, Ry)));
      int circleRadius = Math.min(circleRadiusBasedOnSpacing, circleRadiusBasedOnPanelSize);

      // Draw a circle for each state:
      State<E> currentState = stateMachine.getCurrentState();

      for (int i = 0; i < numberOfStates; i++)
      {
         State<E> state = stateMachine.states.get(i);
         if (state == currentState)
            g.setColor(Color.RED);
         else
            g2.setColor(Color.BLACK);

         double angle = (2.0 * Math.PI * i) / (numberOfStates);
         stateCenters[i].setX(Rx * Math.cos(angle) + width / 2);
         stateCenters[i].setY(Ry * Math.sin(angle) + height / 2);

         // g2.drawOval(stateCentersX[i] - circleRadius, stateCentersY[i] - circleRadius, circleRadius*2, circleRadius*2);
         g2.draw(new Ellipse2D.Double(stateCenters[i].getX() - circleRadius, stateCenters[i].getY() - circleRadius, circleRadius * 2, circleRadius * 2));
         String stateString = state.getStateEnum().toString();

         g2.drawString(state.getStateEnum().toString(), (int) stateCenters[i].getX() - 8 * stateString.length() / 2, (int) stateCenters[i].getY());
      }

      g2.setColor(Color.BLACK);

      // Draw a line for each transition condition:
      for (int i = 0; i < numberOfStates; i++)
      {
         State<E> state = stateMachine.states.get(i);

         ArrayList<StateTransition<E>> stateTransitions = state.getStateTransitions();

         for (StateTransition<E> stateTransition : stateTransitions)
         {
            State<E> nextState = stateMachine.getState(stateTransition.getNextStateEnum());

            if (nextState == null)
            {
               System.err.println("Error. StateMachine doesn't include state with enum " + stateTransition.getNextStateEnum());

               return;
            }

            int nextIndex = stateMachine.states.indexOf(nextState);

            if (nextIndex >= 0)
            {
               tempVector2d.sub(stateCenters[nextIndex], stateCenters[i]);
               tempVector2d.normalize();
               tempVector2d.scale(circleRadius);

               g2.draw(new Line2D.Double(stateCenters[i].getX() + tempVector2d.getX(), stateCenters[i].getY() + tempVector2d.getY(), stateCenters[nextIndex].getX() - tempVector2d.getX(),
                                         stateCenters[nextIndex].getY() - tempVector2d.getY()));
            }
            else
            {
               System.err.println("Error. StateMachine doesn't include state " + nextState);
            }
         }
      }
   }

   private boolean initiate;
   private double tempHeight = 0;
   private double tempWidth = 0;

   private void reScalingStateMachine(double width, double height)
   {
      if (initiate)
      {
         tempWidth = width;
         tempHeight = height;
         initiate = false;
      }

      if (tempWidth > width)
      {
         graph.setScale(graph.getScale() + (width - tempWidth) / 3000);
         tempWidth = width;
      }

      if (tempWidth < width)
      {
         graph.setScale(graph.getScale() + (width - tempWidth) / 3000);
         tempWidth = width;
      }

      if (tempHeight > height)
      {
         graph.setScale(graph.getScale() - (tempHeight - height) / 3000);
         tempHeight = height;
      }

      if (tempHeight < height)
      {
         graph.setScale(graph.getScale() + (height - tempHeight) / 3000);
         tempHeight = height;
      }
   }

   public void paintComponent(Graphics g)
   {
      super.paintComponent(g);
      this.getSize(dimension);
      double width = dimension.getWidth();
      double height = dimension.getHeight();

      if (oldStateDiagram)
      {
         OldStateMachineDiagram(g, width, height);
      }
      else
      {
         graph.setBounds(0, 0, (int) width, (int) height);
         graph.setAlignmentX(CENTER_ALIGNMENT);
         reScalingStateMachine(width, height);
         this.add(graph);
      }
   }

   private static double approximateElipseCircumference(double a, double b)
   {
      double x = (a - b) / (a + b);

      return Math.PI * (a + b) * (1.0 + 3.0 * x * x / (10.0 + Math.sqrt(4.0 - 3.0 * x * x)));
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/msgToPacket/converter/CustomFieldConversions.java<|end_filename|>
package us.ihmc.utilities.ros.msgToPacket.converter;

import org.ros.internal.message.Message;

import java.util.HashMap;
import java.util.Map;
import java.util.function.Function;
import java.util.stream.Stream;

public class CustomFieldConversions
{
   private static CustomFieldConversions instance = null;

   private final Map<Class, Function> classToPacketFieldConveterMap = new HashMap<>();
   private final Map<Class<? extends Message>, Function> classToMessageFieldConverterMap = new HashMap<>();

   private CustomFieldConversions()
   {
      // singleton constructor
   }

   public static CustomFieldConversions getInstance()
   {
      if(instance == null) instance = new CustomFieldConversions();

      return instance;
   }

   public <T, S extends Message> void registerIHMCPacketFieldConverter(Class<T> clazz, Function<T, S> converter)
   {
      classToPacketFieldConveterMap.put(clazz, converter);
   }

   public <T, S extends Message> void registerROSMessageFieldConverter(Class<S> clazz, Function<S, T> converter)
   {
      classToMessageFieldConverterMap.put(clazz, converter);
   }

   public <T, S extends Message> S convert(T field)
   {
      Function<T, S> function = classToPacketFieldConveterMap.get(field.getClass());
      return function.apply(field);
   }

   public <T, S extends Message> T convert(S field)
   {
      Function<S, T> function = classToMessageFieldConverterMap.get(field.getClass().getInterfaces()[0]);
      return function.apply(field);
   }

   public boolean containsConverterFor(Class<?> clazz)
   {
      boolean ret = false;

      for (Class aClass : classToPacketFieldConveterMap.keySet())
      {
         ret |= aClass.isAssignableFrom(clazz);
      }

      for (Class<? extends Message> aClass : classToMessageFieldConverterMap.keySet())
      {
         ret |= aClass.isAssignableFrom(clazz);
      }

      return ret;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/geometry/VectorMean.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.geometry;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;

public class VectorMean extends Vector3D
{
   private int sampleSize = 0;

   public VectorMean()
   {
   }

   public void update(Tuple3DBasics tuple)
   {
      update(tuple.getX(), tuple.getY(), tuple.getZ());
   }

   public void update(Tuple3DBasics tuple, int updateSize)
   {
      update(tuple.getX(), tuple.getY(), tuple.getZ(), updateSize);
   }

   public void update(double x, double y, double z)
   {
      update(x, y, z, 1);
   }

   public void update(double x, double y, double z, int updateSize)
   {
      sampleSize += updateSize;
      double nInv = (double) updateSize / (double) sampleSize;
      this.addX((x - this.getX()) * nInv);
      this.addY((y - this.getY()) * nInv);
      this.addZ((z - this.getZ()) * nInv);
   }

   public void clear()
   {
      sampleSize = 0;
      set(0.0, 0.0, 0.0);
   }

   public int getNumberOfSamples()
   {
      return sampleSize;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ReachingManifoldMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "ReachingManifoldMessage" defined in "ReachingManifoldMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from ReachingManifoldMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit ReachingManifoldMessage_.idl instead.
*
*/
public class ReachingManifoldMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.ReachingManifoldMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::ReachingManifoldMessage_";

   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.ReachingManifoldMessage 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.ReachingManifoldMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += geometry_msgs.msg.dds.QuaternionPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ReachingManifoldMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ReachingManifoldMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getManifoldOriginPosition(), current_alignment);

      current_alignment += geometry_msgs.msg.dds.QuaternionPubSubType.getCdrSerializedSize(data.getManifoldOriginOrientation(), current_alignment);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getManifoldConfigurationSpaceNames().size() * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getManifoldLowerLimits().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getManifoldUpperLimits().size() * 8) + us.ihmc.idl.CDR.alignment(current_alignment, 8);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.ReachingManifoldMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_2(data.getEndEffectorHashCode());

      geometry_msgs.msg.dds.PointPubSubType.write(data.getManifoldOriginPosition(), cdr);
      geometry_msgs.msg.dds.QuaternionPubSubType.write(data.getManifoldOriginOrientation(), cdr);
      if(data.getManifoldConfigurationSpaceNames().size() <= 100)
      cdr.write_type_e(data.getManifoldConfigurationSpaceNames());else
          throw new RuntimeException("manifold_configuration_space_names field exceeds the maximum length");

      if(data.getManifoldLowerLimits().size() <= 100)
      cdr.write_type_e(data.getManifoldLowerLimits());else
          throw new RuntimeException("manifold_lower_limits field exceeds the maximum length");

      if(data.getManifoldUpperLimits().size() <= 100)
      cdr.write_type_e(data.getManifoldUpperLimits());else
          throw new RuntimeException("manifold_upper_limits field exceeds the maximum length");

   }

   public static void read(controller_msgs.msg.dds.ReachingManifoldMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setEndEffectorHashCode(cdr.read_type_2());
      	
      geometry_msgs.msg.dds.PointPubSubType.read(data.getManifoldOriginPosition(), cdr);	
      geometry_msgs.msg.dds.QuaternionPubSubType.read(data.getManifoldOriginOrientation(), cdr);	
      cdr.read_type_e(data.getManifoldConfigurationSpaceNames());	
      cdr.read_type_e(data.getManifoldLowerLimits());	
      cdr.read_type_e(data.getManifoldUpperLimits());	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.ReachingManifoldMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_2("end_effector_hash_code", data.getEndEffectorHashCode());
      ser.write_type_a("manifold_origin_position", new geometry_msgs.msg.dds.PointPubSubType(), data.getManifoldOriginPosition());

      ser.write_type_a("manifold_origin_orientation", new geometry_msgs.msg.dds.QuaternionPubSubType(), data.getManifoldOriginOrientation());

      ser.write_type_e("manifold_configuration_space_names", data.getManifoldConfigurationSpaceNames());
      ser.write_type_e("manifold_lower_limits", data.getManifoldLowerLimits());
      ser.write_type_e("manifold_upper_limits", data.getManifoldUpperLimits());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.ReachingManifoldMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setEndEffectorHashCode(ser.read_type_2("end_effector_hash_code"));
      ser.read_type_a("manifold_origin_position", new geometry_msgs.msg.dds.PointPubSubType(), data.getManifoldOriginPosition());

      ser.read_type_a("manifold_origin_orientation", new geometry_msgs.msg.dds.QuaternionPubSubType(), data.getManifoldOriginOrientation());

      ser.read_type_e("manifold_configuration_space_names", data.getManifoldConfigurationSpaceNames());
      ser.read_type_e("manifold_lower_limits", data.getManifoldLowerLimits());
      ser.read_type_e("manifold_upper_limits", data.getManifoldUpperLimits());
   }

   public static void staticCopy(controller_msgs.msg.dds.ReachingManifoldMessage src, controller_msgs.msg.dds.ReachingManifoldMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.ReachingManifoldMessage createData()
   {
      return new controller_msgs.msg.dds.ReachingManifoldMessage();
   }
   @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.ReachingManifoldMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.ReachingManifoldMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.ReachingManifoldMessage src, controller_msgs.msg.dds.ReachingManifoldMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public ReachingManifoldMessagePubSubType newInstance()
   {
      return new ReachingManifoldMessagePubSubType();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/example/ReachabilitySphereMapExample.java<|end_filename|>
package us.ihmc.avatar.reachabilityMap.example;

import us.ihmc.avatar.reachabilityMap.ReachabilityMapListener;
import us.ihmc.avatar.reachabilityMap.ReachabilitySphereMapCalculator;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.tools.MultiBodySystemTools;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.SimulationConstructionSetParameters;

public class ReachabilitySphereMapExample
{
   public ReachabilitySphereMapExample()
   {
      final RobotArm robot = new RobotArm();
      SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(true, 16000);
      SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters);
      Graphics3DObject coordinate = new Graphics3DObject();
      coordinate.transform(robot.getElevatorFrameTransformToWorld());
      coordinate.addCoordinateSystem(1.0);
      scs.addStaticLinkGraphics(coordinate);
      scs.startOnAThread();

      OneDoFJointBasics[] armJoints = MultiBodySystemTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJointBasics.class);
      ReachabilitySphereMapCalculator reachabilitySphereMapCalculator = new ReachabilitySphereMapCalculator(armJoints, scs);
//      reachabilitySphereMapCalculator.setControlFrameFixedInEndEffector(robot.getControlFrame());
      ReachabilityMapListener listener = new ReachabilityMapListener()
      {
         @Override
         public void hasReachedNewConfiguration()
         {
            robot.copyRevoluteJointConfigurationToPinJoints();
         }
      };
      reachabilitySphereMapCalculator.attachReachabilityMapListener(listener);
      reachabilitySphereMapCalculator.buildReachabilitySpace();
   }

   public static void main(String[] args)
   {
      new ReachabilitySphereMapExample();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasWristSensorCalibrationRequestPacket.java<|end_filename|>
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;

/**
       * Atlas specific message: request taring of the wrist force/torque sensors.
       */
public class AtlasWristSensorCalibrationRequestPacket extends Packet<AtlasWristSensorCalibrationRequestPacket> implements Settable<AtlasWristSensorCalibrationRequestPacket>, EpsilonComparable<AtlasWristSensorCalibrationRequestPacket>
{
   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_;
   /**
            * The robot side (left or right) for the wrist sensor you would like to request calibration for.
            */
   public byte robot_side_ = (byte) 255;

   public AtlasWristSensorCalibrationRequestPacket()
   {
   }

   public AtlasWristSensorCalibrationRequestPacket(AtlasWristSensorCalibrationRequestPacket other)
   {
      this();
      set(other);
   }

   public void set(AtlasWristSensorCalibrationRequestPacket other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

   }

   /**
            * 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_;
   }

   /**
            * The robot side (left or right) for the wrist sensor you would like to request calibration for.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * The robot side (left or right) for the wrist sensor you would like to request calibration for.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }


   public static Supplier<AtlasWristSensorCalibrationRequestPacketPubSubType> getPubSubType()
   {
      return AtlasWristSensorCalibrationRequestPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AtlasWristSensorCalibrationRequestPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AtlasWristSensorCalibrationRequestPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AtlasWristSensorCalibrationRequestPacket)) return false;

      AtlasWristSensorCalibrationRequestPacket otherMyClass = (AtlasWristSensorCalibrationRequestPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AtlasWristSensorCalibrationRequestPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PelvisHeightTrajectoryMessage.java<|end_filename|>
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 commands the controller to move the pelvis to a new height in the trajectory frame while going through the specified trajectory points.
       * Sending this command will not affect the pelvis horizontal position.
       * To control the pelvis 3D position use the PelvisTrajectoryMessage instead.
       */
public class PelvisHeightTrajectoryMessage extends Packet<PelvisHeightTrajectoryMessage> implements Settable<PelvisHeightTrajectoryMessage>, EpsilonComparable<PelvisHeightTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Execute this trajectory in user mode. User mode tries to achieve the desired regardless of the leg kinematics.
            */
   public boolean enable_user_pelvis_control_;
   /**
            * If enable_user_pelvis_control is true then enable_user_pelvis_height_control_during_walking
            * will keep the height manager in user mode while walking.
            * If this is false the height manager will switch back to controller mode when walking.
            */
   public boolean enable_user_pelvis_control_during_walking_;
   /**
            * The position trajectory information.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage euclidean_trajectory_;

   public PelvisHeightTrajectoryMessage()
   {
      euclidean_trajectory_ = new controller_msgs.msg.dds.EuclideanTrajectoryMessage();
   }

   public PelvisHeightTrajectoryMessage(PelvisHeightTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(PelvisHeightTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      enable_user_pelvis_control_ = other.enable_user_pelvis_control_;

      enable_user_pelvis_control_during_walking_ = other.enable_user_pelvis_control_during_walking_;

      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.staticCopy(other.euclidean_trajectory_, euclidean_trajectory_);
   }

   /**
            * 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_;
   }

   /**
            * Execute this trajectory in user mode. User mode tries to achieve the desired regardless of the leg kinematics.
            */
   public void setEnableUserPelvisControl(boolean enable_user_pelvis_control)
   {
      enable_user_pelvis_control_ = enable_user_pelvis_control;
   }
   /**
            * Execute this trajectory in user mode. User mode tries to achieve the desired regardless of the leg kinematics.
            */
   public boolean getEnableUserPelvisControl()
   {
      return enable_user_pelvis_control_;
   }

   /**
            * If enable_user_pelvis_control is true then enable_user_pelvis_height_control_during_walking
            * will keep the height manager in user mode while walking.
            * If this is false the height manager will switch back to controller mode when walking.
            */
   public void setEnableUserPelvisControlDuringWalking(boolean enable_user_pelvis_control_during_walking)
   {
      enable_user_pelvis_control_during_walking_ = enable_user_pelvis_control_during_walking;
   }
   /**
            * If enable_user_pelvis_control is true then enable_user_pelvis_height_control_during_walking
            * will keep the height manager in user mode while walking.
            * If this is false the height manager will switch back to controller mode when walking.
            */
   public boolean getEnableUserPelvisControlDuringWalking()
   {
      return enable_user_pelvis_control_during_walking_;
   }


   /**
            * The position trajectory information.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage getEuclideanTrajectory()
   {
      return euclidean_trajectory_;
   }


   public static Supplier<PelvisHeightTrajectoryMessagePubSubType> getPubSubType()
   {
      return PelvisHeightTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PelvisHeightTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PelvisHeightTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_user_pelvis_control_, other.enable_user_pelvis_control_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_user_pelvis_control_during_walking_, other.enable_user_pelvis_control_during_walking_, epsilon)) return false;

      if (!this.euclidean_trajectory_.epsilonEquals(other.euclidean_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PelvisHeightTrajectoryMessage)) return false;

      PelvisHeightTrajectoryMessage otherMyClass = (PelvisHeightTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.enable_user_pelvis_control_ != otherMyClass.enable_user_pelvis_control_) return false;

      if(this.enable_user_pelvis_control_during_walking_ != otherMyClass.enable_user_pelvis_control_during_walking_) return false;

      if (!this.euclidean_trajectory_.equals(otherMyClass.euclidean_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PelvisHeightTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("enable_user_pelvis_control=");
      builder.append(this.enable_user_pelvis_control_);      builder.append(", ");
      builder.append("enable_user_pelvis_control_during_walking=");
      builder.append(this.enable_user_pelvis_control_during_walking_);      builder.append(", ");
      builder.append("euclidean_trajectory=");
      builder.append(this.euclidean_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/controlModules/foot/QuadrupedFootState.java<|end_filename|>
package us.ihmc.quadrupedRobotics.controlModules.foot;

import us.ihmc.commonWalkingControlModules.controllerCore.command.feedbackController.FeedbackControlCommand;
import us.ihmc.commonWalkingControlModules.controllerCore.command.inverseDynamics.InverseDynamicsCommand;
import us.ihmc.commonWalkingControlModules.controllerCore.command.virtualModelControl.VirtualModelControlCommand;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
import us.ihmc.quadrupedRobotics.controller.toolbox.QuadrupedStepTransitionCallback;
import us.ihmc.quadrupedRobotics.controller.toolbox.QuadrupedWaypointCallback;
import us.ihmc.robotics.stateMachine.extra.EventState;

public abstract class QuadrupedFootState implements EventState
{
   protected QuadrupedStepTransitionCallback stepTransitionCallback = null;
   protected QuadrupedWaypointCallback waypointCallback = null;

   public VirtualModelControlCommand<?> getVirtualModelControlCommand()
   {
      return null;
   }

   public InverseDynamicsCommand<?> getInverseDynamicsCommand()
   {
      return null;
   }

   public abstract FeedbackControlCommand<?> getFeedbackControlCommand();
   public abstract FeedbackControlCommand<?> createFeedbackControlTemplate();

   public void registerStepTransitionCallback(QuadrupedStepTransitionCallback stepTransitionCallback)
   {
      this.stepTransitionCallback = stepTransitionCallback;
   }

   public void registerWaypointCallback(QuadrupedWaypointCallback waypointCallback)
   {
      this.waypointCallback = waypointCallback;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/Sliderboard.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

import javax.sound.midi.InvalidMidiDataException;
import javax.sound.midi.MidiDevice;
import javax.sound.midi.MidiSystem;
import javax.sound.midi.MidiUnavailableException;
import javax.sound.midi.Receiver;
import javax.sound.midi.ShortMessage;
import javax.sound.midi.Transmitter;

import us.ihmc.commons.PrintTools;
import us.ihmc.robotics.sliderboard.boards.SliderboardType;

/**
 * A lightweight sliderboard.
 * <p>
 * This class will automatically detect supported sliderboards if they are plugged in and available as MIDI devices.
 * To determine whether a slider board was connected successfully you may call {@link #isConnected()}. To close the
 * connection to any sliderboard call {@link #close()}.
 * </p>
 * <p>
 * To interact with the sliderboard listeners for sliders can be added and the value of a slider can be set through
 * {@link setSliderValue(double, int)}.
 * </p>
 *
 * @author Georg
 *
 */
public class Sliderboard
{
   private static final SliderboardType sliderboardType = SliderboardType.findConnectedSliderBoard();

   private final MidiControlMap channelMapper;
   private final SliderboardDataReciever sliderboardDataReciever;

   // Keep these around so they can be closed properly.
   private MidiDevice outDevice;
   private MidiDevice inDevice;
   private Receiver receiver;
   private Transmitter transmitter;

   public Sliderboard()
   {
      if (sliderboardType == null)
      {
         channelMapper = null;
         sliderboardDataReciever = null;
         return;
      }

      channelMapper = sliderboardType.getChannelMapper();
      sliderboardDataReciever = new SliderboardDataReciever(channelMapper);

      // Go through all system MIDI devices and find the ones for receiving from and sending to the sliderboard.
      for (MidiDevice.Info info : MidiSystem.getMidiDeviceInfo())
      {
         if (SliderboardTools.infoMatchesSliderboard(info, sliderboardType.getStringIdentifier()))
         {
            try
            {
               MidiDevice current = MidiSystem.getMidiDevice(info);

               if (receiver == null && (receiver = SliderboardTools.getReceiver(current)) != null)
               {
                  outDevice = current;
               }

               if (transmitter == null && (transmitter = SliderboardTools.getTransmitter(current)) != null)
               {
                  inDevice = current;
               }
            }
            catch (MidiUnavailableException e)
            {
               continue;
            }
         }
      }

      // If we were not able to find the MIDI devices or they did not connect, close everything.
      if (!isConnected())
      {
         close();
         return;
      }

      // Setup the callback for receiving data from the slider board.
      transmitter.setReceiver(sliderboardDataReciever);
   }

   public void close()
   {
      if (receiver != null)
      {
         receiver.close();
         receiver = null;
      }
      if (transmitter != null)
      {
         transmitter.close();
         transmitter = null;
      }
      if (outDevice != null)
      {
         outDevice.close();
         outDevice = null;
      }
      if (inDevice != null)
      {
         inDevice.close();
         inDevice = null;
      }
   }

   public boolean isConnected()
   {
      return receiver != null && transmitter != null && outDevice != null && inDevice != null && outDevice.isOpen() && inDevice.isOpen();
   }

   /**
    * Attaches a listener to the specified slider that will get called whenever a slider is moved.
    *
    * @param sliderListener the listener to ass to the slider
    * @param sliderIndex the index of the slider
    */
   public boolean addListener(SliderboardListener sliderListener, int sliderIndex)
   {
      if (!isConnected())
      {
         return false;
      }

      return sliderboardDataReciever.addListener(sliderListener, sliderIndex);
   }

   /**
    * Attaches a listener to the specified button that will get called whenever a button is pressed.
    *
    * @param buttonListener the listener for the button
    * @param buttonIndex the index of the button
    */
   public boolean addListener(ButtonListener buttonListener, int buttonIndex)
   {
      if (!isConnected())
      {
         return false;
      }

      return sliderboardDataReciever.addListener(buttonListener, buttonIndex);
   }

   /**
    * Sets the value of a slider.
    *
    * @param sliderPercent the new value for the slider between 0.0 and 1.0
    * @param sliderIndex the index of the slider
    */
   public void setSliderValue(double sliderPercent, int sliderIndex)
   {
      if (!isConnected())
      {
         return;
      }

      int channel = channelMapper.getSliderChannel(sliderIndex);
      if (channel == -1)
      {
         PrintTools.info("Unknown slider index: " + sliderIndex);
         return;
      }

      try
      {
         ShortMessage message = new ShortMessage();
         int data = SliderboardTools.toDataByte(sliderPercent);
         message.setMessage(SliderboardTools.STATUS, channel, data);

         // Should this be done on a thread?
         receiver.send(message, -1);
      }
      catch (InvalidMidiDataException e)
      {
         PrintTools.info("Was unable to create slider board message.");
      }
   }

   public void setButtonValue(boolean status, int buttonIndex)
   {
      if (!isConnected())
      {
         return;
      }

      int channel = channelMapper.getButtonChannel(buttonIndex);
      if (channel == -1)
      {
         PrintTools.info("Unknown button index: " + buttonIndex);
         return;
      }

      try
      {
         ShortMessage message = new ShortMessage();
         int data = SliderboardTools.toDataByte(status);
         message.setMessage(SliderboardTools.STATUS, channel, data);

         // Should this be done on a thread?
         receiver.send(message, -1);
      }
      catch (InvalidMidiDataException e)
      {
         PrintTools.info("Was unable to create slider board message.");
      }
   }

   public void clearListeners()
   {
      sliderboardDataReciever.clearListeners();
   }

   public void removeListener(SliderboardListener sliderListener, int sliderIndex)
   {
      sliderboardDataReciever.removeListener(sliderListener, sliderIndex);
   }

   public void removeListener(ButtonListener buttonListener, int buttonIndex)
   {
      sliderboardDataReciever.removeListener(buttonListener, buttonIndex);
   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/remote/DataObjectClient.java<|end_filename|>
package us.ihmc.communication.remote;

import java.io.IOException;
import java.net.ConnectException;
import java.net.Socket;

import us.ihmc.commons.thread.ThreadTools;

public class DataObjectClient extends DataObjectTransponder
{
   private String host;
   private int port;

   public DataObjectClient(String host, int port)
   {
      this.host = host;
      this.port = port;
      ThreadTools.startAsDaemon(new ClientConnectionDaemon(), "Data Object Client Conn Daemon");
   }

   public class ClientConnectionDaemon implements Runnable
   {
      private static final int RECONNECTION_PAUSE_DURATION = 100;
      private static final int LOST_CONNECTION_SLEEP_DURATION = 100;

      @Override
      public void run()
      {
         while (true)
         {
            while (!isConnected() && (socket == null))
            {
               tryToConnect();
            }

            sleep(LOST_CONNECTION_SLEEP_DURATION);
         }
      }

      private void tryToConnect()
      {
         try
         {
            connectToSocket(new Socket(host, port));
         }
         catch (ConnectException ce)
         {
            printIfDebug("Problem connecting to data source at " + host + ":" + port + ". Trying again...");
            sleep(RECONNECTION_PAUSE_DURATION);
         }
         catch (IOException e)
         {
            e.printStackTrace();
         }
      }

      private void sleep(int duration)
      {
         try
         {
            Thread.sleep(duration);
         }
         catch (InterruptedException e)
         {
            e.printStackTrace();
         }
      }
   }
}


<|start_filename|>ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerStatus.java<|end_filename|>
package us.ihmc.footstepPlanning;

public enum FootstepPlannerStatus
{
   IDLE,
   PLANNING_PATH,
   PLANNING_STEPS;

   public static final FootstepPlannerStatus[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static FootstepPlannerStatus fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/planarRegionEnvironments/CinderBlockFieldPlanarRegionEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.planarRegionEnvironments;

import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.robotics.geometry.PlanarRegionsListGenerator;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class CinderBlockFieldPlanarRegionEnvironment extends PlanarRegionEnvironmentInterface
{
   private enum BlockOrientation {NORTH, SOUTH, EAST, WEST}

   private static final double cinderBlockTiltDegrees = 15;
   private static final double cinderBlockTiltRadians = Math.toRadians(cinderBlockTiltDegrees);

   private static final double cinderBlockLength = 0.40; // 40 cm (approx 16 in, just less than 16in)
   private static final double flatCinderBlockHeight = 0.08; // 15 cm (approx 6 in, less than 6 in, but consistent with other cm measurements)
   private static final double cinderBlockHeight = 0.15; // 15 cm (approx 6 in, less than 6 in, but consistent with other cm measurements)

   private static final double distanceToFirstCinderBlock = 0.75;
   private static final double gapBetweenCinderBlockSets = 1.0;

   public static void main(String[] args)
   {
      SimulationConstructionSet scs = new SimulationConstructionSet();
      scs.setGroundVisible(false);
      scs.addStaticLinkGraphics(new CinderBlockFieldPlanarRegionEnvironment().getTerrainObject3D().getLinkGraphics());
      scs.startOnAThread();
   }

   public CinderBlockFieldPlanarRegionEnvironment()
   {
      generator.translate(0.0, 0.0, -0.01);
      generator.addCubeReferencedAtBottomMiddle(2.0 * distanceToFirstCinderBlock, 4 * cinderBlockLength, 0.01);
      generator.identity();

      double xPosition = distanceToFirstCinderBlock + 0.5 * cinderBlockLength;

      setUpFlatBlock(generator, xPosition, 0.2, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.6, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.2, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.6, 0.0, flatCinderBlockHeight);

      xPosition += cinderBlockLength;

      setUpFlatBlock(generator, xPosition, -0.6, flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.2, flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.2, -flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.6, flatCinderBlockHeight, flatCinderBlockHeight);

      xPosition += cinderBlockLength;

      setUpFlatBlock(generator, xPosition, -0.6, 2.0 * flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.2, flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.2, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.6, flatCinderBlockHeight, flatCinderBlockHeight);

      xPosition += cinderBlockLength;

      setUpFlatBlock(generator, xPosition, -0.6, flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.2, 2.0 * flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.2, flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.6, 0.0, flatCinderBlockHeight);

      xPosition += cinderBlockLength;

      setUpFlatBlock(generator, xPosition, -0.6, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.2, flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.2, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.6, -flatCinderBlockHeight, flatCinderBlockHeight);

      xPosition += cinderBlockLength;

      setUpFlatBlock(generator, xPosition, -0.6, -flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, -0.2, 0.0, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.2, -flatCinderBlockHeight, flatCinderBlockHeight);
      setUpFlatBlock(generator, xPosition, 0.6, -flatCinderBlockHeight, flatCinderBlockHeight);



      xPosition += 0.5 * (gapBetweenCinderBlockSets + cinderBlockLength);

      generator.translate(xPosition, 0.0, -0.01);
      generator.addCubeReferencedAtBottomMiddle(gapBetweenCinderBlockSets, 4.0 * cinderBlockLength, 0.01);
      generator.identity();

      xPosition += 0.5 * (gapBetweenCinderBlockSets + cinderBlockLength) + 0.04;

      setUpRampBlock(generator, 4.39, -0.6, 0.0, BlockOrientation.NORTH);
      setUpRampBlock(generator, 4.39, -0.2, 0.0, BlockOrientation.NORTH);
      setUpRampBlock(generator, 4.39, 0.2, 0.0, BlockOrientation.NORTH);
      setUpRampBlock(generator, 4.39, 0.6, 0.0, BlockOrientation.NORTH);

      xPosition += cinderBlockLength;

      setUpRampBlock(generator, 4.79, -0.6, 0.0, BlockOrientation.SOUTH);
      setUpRampBlock(generator, 4.79, -0.2, 0.0, BlockOrientation.SOUTH);
      setUpRampBlock(generator, 4.79, 0.2, 0.0, BlockOrientation.SOUTH);
      setUpRampBlock(generator, 4.79, 0.6, 0.0, BlockOrientation.SOUTH);

      xPosition += cinderBlockLength + 0.04;

      setUpRampBlock(generator, 5.23, -0.665, 0.0, BlockOrientation.EAST);
      setUpRampBlock(generator, 5.23, -0.195, 0.0, BlockOrientation.WEST);
      setUpRampBlock(generator, 5.23, 0.195, 0.0, BlockOrientation.EAST);
      setUpRampBlock(generator, 5.23, 0.665, 0.0, BlockOrientation.WEST);

      xPosition += cinderBlockLength + 0.01;

      setUpRampBlock(generator, 5.64, -0.635, 0.0, BlockOrientation.WEST);
      setUpRampBlock(generator, 5.64, -0.235, 0.0, BlockOrientation.EAST);
      setUpRampBlock(generator, 5.64, 0.235, 0.0, BlockOrientation.WEST);
      setUpRampBlock(generator, 5.64, 0.635, 0.0, BlockOrientation.EAST);


      xPosition += cinderBlockLength + 0.01;

      setUpFlatBlock(generator, 6.05, -0.6, cinderBlockHeight);
      setUpFlatBlock(generator, 6.05, -0.2, cinderBlockHeight);
      setUpFlatBlock(generator, 6.05, 0.2, cinderBlockHeight);
      setUpFlatBlock(generator, 6.05, 0.6, cinderBlockHeight);

      xPosition += cinderBlockLength + 0.025;

      setUpRampBlock(generator, 6.475 + 0.02, -0.6 - 0.04, 0.0, BlockOrientation.NORTH);
      setUpRampBlock(generator, 6.475, -0.2 - 0.04, 0.0, BlockOrientation.EAST);
      setUpRampBlock(generator, 6.475 - 0.01, 0.2, 0.0, BlockOrientation.SOUTH);
      setUpRampBlock(generator, 6.475, 0.6 + 0.04, 0.0, BlockOrientation.WEST);

      xPosition += cinderBlockLength + 0.04;

      setUpRampBlock(generator, 6.915 - 0.015, -0.64, cinderBlockHeight, BlockOrientation.EAST);
      setUpRampBlock(generator, 6.915 - 0.03, -0.2, cinderBlockHeight, BlockOrientation.SOUTH);
      setUpRampBlock(generator, 6.915 - 0.015, 0.24, cinderBlockHeight, BlockOrientation.WEST);
      setUpRampBlock(generator, 6.915, 0.64, cinderBlockHeight, BlockOrientation.NORTH);

      xPosition += cinderBlockLength + 0.015;

      setUpRampBlock(generator, 7.33 -0.015, -0.6 - 0.04, 0.0, BlockOrientation.SOUTH);
      setUpRampBlock(generator, 7.33, -0.2, 0.0, BlockOrientation.WEST);
      setUpRampBlock(generator, 7.33 + 0.015, 0.2, 0.0, BlockOrientation.NORTH);
      setUpRampBlock(generator, 7.33, 0.6, 0.0, BlockOrientation.EAST);

      xPosition += 0.75 + 0.5 * cinderBlockLength + 0.02;
      generator.translate(xPosition, 0.0, -0.01);
      generator.addCubeReferencedAtBottomMiddle(1.5, 4.0 * cinderBlockLength, 0.01);
      generator.identity();

      addPlanarRegionsToTerrain(YoAppearance.Grey());
   }

   private static void setUpFlatBlock(PlanarRegionsListGenerator generator, double xInWorld, double yInWorld, double bottomSurfaceHeightLowestPoint)
   {
      setUpFlatBlock(generator, xInWorld, yInWorld, bottomSurfaceHeightLowestPoint, cinderBlockHeight);
   }

   private static void setUpFlatBlock(PlanarRegionsListGenerator generator, double xInWorld, double yInWorld, double bottomSurfaceHeightLowestPoint, double blockHeight)
   {
      generator.translate(xInWorld, yInWorld, bottomSurfaceHeightLowestPoint);

      generator.addCubeReferencedAtBottomMiddle(cinderBlockLength, cinderBlockLength, blockHeight);

      generator.identity();
   }

   private static void setUpRampBlock(PlanarRegionsListGenerator generator, double xInWorld, double yInWorld, double surfaceHeightLowestPoint,
                                      BlockOrientation orientation)
   {
      double zHeightToCenter = 0.5 * cinderBlockLength * Math.sin(cinderBlockTiltRadians);

      generator.translate(xInWorld, yInWorld, surfaceHeightLowestPoint + zHeightToCenter);

      Quaternion quaternion = new Quaternion();
      switch (orientation)
      {
      case NORTH:
         quaternion.appendPitchRotation(-cinderBlockTiltRadians);
         break;
      case EAST:
         quaternion.appendRollRotation(-cinderBlockTiltRadians);
         break;
      case WEST:
         quaternion.appendRollRotation(cinderBlockTiltRadians);
         break;
      case SOUTH:
         quaternion.appendPitchRotation(cinderBlockTiltRadians);
         break;
      }
      generator.rotate(quaternion);

      generator.addCubeReferencedAtBottomMiddle(cinderBlockLength, cinderBlockLength, cinderBlockHeight);

      switch (orientation)
      {
      case NORTH:
         quaternion.appendPitchRotation(cinderBlockTiltRadians);
         break;
      case EAST:
         quaternion.appendRollRotation(cinderBlockTiltRadians);
         break;
      case WEST:
         quaternion.appendRollRotation(-cinderBlockTiltRadians);
         break;
      case SOUTH:
         quaternion.appendPitchRotation(-cinderBlockTiltRadians);
         break;
      }
      generator.identity();
   }

}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/estimator/sensorProcessing/sensorProcessors/FootSwitchUpdaterBasedOnOtherFeet.java<|end_filename|>
package us.ihmc.quadrupedRobotics.estimator.sensorProcessing.sensorProcessors;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.commons.MathTools;
import us.ihmc.robotics.robotSide.QuadrantDependentList;
import us.ihmc.robotics.robotSide.RobotQuadrant;

public class FootSwitchUpdaterBasedOnOtherFeet implements FootSwitchUpdater
{ 
   private static final double EPSILON = 1e-5;
   private QuadrantDependentList<FramePoint3D> footPositions = new QuadrantDependentList<FramePoint3D>();
   
   public FootSwitchUpdaterBasedOnOtherFeet(QuadrantDependentList<FramePoint3D> footPositions)
   {
      this.footPositions = footPositions;
   }

   @Override
   public boolean isFootInContactWithGround(RobotQuadrant footToBeChecked)
   {
      double footToBeCheckedZ = footPositions.get(footToBeChecked).getZ(); 
      double otherFeetZ = 0.0;
      
      for(RobotQuadrant robotQuadrant : RobotQuadrant.values())
      {
         if(robotQuadrant != footToBeChecked)
         {
            otherFeetZ += footPositions.get(robotQuadrant).getZ();
         }
         otherFeetZ /= 3.0;
      }
     
      if(MathTools.epsilonEquals(footToBeCheckedZ, otherFeetZ, EPSILON))
         return true;
      
      return false;
   }
}




<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/CylindricalPDGains.java<|end_filename|>
package us.ihmc.robotics.controllers;

import us.ihmc.robotics.controllers.pidGains.GainCalculator;

/**
 * @author twan
 *         Date: 6/15/13
 */
public class CylindricalPDGains
{
   private final double kpRadius;
   private final double kpAngle;
   private final double kpZ;

   private final double kdRadius;
   private final double kdAngle;
   private final double kdZ;

   public CylindricalPDGains(double kpRadius, double kpAngle, double kpZ, double zeta)
   {
      this.kpRadius = kpRadius;
      this.kpAngle = kpAngle;
      this.kpZ = kpZ;

      this.kdRadius = GainCalculator.computeDerivativeGain(kpRadius, zeta);
      this.kdAngle = GainCalculator.computeDerivativeGain(kpAngle, zeta);
      this.kdZ = GainCalculator.computeDerivativeGain(kpZ, zeta);
   }

   public double getKpRadius()
   {
      return kpRadius;
   }

   public double getKpAngle()
   {
      return kpAngle;
   }

   public double getKpZ()
   {
      return kpZ;
   }

   public double getKdRadius()
   {
      return kdRadius;
   }

   public double getKdAngle()
   {
      return kdAngle;
   }

   public double getKdZ()
   {
      return kdZ;
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/StairsUpAndDownEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationConstructionSetTools.robotController.ContactController;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.Contactable;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class StairsUpAndDownEnvironment implements CommonAvatarEnvironmentInterface
{
   private final List<Robot> contactableRobots = new ArrayList<Robot>();
   private final CombinedTerrainObject3D combinedTerrainObject;
   private final ArrayList<ExternalForcePoint> contactPoints = new ArrayList<ExternalForcePoint>();

   private static final int totalStepsUp = 3;
   private static final int totalStepsDown = 3;
   private static final double stepUpHeight = 0.19685;
   private static final double totalHeightUp = totalStepsUp * stepUpHeight;
   private static final double stepDownHeight = totalHeightUp / totalStepsDown;

   private static final double stairUpDepth = 0.2921;
   private static final double stairDownDepth = 0.38;
   private static final double startingPosition = 1.0;

   private static final double landingDepth = 0.85;
   private static final double landingWidth = 2.1;

   private final List<List<FramePose3D>> stairPoses = new ArrayList<>();

   public StairsUpAndDownEnvironment()
   {
      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject.addTerrainObject(setUpGround("Ground"));

      createStairsUp();
      createStairsDown();
   }

   private void createStairsUp()
   {
      AdjustableStairsEnvironment environment = new AdjustableStairsEnvironment();
      environment.setStairsParameters(totalStepsUp, 1.016, stepUpHeight, stairUpDepth);
      environment.setRailingParameters(0.05, 0.3, 0.05, 0.8128, 2, false);
      environment.setLandingPlatformParameters(landingDepth, landingWidth, 1.143, 2);
      environment.setCourseStartDistance(startingPosition);

      environment.generateTerrains();
      ArrayList<TerrainObject3D> stairs = ((CombinedTerrainObject3D) environment.getTerrainObject3D()).getTerrainObjects();
      for (TerrainObject3D object : stairs)
      {
         if (object instanceof CombinedTerrainObject3D)
         {
            if (!((CombinedTerrainObject3D) object).getName().contains("ground"))
            {
               combinedTerrainObject.addTerrainObject(object);
            }
         }
      }

      List<FramePose3D> stepPoses = new ArrayList<>();

      //double xPosition = startingPosition + 0.5 * stairDepth;
      double xPosition = startingPosition + 1.5 * stairUpDepth;
      double yPosition = 1.0;
      double zPosition = stepUpHeight;
      for (int i = 0; i < totalStepsUp; i++)
      {
         FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame());
         pose.setX(xPosition);
         pose.setY(yPosition);
         pose.setZ(zPosition);
         stepPoses.add(pose);
         xPosition += stairUpDepth;
         zPosition += stepUpHeight;
      }

      stairPoses.add(stepPoses);
   }

   private void createStairsDown()
   {
      AdjustableStairsEnvironment environment = new AdjustableStairsEnvironment();
      environment.setStairsParameters(totalStepsDown, 1.016, stepDownHeight, stairDownDepth);
      environment.setRailingParameters(0.05, 0.3, 0.05, 0.8128, 2, false);
      environment.setLandingPlatformParameters(landingDepth, landingWidth, 1.143, 2);
      environment.setCourseStartDistance(-(startingPosition + totalStepsUp * stairUpDepth + totalStepsDown * stairDownDepth + landingDepth));
      environment.setCourseAngle(180);

      environment.generateTerrains();
      ArrayList<TerrainObject3D> stairs = ((CombinedTerrainObject3D) environment.getTerrainObject3D()).getTerrainObjects();
      for (TerrainObject3D object : stairs)
      {
         if (object instanceof CombinedTerrainObject3D)
         {
            if (!((CombinedTerrainObject3D) object).getName().contains("ground"))
            {
               combinedTerrainObject.addTerrainObject(object);
            }
         }
      }

      List<FramePose3D> stepPoses = new ArrayList<>();

      double xPosition = startingPosition + totalStepsUp * stairUpDepth + landingDepth + 0.5 * stairDownDepth;
      double yPosition = 1.0;
      double zPosition = totalHeightUp - stepDownHeight;
      for (int i = 0; i < totalStepsDown; i++)
      {
         FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame());
         pose.setX(xPosition);
         pose.setY(yPosition);
         pose.setZ(zPosition);
         stepPoses.add(pose);
         xPosition += stairDownDepth;
         zPosition -= stepDownHeight;
      }

      stairPoses.add(stepPoses);
   }

   private CombinedTerrainObject3D setUpGround(String name)
   {
      CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);

      combinedTerrainObject.addBox(-5.0, -30.0, 5.0, 5.0, -0.05, 0.0, YoAppearance.DarkGray());
      return combinedTerrainObject;
   }

   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return contactableRobots;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
      ContactController contactController = new ContactController();
      contactController.setContactParameters(100000.0, 100.0, 0.5, 0.3);

      contactController.addContactPoints(contactPoints);

      for (Robot r : contactableRobots)
      {
         if (r instanceof Contactable)
            contactController.addContactable((Contactable) r);

      }
      if (contactableRobots.size() > 0)
         contactableRobots.get(0).setController(contactController);
   }

   public List<List<FramePose3D>> getStairPoses()
   {
      return stairPoses;
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
      this.contactPoints.addAll(externalForcePoints);
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/FreezeControllerStateFactory.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.FreezeControllerState;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;

public class FreezeControllerStateFactory implements HighLevelControllerStateFactory
{
   private FreezeControllerState freezeControllerState;

   @Override
   public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper)
   {
      if (freezeControllerState == null)
      {
         freezeControllerState = new FreezeControllerState(controllerFactoryHelper.getHighLevelHumanoidControllerToolbox().getControlledOneDoFJoints(),
                                                           controllerFactoryHelper.getHighLevelControllerParameters(),
                                                           controllerFactoryHelper.getLowLevelControllerOutput());
      }

      return freezeControllerState;
   }

   @Override
   public HighLevelControllerName getStateEnum()
   {
      return HighLevelControllerName.FREEZE_STATE;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/updaters/AdaptiveRayMissProbabilityUpdater.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.updaters;

import us.ihmc.commons.MathTools;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.jOctoMap.node.NormalOcTreeNode;
import us.ihmc.jOctoMap.ocTree.NormalOcTree.RayMissProbabilityUpdater;
import us.ihmc.jOctoMap.occupancy.OccupancyParameters;

public class AdaptiveRayMissProbabilityUpdater implements RayMissProbabilityUpdater
{
   private double distanceSquaredToEndThreshold = MathTools.square(0.06);
   private double rayEndMissProbability = 0.47;

   private int normalConsensusThreshold = 10;
   private double dotRayToNormalThreshold = Math.cos(Math.toRadians(150.0));
   private double shallowAngleMissProbability = 0.45;

   @Override
   public double computeRayMissProbability(Point3DReadOnly rayOrigin, Point3DReadOnly rayEnd, Vector3DReadOnly rayDirection, NormalOcTreeNode node, OccupancyParameters parameters)
   {
      Point3D hitLocation = new Point3D();
      node.getHitLocation(hitLocation);

      if (hitLocation.distanceSquared(rayEnd) < distanceSquaredToEndThreshold)
      {
         return rayEndMissProbability;
      }
      else if (node.getNormalConsensusSize() > normalConsensusThreshold && node.isNormalSet())
      {
         Point3D nodeHitLocation = new Point3D();
         Vector3D nodeNormal = new Vector3D();
         node.getHitLocation(nodeHitLocation);
         node.getNormal(nodeNormal);

         if (Math.abs(rayDirection.dot(nodeNormal)) < dotRayToNormalThreshold)
            return shallowAngleMissProbability;
         else
            return parameters.getMissProbability();
      }
      else
      {
         return parameters.getMissProbability();
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/DetectedObjectPacket.java<|end_filename|>
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 behavior module.
       */
public class DetectedObjectPacket extends Packet<DetectedObjectPacket> implements Settable<DetectedObjectPacket>, EpsilonComparable<DetectedObjectPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.geometry.Pose3D pose_;
   public int id_;

   public DetectedObjectPacket()
   {
      pose_ = new us.ihmc.euclid.geometry.Pose3D();
   }

   public DetectedObjectPacket(DetectedObjectPacket other)
   {
      this();
      set(other);
   }

   public void set(DetectedObjectPacket other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.pose_, pose_);
      id_ = other.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 us.ihmc.euclid.geometry.Pose3D getPose()
   {
      return pose_;
   }

   public void setId(int id)
   {
      id_ = id;
   }
   public int getId()
   {
      return id_;
   }


   public static Supplier<DetectedObjectPacketPubSubType> getPubSubType()
   {
      return DetectedObjectPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return DetectedObjectPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(DetectedObjectPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.pose_.epsilonEquals(other.pose_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.id_, other.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 DetectedObjectPacket)) return false;

      DetectedObjectPacket otherMyClass = (DetectedObjectPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.pose_.equals(otherMyClass.pose_)) return false;
      if(this.id_ != otherMyClass.id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("DetectedObjectPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("pose=");
      builder.append(this.pose_);      builder.append(", ");
      builder.append("id=");
      builder.append(this.id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SimpleCoactiveBehaviorDataPacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class SimpleCoactiveBehaviorDataPacket extends Packet<SimpleCoactiveBehaviorDataPacket> implements Settable<SimpleCoactiveBehaviorDataPacket>, EpsilonComparable<SimpleCoactiveBehaviorDataPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public java.lang.StringBuilder key_;
   public double value_;

   public SimpleCoactiveBehaviorDataPacket()
   {
      key_ = new java.lang.StringBuilder(255);
   }

   public SimpleCoactiveBehaviorDataPacket(SimpleCoactiveBehaviorDataPacket other)
   {
      this();
      set(other);
   }

   public void set(SimpleCoactiveBehaviorDataPacket other)
   {
      sequence_id_ = other.sequence_id_;

      key_.setLength(0);
      key_.append(other.key_);

      value_ = other.value_;

   }

   /**
            * 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 void setKey(java.lang.String key)
   {
      key_.setLength(0);
      key_.append(key);
   }

   public java.lang.String getKeyAsString()
   {
      return getKey().toString();
   }
   public java.lang.StringBuilder getKey()
   {
      return key_;
   }

   public void setValue(double value)
   {
      value_ = value;
   }
   public double getValue()
   {
      return value_;
   }


   public static Supplier<SimpleCoactiveBehaviorDataPacketPubSubType> getPubSubType()
   {
      return SimpleCoactiveBehaviorDataPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SimpleCoactiveBehaviorDataPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SimpleCoactiveBehaviorDataPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.key_, other.key_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.value_, other.value_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SimpleCoactiveBehaviorDataPacket)) return false;

      SimpleCoactiveBehaviorDataPacket otherMyClass = (SimpleCoactiveBehaviorDataPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!us.ihmc.idl.IDLTools.equals(this.key_, otherMyClass.key_)) return false;

      if(this.value_ != otherMyClass.value_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SimpleCoactiveBehaviorDataPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("key=");
      builder.append(this.key_);      builder.append(", ");
      builder.append("value=");
      builder.append(this.value_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/buildingPendulum/BuildingPendulumController.java<|end_filename|>
package us.ihmc.exampleSimulations.buildingPendulum;

import us.ihmc.yoVariables.variable.YoBoolean;
import us.ihmc.yoVariables.variable.YoDouble;
import us.ihmc.yoVariables.variable.YoEnum;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.simulationConstructionSetTools.robotController.SimpleRobotController;

public class BuildingPendulumController extends SimpleRobotController
{
   public static String variableName = "MySliderBoardTest";

   private final BuildingPendulumRobot robot;
   private final YoBoolean atCenter = new YoBoolean("AtCenter", registry);

   private RobotSide activeSide;
   private final YoEnum<RobotSide> yoActiveSide = new YoEnum<>("ActiveSide", registry, RobotSide.class);

   private double pendulumAngle;
   private double pendulumAngleSwitch;
   private double angularChange = 2*Math.asin(BuildingPendulumRobot.distance/(2*BuildingPendulumRobot.length));


   private final YoDouble mySliderBoardTest = new YoDouble(variableName, registry);

   public BuildingPendulumController(BuildingPendulumRobot robot)
   {
      this.robot = robot;

      activeSide = RobotSide.LEFT;

//      if(robot.getPendulumAngle(RobotSide.LEFT)> robot.getPendulumAngle(RobotSide.RIGHT))
//         activeSide = RobotSide.LEFT;
//      else if(robot.getPendulumAngle(RobotSide.LEFT)< robot.getPendulumAngle(RobotSide.RIGHT))
//         activeSide = RobotSide.RIGHT;

   }

   public void setPendulumAngles()
   {
      pendulumAngle =robot.getPendulumAngle(activeSide);
      pendulumAngleSwitch = robot.getSwitchAngle(activeSide);
   }



   public  void doControl()
   {
      setPendulumAngles();

      boolean atCenter;
      if (activeSide == RobotSide.LEFT)
         atCenter = (pendulumAngle > pendulumAngleSwitch);
      else
         atCenter = (pendulumAngle < pendulumAngleSwitch);

      if (atCenter)
      {

         activeSide = activeSide.getOppositeSide();
         robot.setPendulumAngle(activeSide, robot.getSwitchAngle(activeSide));
         
         double velocity = (robot.getPendulumVelocity(activeSide.getOppositeSide())*Math.cos(angularChange));

         robot.setPendulumVelocity(activeSide, velocity);
      }

      // set the inactive pendulum to stay at the switching position
      robot.setPendulumAngle(activeSide.getOppositeSide(), robot.getSwitchAngle(activeSide.getOppositeSide()));
      robot.setPendulumVelocity(activeSide.getOppositeSide(), 0.0);

      // set yoVariables for debugging in SCS
      this.atCenter.set(atCenter);
      yoActiveSide.set(activeSide);

      // try out new slider variable
      if (mySliderBoardTest.getDoubleValue() <= -0.5)
         System.out.println("small value...");
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/quadTreeHeightMap/HeightQuadTreeNode.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.quadTreeHeightMap;

import us.ihmc.euclid.tuple2D.Point2D32;
import us.ihmc.euclid.tuple2D.Vector2D32;

public class HeightQuadTreeNode
{
   private float height;
   private Point2D32 center = new Point2D32();
   private Vector2D32 size = new Vector2D32();
   private HeightQuadTreeNode[] children;

   public HeightQuadTreeNode()
   {
   }

   public boolean hasChildrenArray()
   {
      return children != null;
   }

   public int getNumberOfChildren()
   {
      if (children == null)
         return 0;
      int numberOfChildren = 0;

      for (int childIndex = 0; childIndex < 4; childIndex++)
         numberOfChildren += children[childIndex] == null ? 0 : 1;

      return numberOfChildren;
   }

   public HeightQuadTreeNode getChild(int childIndex)
   {
      return children == null ? null : children[childIndex];
   }

   public void assignChildrenArray()
   {
      children = new HeightQuadTreeNode[4];
   }

   public void setChild(int childIndex, HeightQuadTreeNode child)
   {
      children[childIndex] = child;
   }

   public float getHeight()
   {
      return height;
   }

   public Point2D32 getCenter()
   {
      return center;
   }

   public float getCenterX()
   {
      return center.getX32();
   }

   public float getCenterY()
   {
      return center.getY32();
   }

   public Vector2D32 getSize()
   {
      return size;
   }

   public float getSizeX()
   {
      return size.getX32();
   }

   public float getSizeY()
   {
      return size.getY32();
   }

   public void setHeight(float height)
   {
      this.height = height;
   }

   public void setCenterX(float centerX)
   {
      this.center.setX(centerX);
   }

   public void setCenterY(float centerY)
   {
      this.center.setY(centerY);
   }

   public void setSizeX(float sizeX)
   {
      this.size.setX(sizeX);
   }

   public void setSizeY(float sizeY)
   {
      this.size.setY(sizeY);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/DrillDetectionPacket.java<|end_filename|>
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;

public class DrillDetectionPacket extends Packet<DrillDetectionPacket> implements Settable<DrillDetectionPacket>, EpsilonComparable<DrillDetectionPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean is_drill_on_;

   public DrillDetectionPacket()
   {
   }

   public DrillDetectionPacket(DrillDetectionPacket other)
   {
      this();
      set(other);
   }

   public void set(DrillDetectionPacket other)
   {
      sequence_id_ = other.sequence_id_;

      is_drill_on_ = other.is_drill_on_;

   }

   /**
            * 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 void setIsDrillOn(boolean is_drill_on)
   {
      is_drill_on_ = is_drill_on;
   }
   public boolean getIsDrillOn()
   {
      return is_drill_on_;
   }


   public static Supplier<DrillDetectionPacketPubSubType> getPubSubType()
   {
      return DrillDetectionPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return DrillDetectionPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(DrillDetectionPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_drill_on_, other.is_drill_on_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof DrillDetectionPacket)) return false;

      DrillDetectionPacket otherMyClass = (DrillDetectionPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.is_drill_on_ != otherMyClass.is_drill_on_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("DrillDetectionPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("is_drill_on=");
      builder.append(this.is_drill_on_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/whiteBoard/YoWhiteBoardType.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.whiteBoard;

public enum YoWhiteBoardType
{
   LOCAL, TCP, UDP
}

<|start_filename|>websitedocs/website/node_modules/docusaurus/lib/core/anchors.js<|end_filename|>
/**
 * Copyright (c) 2017-present, Facebook, Inc.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

const toSlug = require('./toSlug.js');

/**
 * The anchors plugin adds GFM-style anchors to headings.
 */
function anchors(md) {
  const originalRender = md.renderer.rules.heading_open;

  md.renderer.rules.heading_open = function(tokens, idx, options, env) {
    const textToken = tokens[idx + 1];

    if (textToken.content) {
      const anchor = toSlug(textToken.content, env);

      return (
        '<h' +
        tokens[idx].hLevel +
        '><a class="anchor" aria-hidden="true" id="' +
        anchor +
        '"></a><a href="#' +
        anchor +
        '" aria-hidden="true" class="hash-link"><svg class="hash-link-icon" aria-hidden="true" height="16" version="1.1" viewBox="0 0 16 16" width="16"><path fill-rule="evenodd" d="M4 9h1v1H4c-1.5 0-3-1.69-3-3.5S2.55 3 4 3h4c1.45 0 3 1.69 3 3.5 0 1.41-.91 2.72-2 3.25V8.59c.58-.45 1-1.27 1-2.09C10 5.22 8.98 4 8 4H4c-.98 0-2 1.22-2 2.5S3 9 4 9zm9-3h-1v1h1c1 0 2 1.22 2 2.5S13.98 12 13 12H9c-.98 0-2-1.22-2-2.5 0-.83.42-1.64 1-2.09V6.25c-1.09.53-2 1.84-2 3.25C6 11.31 7.55 13 9 13h4c1.45 0 3-1.69 3-3.5S14.5 6 13 6z"></path></svg></a>'
      );
    }

    return originalRender(tokens, idx, options, env);
  };
}

module.exports = anchors;


<|start_filename|>ihmc-ros-tools/generated-src/ihmc_atlas_ros/AtlasWristSensorCalibrationRequestPacketRosMessage.java<|end_filename|>
package ihmc_atlas_ros;

public interface AtlasWristSensorCalibrationRequestPacketRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_atlas_ros/AtlasWristSensorCalibrationRequestPacketRosMessage";
  static final java.lang.String _DEFINITION = "## AtlasWristSensorCalibrationRequestPacketRosMessage\n# Request taring of the wrist force/torque sensors.\n\n# The robot side (left or right) for the wrist sensor you would like to request calibration for.\nint8 robot_side\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  byte getRobotSide();
  void setRobotSide(byte value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/converter/FrameMessageCommandConverter.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.controllerAPI.converter;

import us.ihmc.communication.controllerAPI.CommandConversionInterface;
import us.ihmc.communication.controllerAPI.command.Command;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.sensorProcessing.frames.ReferenceFrameHashCodeResolver;

public class FrameMessageCommandConverter implements CommandConversionInterface
{
   private final ReferenceFrameHashCodeResolver referenceFrameHashCodeResolver;

   public FrameMessageCommandConverter(ReferenceFrameHashCodeResolver referenceFrameHashCodeResolver)
   {
      this.referenceFrameHashCodeResolver = referenceFrameHashCodeResolver;
   }

   /** {@inheritDoc} */
   @Override
   public <C extends Command<?, M>, M extends Settable<M>> boolean isConvertible(C command, M message)
   {
      return command instanceof FrameBasedCommand;
   }
   
   /** {@inheritDoc} */
   @SuppressWarnings("unchecked")
   @Override
   public <C extends Command<?, M>, M extends Settable<M>> void process(C command, M message)
   {
      FrameBasedCommand<M> frameBasedCommand = (FrameBasedCommand<M>) command;
      frameBasedCommand.set(referenceFrameHashCodeResolver, message);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/hyperCubeTree/Octree.java<|end_filename|>
package us.ihmc.robotics.hyperCubeTree;

import java.util.List;

import us.ihmc.euclid.geometry.LineSegment3D;
import us.ihmc.euclid.tuple3D.Point3D;

public class Octree extends HyperCubeTree<Boolean, Void>
{
   private final ResolutionProvider constantResolution;

   public Octree(OneDimensionalBounds[] bounds, double resolution)
   {
      this(bounds, new ConstantResolutionProvider(resolution));
   }

   public Octree(OneDimensionalBounds[] bounds, ResolutionProvider resolutionProvider)
   {
      super(bounds);
      this.constantResolution = resolutionProvider;
      if (bounds.length != 3)
         throw new DimensionalityMismatchException();
   }

   public void mergeIfPossible()
   {
      lock();
      mergeWherePossibleRecursively(getRootNode());
      unlock();
   }

   private void mergeTruth(RecursableHyperTreeNode<Boolean, Void> node)
   {
      if (!node.hasChildren())
         return;
      HyperCubeLeaf<Boolean> firstLeaf = null;
      for (int i = 0; i < node.getChildNumber(); i++)
      {
         RecursableHyperTreeNode<Boolean, Void> lowerLevelNode = node.getChild(i);
         if (lowerLevelNode.hasChildren())
            return;
         if (null == lowerLevelNode.getLeaf())
            continue;

         if (null == firstLeaf)
         {
            firstLeaf = lowerLevelNode.getLeaf();

            continue;
         }

         if (false == lowerLevelNode.getLeaf().getValue())
            return;
      }

      if (null == firstLeaf)
         return;
      node.clear();
      node.setLeaf(firstLeaf);
   }

   public void nodeAdded(String id, OneDimensionalBounds[] bounds, HyperCubeLeaf<Boolean> leaf)
   {
      super.nodeAdded(id, bounds, leaf);
   }

   public void nodeRemoved(String id)
   {
      super.nodeRemoved(id);
   }

   public void putLidarAtGraduallyMoreAccurateResolution(Point3D start, Point3D end)
   {
      HyperVolume line = new LineSegmentSearchVolume(new LineSegment3D(start, end));
      double[] location = new double[] {end.getX(), end.getY(), end.getZ()};
      this.put(location, true);
      this.leafAdded(new HyperCubeLeaf<Boolean>(true, location));
      RecursableHyperTreeNode<Boolean, Void> endNode = this.getLeafNodeAtLocation(location);
      List<RecursableHyperTreeNode<Boolean, Void>> lineNodes = this.getHyperVolumeIntersection(line);
      lineNodes.remove(endNode);

      for (RecursableHyperTreeNode<Boolean, Void> node : lineNodes)
      {
         double[] intersectionWithBounds = line.intersectionWithBounds(new BoundsGetter(node).get());
         HyperCubeLeaf<Boolean> hyperCubeLeaf = new HyperCubeLeaf<Boolean>(false, intersectionWithBounds);
         this.replacementPut(hyperCubeLeaf);
      }
   }

   public void putLidarAtMinimumResolution(Point3D start, Point3D end)
   {
      HyperVolume line = new LineSegmentSearchVolume(new LineSegment3D(start, end));
      double[] location = new double[] {end.getX(), end.getY(), end.getZ()};
      this.upRezz(location);
      this.put(location, true);
      this.leafAdded(new HyperCubeLeaf<Boolean>(true, location));
      RecursableHyperTreeNode<Boolean, Void> endNode = this.getLeafNodeAtLocation(location);
      List<RecursableHyperTreeNode<Boolean, Void>> lineNodes = this.getHyperVolumeIntersection(line);
      lineNodes.remove(endNode);

      for (RecursableHyperTreeNode<Boolean, Void> node : lineNodes)
      {
         double[] intersectionWithBounds = line.intersectionWithBounds(new BoundsGetter(node).get());
         HyperCubeLeaf<Boolean> hyperCubeLeaf = new HyperCubeLeaf<Boolean>(false, intersectionWithBounds);
         this.replacementPut(hyperCubeLeaf);
      }

      lock();
      this.mergeParentRecursively(getRootNode(), location);
      unlock();
   }

   protected boolean canMergeLeaves(HyperCubeLeaf<Boolean> firstLeaf, HyperCubeLeaf<Boolean> secondLeaf)
   {
      return firstLeaf.getValue() == secondLeaf.getValue();
   }

   protected boolean canSplit(RecursableHyperTreeNode<Boolean, Void> node)
   {
      for (int i = 0; i < node.getDimensionality(); i++)
      {
         if (node.getBounds(i).size() <= constantResolution.getResolution(node.getMidpoint()))
            return false;
      }

      return true;
   }

   protected HyperCubeLeaf<Boolean> mergeLeaves(HyperCubeLeaf<Boolean> oldLeaf, HyperCubeLeaf<Boolean> newLeaf)
   {
      return newLeaf;
   }

   private void mergeParentRecursively(RecursableHyperTreeNode<Boolean, Void> node, double[] location)
   {
      if (null == node)
         return;

      if (node.hasChildren())
      {
         mergeParentRecursively(node.getChildAtLocation(location), location);
         mergeTruth(node);
      }
   }

   public static class BoundsGetter
   {
      RecursableHyperTreeNode<?, ?> node;

      public BoundsGetter(RecursableHyperTreeNode<?, ?> node)
      {
         this.node = node;
      }

      public OneDimensionalBounds[] get()
      {
         OneDimensionalBounds[] ret = new OneDimensionalBounds[node.getDimensionality()];
         for (int i = 0; i < node.getDimensionality(); i++)
         {
            ret[i] = node.getBounds(i);
         }

         return ret;
      }
   }


   public void treeCleared()
   {

   }

   public boolean put(HyperCubeLeaf<Boolean> leaf)
   {
      lock();
      boolean success = putRecursively(getRootNode(), leaf);
      unlock();
      return success;
   }

   private boolean putRecursively(RecursableHyperTreeNode<Boolean, Void> node, final HyperCubeLeaf<Boolean> leaf)
   {
      if (node.hasChildren())
      {
         return putRecursively(node.getChildAtLocation(leaf.getLocation()), leaf);
      }

      if (node.getLeaf() != null)
      {
         if (canSplit(node))
         {
            HyperCubeLeaf<Boolean> oldLeaf = node.getLeaf();
            node.setLeaf(null);
            node.split();
            node.getChildAtLocation(oldLeaf.getLocation()).setLeaf(oldLeaf);
            RecursableHyperTreeNode<Boolean, Void> childAtLocation = node.getChildAtLocation(leaf.getLocation());
            childAtLocation.setLeaf(mergeLeaves(childAtLocation.getLeaf(), leaf));

            return true;
         }
         else
         {
            if (node.getLeaf().getValue().booleanValue() == leaf.getValue().booleanValue())
            {
               return false;
            }
            else
            {
               node.setLeaf(leaf);

               return true;
            }
         }
      }
      else
      {
         node.setLeaf(leaf);

         return true;
      }
   }

}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/viewers/StartGoalPositionViewer.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui.viewers;

import java.util.concurrent.atomic.AtomicReference;

import javafx.animation.AnimationTimer;
import javafx.collections.ObservableList;
import javafx.scene.Group;
import javafx.scene.Node;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.Sphere;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.messager.Messager;
import us.ihmc.messager.MessagerAPIFactory.Topic;

public class StartGoalPositionViewer extends AnimationTimer
{
   public static final double RADIUS = 0.05;

   private final Group root = new Group();
   private final ObservableList<Node> rootChildren = root.getChildren();

   private final Sphere startSphere = new Sphere(RADIUS);
   private final Sphere goalSphere = new Sphere(RADIUS);
   private boolean isStartCurrentlyShown;
   private boolean isGoalCurrentlyShown;

   public enum StartGoalViewMode
   {
      TRANSLUCENT, OPAQUE
   };

   public static final PhongMaterial startOpaqueMaterial = new PhongMaterial(Color.GREEN);
   public static final PhongMaterial startTransparentMaterial = new PhongMaterial(toTransparentColor(Color.ORANGE, 0.7));
   public static final PhongMaterial goalOpaqueMaterial = new PhongMaterial(Color.RED);
   public static final PhongMaterial goalTransparentMaterial = new PhongMaterial(toTransparentColor(Color.ORANGE, 0.7));

   private AtomicReference<Boolean> showStartPosition = null;
   private AtomicReference<Boolean> showGoalPosition = null;
   private AtomicReference<Boolean> startEditModeEnabled = null;
   private AtomicReference<Boolean> goalEditModeEnabled = null;
   private AtomicReference<Point3D> startPositionReference = null;
   private AtomicReference<Point3D> goalPositionReference = null;

   private final Messager messager;

   public StartGoalPositionViewer(Messager messager)
   {
      this.messager = messager;
      startSphere.setMouseTransparent(true);
      goalSphere.setMouseTransparent(true);

      showStart(true);
      showGoal(true);
   }

   public StartGoalPositionViewer(Messager messager, Topic<Boolean> startEditModeEnabledTopic, Topic<Boolean> goalEditModeEnabledTopic,
                                  Topic<Point3D> startPositionTopic, Topic<Point3D> goalPositionTopic)
   {
      this(messager);
      setEditStartGoalTopics(startEditModeEnabledTopic, goalEditModeEnabledTopic);
      setPositionStartGoalTopics(startPositionTopic, goalPositionTopic);
   }

   public void setPositionStartGoalTopics(Topic<Point3D> startPositionTopic, Topic<Point3D> goalPositionTopic)
   {
      startPositionReference = messager.createInput(startPositionTopic, new Point3D());
      goalPositionReference = messager.createInput(goalPositionTopic, new Point3D());
   }

   public void setEditStartGoalTopics(Topic<Boolean> startEditModeEnabledTopic, Topic<Boolean> goalEditModeEnabledTopic)
   {
      startEditModeEnabled = messager.createInput(startEditModeEnabledTopic, false);
      goalEditModeEnabled = messager.createInput(goalEditModeEnabledTopic, false);
   }

   public void setShowStartGoalTopics(Topic<Boolean> showStartTopic, Topic<Boolean> showGoalTopic)
   {
      showStartPosition = messager.createInput(showStartTopic, true);
      showGoalPosition = messager.createInput(showGoalTopic, true);
   }

   @Override
   public void handle(long now)
   {
      if (showStartPosition != null)
         showStart(showStartPosition.get());

      if (showGoalPosition != null)
         showGoal(showGoalPosition.get());

      if (startEditModeEnabled != null && startEditModeEnabled.get())
         startSphere.setMaterial(startTransparentMaterial);
      else
         startSphere.setMaterial(startOpaqueMaterial);

      if (startPositionReference != null)
      {
         Point3D startPosition = startPositionReference.get();
         if (startPosition != null)
         {
            setStartPosition(startPosition);
         }
      }

      if (goalEditModeEnabled != null && goalEditModeEnabled.get())
         goalSphere.setMaterial(goalTransparentMaterial);
      else
         goalSphere.setMaterial(goalOpaqueMaterial);

      if (goalPositionReference != null)
      {
         Point3D goalPosition = goalPositionReference.get();
         if (goalPosition != null)
         {
            setGoalPosition(goalPosition);
         }
      }
   }

   private void setStartPosition(Point3D position)
   {
      startSphere.setTranslateX(position.getX());
      startSphere.setTranslateY(position.getY());
      startSphere.setTranslateZ(position.getZ());
   }

   private void setGoalPosition(Point3D position)
   {
      goalSphere.setTranslateX(position.getX());
      goalSphere.setTranslateY(position.getY());
      goalSphere.setTranslateZ(position.getZ());
   }

   private void showStart(boolean show)
   {
      if (show)
      {
         if (!isStartCurrentlyShown)
            rootChildren.add(startSphere);
      }
      else
      {
         if (isStartCurrentlyShown)
            rootChildren.remove(startSphere);
      }
      isStartCurrentlyShown = show;
   }

   private void showGoal(boolean show)
   {
      if (show)
      {
         if (!isGoalCurrentlyShown)
            rootChildren.add(goalSphere);
      }
      else
      {
         if (isGoalCurrentlyShown)
            rootChildren.remove(goalSphere);
      }
      isGoalCurrentlyShown = show;
   }

   public static Color toTransparentColor(Color opaqueColor, double opacity)
   {
      double red = opaqueColor.getRed();
      double green = opaqueColor.getGreen();
      double blue = opaqueColor.getBlue();
      return new Color(red, green, blue, opacity);
   }

   public Node getRoot()
   {
      return root;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/AvatarTestGoals.java<|end_filename|>
package us.ihmc.avatar;

import us.ihmc.robotics.testing.YoVariableTestGoal;

public class AvatarTestGoals
{
   public static YoVariableTestGoal notFallen(AvatarTestYoVariables variables)
   {
      return YoVariableTestGoal.deltaGreaterThan(variables.getPelvisZ(), variables.getMidFeetZUpZ(), variables.getDesiredCOMHeight().getDoubleValue() / 2.0);
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/contactPoints/FrictionConeRotationCalculator.java<|end_filename|>
package us.ihmc.wholeBodyController.contactPoints;

import java.util.List;

import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple3D.Vector3D;

public interface FrictionConeRotationCalculator
{
   default public void packVector(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx, double friction,
                                  Vector3D directionToPack)
   {
      double angleOffset = computeYawOffset(contactPoints, contactIdx, vectors, vectorIdx);
      double angle = angleOffset  + 2.0 * Math.PI * vectorIdx / vectors;
      double x = Math.cos(angle) * friction;
      double y = Math.sin(angle) * friction;
      directionToPack.set(x, y, 1.0);
      directionToPack.normalize();
   }

   public abstract double computeYawOffset(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx);
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/modules/mocap/MocapPlanarRegionsListManager.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.modules.mocap;

import controller_msgs.msg.dds.PlanarRegionsListMessage;
import us.ihmc.communication.net.PacketConsumer;

public class MocapPlanarRegionsListManager implements PacketConsumer<PlanarRegionsListMessage>
{
   private PlanarRegionsListMessage latestPlanarRegionsListMessage;
   private PlanarRegionsListMessage beforeWalkingPlanarRegionsListMessage;
   private PlanarRegionsListMessage afterWalkingPlanarRegionsListMessage;
   
   @Override
   public void receivedPacket(PlanarRegionsListMessage packet)
   {         
      this.latestPlanarRegionsListMessage = packet;
   }
   
   public void savePlanarRegionsBeforeWalking()
   {
      beforeWalkingPlanarRegionsListMessage = latestPlanarRegionsListMessage;
   }

   public void savePlanarRegionsAfterWalking()
   {
      afterWalkingPlanarRegionsListMessage = latestPlanarRegionsListMessage;
   }
   
   public PlanarRegionsListMessage getPlanarRegionsBeforeWalking()
   {
      return beforeWalkingPlanarRegionsListMessage;
   }

   public PlanarRegionsListMessage getPlanarRegionsAfterWalking()
   {
      return afterWalkingPlanarRegionsListMessage;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/CapturabilityBasedStatusSubscriber.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import java.util.ArrayList;
import java.util.concurrent.atomic.AtomicReference;

import controller_msgs.msg.dds.CapturabilityBasedStatus;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class CapturabilityBasedStatusSubscriber implements PacketConsumer<CapturabilityBasedStatus>
{
   private final AtomicReference<FramePoint2D> capturePointReference = new AtomicReference<FramePoint2D>(null);
   private final AtomicReference<FramePoint2D> desiredCapturePointReference = new AtomicReference<FramePoint2D>(null);
   private final SideDependentList<AtomicReference<FrameConvexPolygon2D>> footSupportPolygonReferences = new SideDependentList<>();
   private final AtomicReference<Boolean> doubleSupportReference = new AtomicReference<Boolean>(null);

   private ArrayList<CapturabilityBasedStatusListener> listOfListener = new ArrayList<>();

   public CapturabilityBasedStatusSubscriber()
   {
      for (RobotSide robotSide : RobotSide.values)
         footSupportPolygonReferences.put(robotSide, new AtomicReference<FrameConvexPolygon2D>(null));
   }

   public void registerListener(CapturabilityBasedStatusListener listener)
   {
      listOfListener.add(listener);
   }

   public FramePoint2D getCapturePoint()
   {
      return capturePointReference.getAndSet(null);
   }

   public FramePoint2D getDesiredCapturePoint()
   {
      return desiredCapturePointReference.getAndSet(null);
   }

   public FrameConvexPolygon2D getFootSupportPolygon(RobotSide robotSide)
   {
      return footSupportPolygonReferences.get(robotSide).getAndSet(null);
   }

   public Boolean isInDoubleSupport()
   {
      return doubleSupportReference.getAndSet(null);
   }

   @Override
   public void receivedPacket(CapturabilityBasedStatus object)
   {
      if (object == null)
         return;

      capturePointReference.set(new FramePoint2D(ReferenceFrame.getWorldFrame(), object.getCapturePoint2d()));
      desiredCapturePointReference.set(new FramePoint2D(ReferenceFrame.getWorldFrame(), object.getDesiredCapturePoint2d()));
      for (RobotSide robotSide : RobotSide.values)
         footSupportPolygonReferences.get(robotSide).set(HumanoidMessageTools.unpackFootSupportPolygon(object, robotSide));
      doubleSupportReference.set(HumanoidMessageTools.unpackIsInDoubleSupport(object));

      for (CapturabilityBasedStatusListener listener : listOfListener)
      {
         listener.updateStatusPacket(object);
      }
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/behaviors/scripts/engine/ScriptFileSaver.java<|end_filename|>
package us.ihmc.humanoidBehaviors.behaviors.scripts.engine;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.io.ObjectOutputStream;
import java.lang.reflect.Field;
import java.nio.file.Path;
import java.util.ArrayList;

import com.thoughtworks.xstream.XStream;

import us.ihmc.commons.PrintTools;
import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.idl.IDLSequence;
import us.ihmc.pubsub.TopicDataType;

public class ScriptFileSaver
{
   private final FileWriter fileWriter;
   private final ObjectOutputStream outputStream;

   public ScriptFileSaver(Path scriptFilePath, boolean overwriteExistingScript) throws IOException
   {
      this(scriptFilePath.toFile(), overwriteExistingScript);
   }

   public ScriptFileSaver(File file, boolean overwriteExistingScript) throws IOException
   {
      XStream xStream = new XStream();
      if (!file.exists() || overwriteExistingScript)
      {
         file.createNewFile();
      }
      else
      {
         throw new IOException("File already exists");
      }

      fileWriter = new FileWriter(file.getAbsoluteFile());
      outputStream = xStream.createObjectOutputStream(fileWriter);
   }

   /**
    * @deprecated This method is highly inefficient. The whole scripting needs to be redone to better
    *             implement the transform.
    */
   public void recordObject(long timestamp, Object object, RigidBodyTransform objectTransform) throws IOException
   {

      Object objectToWrite;

      try
      {
         objectToWrite = object.getClass().getConstructor(object.getClass()).newInstance(object);
      }
      catch (Exception e)
      {
         try
         {
            objectToWrite = object.getClass().getConstructor().newInstance();
            objectToWrite.getClass().getMethod("set", object.getClass()).invoke(objectToWrite, object);
         }
         catch (Exception e1)
         {
            String className = object.getClass().getSimpleName();
            PrintTools.error("The class: " + className + " needs either a copy constructor: " + className + "(" + className + "), or a setter: " + className
                  + "set(" + className + ").");
            return;
         }
      }

      MessageTransformer.transform(objectToWrite, objectTransform);
      recordObject(timestamp, objectToWrite);
   }

   @SuppressWarnings({"unchecked", "rawtypes"})
   public void recordObject(long timestamp, Object object) throws IOException
   {
      if (object instanceof Packet)
         object = createCopyTrimmedToCurrentSize((Packet) object);
      outputStream.writeLong(timestamp);
      outputStream.writeObject(object);
      outputStream.flush();
   }

   @SuppressWarnings({"unchecked", "rawtypes"})
   public static <T extends Packet<T>> T createCopyTrimmedToCurrentSize(T message)
   {
      if (message == null)
         return null;

      Class<? extends Packet> messageClass = message.getClass();
      T copy;
      try
      {
         copy = (T) messageClass.newInstance();
         copy.set(message);

         for (Field field : messageClass.getDeclaredFields())
         {
            Class<?> fieldType = field.getType();

            if (Packet.class.isAssignableFrom(fieldType))
            {
               field.set(copy, createCopyTrimmedToCurrentSize((Packet) field.get(message)));
            }
            else if (IDLSequence.Object.class.isAssignableFrom(fieldType))
            {
               field.set(copy, trimIDLSequenceObject((IDLSequence.Object) field.get(message)));
            }
         }

         return copy;
      }
      catch (InstantiationException | IllegalAccessException | IllegalArgumentException | NoSuchFieldException | SecurityException e)
      {
         e.printStackTrace();
         return message;
      }
   }

   @SuppressWarnings({"rawtypes", "unchecked"})
   private static <T> IDLSequence.Object<T> trimIDLSequenceObject(IDLSequence.Object<T> list)
         throws IllegalArgumentException, IllegalAccessException, NoSuchFieldException, SecurityException
   {
      TopicDataType<T> topicDataType = list.getTopicDataType();
      Class clazz = topicDataType.createData().getClass();
      IDLSequence.Object<T> trimmed = new IDLSequence.Object<>(list.size(), clazz, topicDataType);
      Field allocatorField = trimmed.getClass().getSuperclass().getDeclaredField("allocator");
      allocatorField.setAccessible(true);
      allocatorField.set(trimmed, null); // The allocator is a lambda that can not be recorded
      Field sizeField = trimmed.getClass().getSuperclass().getDeclaredField("size");
      sizeField.setAccessible(true);
      sizeField.set(trimmed, list.size());
      Field valuesField = trimmed.getClass().getSuperclass().getDeclaredField("values");
      valuesField.setAccessible(true);
      Object[] array = list.toArray();
      if (Packet.class.isAssignableFrom(clazz))
      {
         for (int i = 0; i < array.length; i++)
         {
            array[i] = createCopyTrimmedToCurrentSize((Packet) array[i]);
         }
      }
      valuesField.set(trimmed, array);

      return trimmed;
   }

   public void close()
   {
      try
      {
         outputStream.close();
         fileWriter.close();
      }
      catch (IOException e)
      {
         System.err.println(e);
      }
   }

   public void recordList(ArrayList<ScriptObject> scriptObjects) throws IOException
   {
      for (ScriptObject scriptObject : scriptObjects)
      {
         recordObject(scriptObject.getTimeStamp(), scriptObject.getScriptObject());
      }
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosTf1Publisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import java.util.ArrayList;
import java.util.List;

import org.ros.internal.message.definition.MessageDefinitionReflectionProvider;
import org.ros.internal.message.topic.TopicMessageFactory;
import org.ros.message.MessageDefinitionProvider;
import org.ros.message.Time;

import geometry_msgs.Quaternion;
import geometry_msgs.Transform;
import geometry_msgs.TransformStamped;
import geometry_msgs.Vector3;
import std_msgs.Header;
import tf.tfMessage;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;

public class RosTf1Publisher extends RosTopicPublisher<tf.tfMessage> implements RosTfPublisherInterface
{
   private TopicMessageFactory topicMessageFactory;
   private int seq;
   public RosTf1Publisher(boolean latched)
   {
      super(tf.tfMessage._TYPE, latched);
      MessageDefinitionProvider messageDefinitionProvider = new MessageDefinitionReflectionProvider();
      topicMessageFactory = new TopicMessageFactory(messageDefinitionProvider);
   }
   
   @Override
   public void connected()
   {
   }
   
   private Transform getRosTransform(RigidBodyTransform transform3d)
   {
      Transform transform = topicMessageFactory.newFromType(Transform._TYPE);
      Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE);
      Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE);
      
      us.ihmc.euclid.tuple4D.Quaternion quat4d = new us.ihmc.euclid.tuple4D.Quaternion();
      Vector3D vector3d = new Vector3D();
      transform3d.get(quat4d, vector3d);
      
      rot.setW(quat4d.getS());
      rot.setX(quat4d.getX());
      rot.setY(quat4d.getY());
      rot.setZ(quat4d.getZ());
      
      transform.setRotation(rot);
      
      trans.setX(vector3d.getX());
      trans.setY(vector3d.getY());
      trans.setZ(vector3d.getZ());
      
      transform.setTranslation(trans);
      
      return transform;
   }
   
   public void publish(RigidBodyTransform transform3d, long timeStamp, String parentFrame, String childFrame)
   {
      TransformStamped transformStamped = topicMessageFactory.newFromType(TransformStamped._TYPE);
      Transform transform = getRosTransform(transform3d);
      transformStamped.setTransform(transform);
      transformStamped.setChildFrameId(childFrame);
      Header header = transformStamped.getHeader();
      header.setStamp(Time.fromNano(timeStamp));
      header.setFrameId(parentFrame);
      header.setSeq(seq++);
      transformStamped.setHeader(header);
      
      
      tfMessage message = getMessage();
      
      List<TransformStamped> tfs = new ArrayList<TransformStamped>();
      tfs.add(transformStamped);
      message.setTransforms(tfs);
      publish(message);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ValkyrieHandFingerTrajectoryMessage.java<|end_filename|>
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;

public class ValkyrieHandFingerTrajectoryMessage extends Packet<ValkyrieHandFingerTrajectoryMessage> implements Settable<ValkyrieHandFingerTrajectoryMessage>, EpsilonComparable<ValkyrieHandFingerTrajectoryMessage>
{
   /**
          * This message commands the finger controller to move robot valkyrie finger.
          */
   public static final byte ROBOT_SIDE_LEFT = (byte) 0;
   public static final byte ROBOT_SIDE_RIGHT = (byte) 1;
   public static final byte THUMB_MOTOR_ROLL = (byte) 0;
   public static final byte THUMB_MOTOR_PITCH1 = (byte) 1;
   public static final byte THUMB_MOTOR_PITCH2 = (byte) 2;
   public static final byte INDEX_FINGER_MOTOR_PITCH1 = (byte) 3;
   public static final byte MIDDLE_FINGER_MOTOR_PITCH1 = (byte) 4;
   public static final byte PINKY_MOTOR_PITCH1 = (byte) 5;
   /**
            * 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 trajectory.
            */
   public byte robot_side_ = (byte) 255;
   /**
            * Specifies the list of finger motor names that we want to control.
            */
   public us.ihmc.idl.IDLSequence.Byte  valkyrie_finger_motor_names_;
   /**
            * The indexing for the joints refers the list of finger motor names.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage jointspace_trajectory_;

   public ValkyrieHandFingerTrajectoryMessage()
   {
      valkyrie_finger_motor_names_ = new us.ihmc.idl.IDLSequence.Byte (6, "type_9");

      jointspace_trajectory_ = new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
   }

   public ValkyrieHandFingerTrajectoryMessage(ValkyrieHandFingerTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(ValkyrieHandFingerTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      valkyrie_finger_motor_names_.set(other.valkyrie_finger_motor_names_);
      controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.staticCopy(other.jointspace_trajectory_, jointspace_trajectory_);
   }

   /**
            * 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_;
   }

   /**
            * Specifies the side of the robot that will execute the trajectory.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * Specifies the side of the robot that will execute the trajectory.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }


   /**
            * Specifies the list of finger motor names that we want to control.
            */
   public us.ihmc.idl.IDLSequence.Byte  getValkyrieFingerMotorNames()
   {
      return valkyrie_finger_motor_names_;
   }


   /**
            * The indexing for the joints refers the list of finger motor names.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage getJointspaceTrajectory()
   {
      return jointspace_trajectory_;
   }


   public static Supplier<ValkyrieHandFingerTrajectoryMessagePubSubType> getPubSubType()
   {
      return ValkyrieHandFingerTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ValkyrieHandFingerTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ValkyrieHandFingerTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.valkyrie_finger_motor_names_, other.valkyrie_finger_motor_names_, epsilon)) return false;

      if (!this.jointspace_trajectory_.epsilonEquals(other.jointspace_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ValkyrieHandFingerTrajectoryMessage)) return false;

      ValkyrieHandFingerTrajectoryMessage otherMyClass = (ValkyrieHandFingerTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if (!this.valkyrie_finger_motor_names_.equals(otherMyClass.valkyrie_finger_motor_names_)) return false;
      if (!this.jointspace_trajectory_.equals(otherMyClass.jointspace_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ValkyrieHandFingerTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("valkyrie_finger_motor_names=");
      builder.append(this.valkyrie_finger_motor_names_);      builder.append(", ");
      builder.append("jointspace_trajectory=");
      builder.append(this.jointspace_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/environments/DRCFinalsStartingLocation.java<|end_filename|>
package us.ihmc.avatar.environments;

import java.util.HashMap;

import us.ihmc.avatar.DRCStartingLocation;
import us.ihmc.avatar.initialSetup.OffsetAndYawRobotInitialSetup;

public enum DRCFinalsStartingLocation implements DRCStartingLocation
{
   DEFAULT;

   public static final HashMap<String, OffsetAndYawRobotInitialSetup> initialSetupMap = new HashMap<String, OffsetAndYawRobotInitialSetup>();

   static
   {
      addMapping(DRCFinalsStartingLocation.DEFAULT, new OffsetAndYawRobotInitialSetup(0.4, 1.3, 0.0, -Math.PI / 2));
   }

   private static void addMapping(DRCFinalsStartingLocation drcDemo01StartingLocation, OffsetAndYawRobotInitialSetup robotInitialSetup)
   {
      initialSetupMap.put(drcDemo01StartingLocation.toString(), robotInitialSetup);
   }

   @Override
   public OffsetAndYawRobotInitialSetup getStartingLocationOffset()
   {
      OffsetAndYawRobotInitialSetup startingLocation = initialSetupMap.get(this.toString());
      return startingLocation;
   }
   
   public static String optionsToString()
   {
      String ret = "[ ";
      for (DRCFinalsStartingLocation location : DRCFinalsStartingLocation.values())
      {
         ret = ret + location.toString() + ", ";
      }
      ret = ret + "]";
      return ret;
   }
   
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/server/RemoteSimulationRunner.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.server;

import java.net.InetAddress;
import java.rmi.Naming;
import java.rmi.RMISecurityManager;
import java.rmi.RemoteException;
import java.rmi.registry.LocateRegistry;
import java.rmi.registry.Registry;
import java.rmi.server.UnicastRemoteObject;

import us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces.RemoteSimulationDescription;
import us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces.RemoteSimulationRunnerInterface;


public class RemoteSimulationRunner extends UnicastRemoteObject implements RemoteSimulationRunnerInterface
{
   private static final long serialVersionUID = 9053419694765471403L;

   private static boolean DEBUG = false;

   private static final boolean SET_CODEBASE = true;    // true;

   // String codebase = System.getProperty("java.rmi.server.codebase");
   // if (codebase == "null"); codebase = "";

   // String codebase = "http://www.coginst.uwf.edu/~jpratt/SimulationDispatcher/codebase/simulationdispatcherinterfaces.jar http://www.coginst.uwf.edu/~jpratt/SimulationDispatcher/codebase/simulationdispatcherserver.jar";
// String codebase = "http://www.ihmc.us/~jpratt/SimulationDispatcher/codebase/simulationdispatcherinterfaces.jar http://www.ihmc.us/~jpratt/SimulationDispatcher/codebase/simulationdispatcherserver.jar";
// String codebase = ""; //http://www.ihmc.us/~jpratt/SimulationDispatcher/codebase/simulationdispatcherinterfaces.jar http://www.ihmc.us/~jpratt/SimulationDispatcher/codebase/simulationdispatcherserver.jar";

//   private final static String codebase =
//      "file://halo/Public/Projects/SimulationDispatcher/SimulationDispatcherInterfaces.jar file://halo/Public/Projects/SimulationDispatcher/SimulationDispatcherClient.jar file://halo/Public/Projects/SimulationDispatcher/SimulationDispatcherServer.jar";


   private final static String codebase = "https://bengal.ihmc.us/svn/SimulationDispatcher/SimulationDispatcherInterfaces/SimulationDispatcherInterfaces.jar "
                                          + "https://bengal.ihmc.us/svn/SimulationDispatcher/SimulationDispatcherClient/SimulationDispatcherClient.jar "
                                          + "https://bengal.ihmc.us/svn/SimulationDispatcher/SimulationDispatcherServer/SimulationDispatcherServer.jar";

   private RemoteSimulationDescription description;
   private static final String password = "***<PASSWORD>***";

   public RemoteSimulationRunner() throws RemoteException
   {
      super();
   }

   @Override
   public int add(int a, int b)
   {
      return a + b;
   }

   @Override
   public void createSimulation(RemoteSimulationDescription description, String[] structuralParameterNames, double[] structuralParameterValues, String password)
           throws RemoteException
   {
      if (DEBUG)
         System.out.println("Creating Simulation!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");
      this.description = description;
      description.createSimulation(structuralParameterNames, structuralParameterValues);
   }

   @Override
   public void destroySimulation(String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Destroy Simulation!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");
      if (description != null)
         description.destroySimulation();
      description = null;
   }

   @Override
   public boolean ping(String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Ping!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");

      return true;
   }

   @Override
   public void setSimulationState(Object state, String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Set Simulation State!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");
      description.setSimulationState(state);
   }

   @Override
   public void startSimulation(String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Starting Simulation!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");
      description.startSimulation();
   }

   @Override
   public boolean isSimulationDone(String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Checking isSimulationDone!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");
      if (description == null)
         return true;

      return description.isSimulationDone();
   }

   @Override
   public Object getSimulationState(String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Getting Simulation State!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");

      return description.getSimulationState();
   }

   @Override
   public Object getSimulationData(String password) throws RemoteException
   {
      if (DEBUG)
         System.out.println("Getting Simulation Data!");

      if (!password.equals(RemoteSimulationRunner.password))
         throw new RemoteException("Invalid Password");

      return description.getSimulationData();
   }

   public static void main(String[] args)
   {
      int numberOfServers = 8;    // 16; //8;

      if ((args != null) && (args.length > 0) && (args[0] != null))
      {
         numberOfServers = Integer.parseInt(args[0]);
      }

      RMISecurityManager laxManager = new RMISecurityManager()
      {
         @Override
         public void checkPermission(java.security.Permission p)
         {
            // System.out.println("In checkPermission. permission = " + p);System.out.flush();

            // if (p.toString().equals("(java.lang.reflect.ReflectPermission suppressAccessChecks)")) return;
            // if (p.toString().equals("(java.util.PropertyPermission java.rmi.server.codebase write)")) return;

            // System.out.println(p);
            // try
            // {super.checkPermission(p);}
            // catch(Exception e)
            {
               // System.out.println(p);
            }

            // super.checkPermission(p);
         }

         /*
          * public void checkPropertiesAccess(){System.out.println("In checkPropertiesAccess");return;}
          * public void checkPropertyAccess(String key)
          * {
          * System.out.println("In checkPropertyAccess.  Key = " + key);
          * System.out.flush();
          * return;
          * }
          *
          * public void checkLink(String lib)
          * {
          * System.out.println("In checkPropertyAccess.  Lib = " + lib);
          * System.out.flush();
          * return;
          * }
          *
          * public void checkRead(String file)
          * {
          * System.out.println("In checkRead(Sting file).  file = " + file);
          * System.out.flush();
          * return;
          * }
          *
          * public void checkRead(String file, Object context)
          * {
          * System.out.println("In checkRead(Sting file, Object context).  file = " + file);
          * System.out.flush();
          * return;
          * }
          * public void checkRead(java.io.FileDescriptor fd)
          * {
          * System.out.println("In checkRead(FileDescriptor fd)." );
          * System.out.flush();
          * return;
          * }
          *
          * public void checkAccess(Thread t)
          * {
          * System.out.println("In checkAccess(Thread t)." );
          * System.out.flush();
          * return;
          * }
          *
          * public void checkAccess(ThreadGroup g)
          * {
          * System.out.println("In checkAccess(ThreadGroup g)." );
          * System.out.flush();
          * return;
          * }
          *
          * public void checkConnect(String host, int port)
          * {
          * System.out.println("In checkConnect(String host, int port). host = " + host + " port = " + port);
          * System.out.flush();
          * return;
          * }
          *
          * public void checkConnect(String host, int port, Object context)
          * {
          * System.out.println("In checkConnect(String host, int port, Object context). host = " + host + " port = " + port);
          * System.out.flush();
          * return;
          * }
          *
          * public void checkWrite(String file)
          * {
          * System.out.println("In checkWrite(String file). file = " + file);
          * System.out.flush();
          * return;
          * }
          *
          * public void checkWrite(java.io.FileDescriptor fd)
          * {
          * System.out.println("In checkWrite(FileDescriptor fd). ");
          * System.out.flush();
          * return;
          * }
          */
      };

      // laxManager.checkWrite(

      System.setSecurityManager(laxManager);    // new RMISecurityManager());


      try
      {
         System.out.println("Creating RMI Registry.");
         Registry registry = LocateRegistry.createRegistry(1099);    // Start an RMI Registry
         System.out.println("RMI Registry " + registry + " Created.");
      }
      catch (RemoteException e)
      {
         System.out.println("RMI Registry allready exists!");

         // e.printStackTrace();
         // return;
      }


      if (SET_CODEBASE)
      {
         System.out.println("Setting the RMI codebase to " + codebase);
         System.setProperty("java.rmi.server.codebase", codebase);
      }


      for (int i = 0; i < numberOfServers; i++)
      {
         try
         {
            String runnerName = "RemoteSimulationRunner" + i;
            System.out.println("Creating and Binding " + runnerName);

            RemoteSimulationRunner rs = new RemoteSimulationRunner();
            Naming.rebind(runnerName, rs);
         }
         catch (Exception e)
         {
            System.out.println("RemoteSimulation Exception: " + e.getMessage());
            e.printStackTrace();

            return;
         }
      }

      System.out.println(numberOfServers + " RemoteSimulationRunners are now bound.");    // Time to tell lynx.coginst.uwf.edu about me...");
      InetAddress address;

      try
      {
         address = InetAddress.getLocalHost();
         System.out.println(address);
      }
      catch (java.net.UnknownHostException e)
      {
         System.out.println("Could not find this computer's address");

         return;
      }

//    while(true)
//    {
//      try
//      {
//        //RemoteIPRegistryInterface registry = (RemoteIPRegistryInterface) Naming.lookup("//lynx.coginst.uwf.edu/RemoteIPRegistry");
//        RemoteIPRegistryInterface registry = (RemoteIPRegistryInterface) Naming.lookup("//jaguar.ihmc.us/RemoteIPRegistry");
//        //RemoteIPRegistryInterface registry = (RemoteIPRegistryInterface) Naming.lookup("//lynx/RemoteIPRegistry");
//        registry.registerMe(address.toString(), RemoteSimulationRunner.password);
//
//        //java.io.net.
//        System.out.println("Waiting to Serve up Remote Simulations");
//
//        try{Thread.sleep(120000);} //0);} // sleep for two minutes...
//        catch(InterruptedException e){}
//      }
//      catch(Exception e)
//      {
//        //System.out.println("Could not find Lynx to tell about me...");
//        //e.printStackTrace();
//        try{Thread.sleep(10000);} //0);} // sleep for ten seconds...
//        catch(InterruptedException e2){}
//      }
//
//    }
   }


}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/NodeData.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

public class NodeData
{
   public double[] q;
   private int dimension;

   public NodeData(int dimension)
   {
      this.q = new double[dimension];
      this.dimension = dimension;
   }

   public NodeData(NodeData nodeData)
   {
      this.q = new double[nodeData.getDimension()];
      for (int i = 0; i < nodeData.getDimension(); i++)
      {
         this.q[i] = nodeData.getQ(i);
      }
      this.dimension = nodeData.getDimension();
   }

   public final int getDimension()
   {
      return dimension;
   }

   public final double getQ(int index)
   {
      return q[index];
   }

   public final void setQ(int index, double value)
   {
      this.q[index] = value;
   }

   public final double distance(NodeData nodeData)
   {
      double ret = 0;
      for (int i = 0; i < dimension; i++)
      {
         ret = ret + (nodeData.getQ(i) - this.getQ(i)) * (nodeData.getQ(i) - this.getQ(i));
      }
      ret = Math.sqrt(ret);

      return ret;
   }
}

<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsPlanningToolboxModule/SolutionQualityConvergenceSettings.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.kinematicsPlanningToolboxModule;

public interface SolutionQualityConvergenceSettings
{
   default boolean useConvergenceDetecting()
   {
      return true;
   }

   default double getSolutionQualityThreshold()
   {
      return 0.005;
   }

   default double getSolutionStabilityThreshold()
   {
      return 0.00002;
   }

   default double getMinimumProgression()
   {
      return 0.005;
   }

   default int getDefaultTerminalIteration()
   {
      return 50;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/DesiredVelocityProvider.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator;

import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;

/**
 * This interface is part of the API of {@link ContinuousStepGenerator}.
 * <p>
 * It is used to provide a protocol for obtaining the current desired walking forward/lateral
 * velocity.
 * </p>
 * 
 * @author <NAME>
 */
public interface DesiredVelocityProvider
{
   /**
    * Gets the current desired forward/lateral velocity.
    * <p>
    * A positive x value corresponds to walking forward, a positive y value corresponds to walking
    * to the left.
    * </p>
    * 
    * @return the desired forward/lateral velocity.
    */
   Vector2DReadOnly getDesiredVelocity();
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/manipulation/AtlasElectricMotorPacketEnum.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.manipulation;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;
import us.ihmc.robotics.robotSide.RobotSide;

/**
 * Enum for the various joints in the Atlas forearm. Note the joint names correspond to their
 * orientation in the canonical 'zero pose' where the arms are held out perpendicular to the
 * sides of the robot.
*/
public enum AtlasElectricMotorPacketEnum
{
   @RosEnumValueDocumentation(documentation = "left wrist roll")
   L_ARM_WRY(0),
   @RosEnumValueDocumentation(documentation = "left upper wrist pitch")
   L_ARM_WRX(1),
   @RosEnumValueDocumentation(documentation = "left lower wrist pitch")
   L_ARM_WRY2(2),
   @RosEnumValueDocumentation(documentation = "right wrist roll")
   R_ARM_WRY(3),
   @RosEnumValueDocumentation(documentation = "right upper wrist pitch")
   R_ARM_WRX(4),
   @RosEnumValueDocumentation(documentation = "right lower wrist pitch")
   R_ARM_WRY2(5);

   public int id;

   AtlasElectricMotorPacketEnum(int id)
   {
      this.id = id;
   }

   public int getId()
   {
      return id;
   }

   public static final AtlasElectricMotorPacketEnum[] values = values();

   public RobotSide getRobotSide()
   {
      switch(this)
      {
      case L_ARM_WRX:
      case L_ARM_WRY:
      case L_ARM_WRY2:
         return RobotSide.LEFT;
         
      case R_ARM_WRX:
      case R_ARM_WRY:
      case R_ARM_WRY2:
         return RobotSide.RIGHT;
         
      default:
         return null;
      }
   }

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static AtlasElectricMotorPacketEnum fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/TurningOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import us.ihmc.euclid.referenceFrame.FrameOrientation2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.commons.MathTools;

public class TurningOverheadPath extends OverheadPath
{
   private final FrameOrientation2D startOrientation;
   private final FrameOrientation2D endOrientation;
   private final double deltaYaw;
   private final FramePoint2D point;

   public TurningOverheadPath(FramePose2D startPose, FrameOrientation2D endOrientation)
   {
      startPose.checkReferenceFrameMatch(endOrientation);
      this.startOrientation = new FrameOrientation2D(startPose.getOrientation());
      this.endOrientation = new FrameOrientation2D(endOrientation);
      this.point = new FramePoint2D(startPose.getPosition());
      this.deltaYaw = endOrientation.difference(startOrientation);
   }

   private FrameOrientation2D tempOrientation;

   public double getDeltaYaw()
   {
      return deltaYaw;//signed
   }

   @Override
   public FramePose2D getPoseAtS(double pathVariableS)
   {
      pathVariableS = MathTools.clamp(pathVariableS, 0.0, 1.0);
      return getExtrapolatedPoseAtS(pathVariableS);
   }

   @Override
   public FramePose2D getExtrapolatedPoseAtS(double pathVariableS)
   {
      if (tempOrientation == null)
         tempOrientation = new FrameOrientation2D(point.getReferenceFrame());
      tempOrientation.interpolate(startOrientation, endOrientation, pathVariableS);
      return new FramePose2D(point, tempOrientation);
   }

   @Override
   public ReferenceFrame getReferenceFrame()
   {
      return point.getReferenceFrame();
   }

   @Override
   public TurningOverheadPath changeFrameCopy(ReferenceFrame desiredFrame)
   {
      FramePoint2D newStartPoint = new FramePoint2D(point);
      newStartPoint.changeFrame(desiredFrame);
      FrameOrientation2D newStartOrientation = new FrameOrientation2D(startOrientation);
      newStartOrientation.changeFrame(desiredFrame);
      FramePose2D newStartPose = new FramePose2D(newStartPoint, newStartOrientation);
      FrameOrientation2D newEndOrientation = new FrameOrientation2D(endOrientation);
      newEndOrientation.changeFrame(desiredFrame);
      return new TurningOverheadPath(newStartPose, newEndOrientation);
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/SelectableObject.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;


public interface SelectableObject
{
   public void select();
   public void unSelect(boolean reset);
   public void addSelectedListeners(SelectableObjectListener selectedListener);
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/DoNothingControllerState.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates;

import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters;
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;

public class DoNothingControllerState extends HighLevelControllerState
{
   private static final HighLevelControllerName controllerState = HighLevelControllerName.DO_NOTHING_BEHAVIOR;
   private final LowLevelOneDoFJointDesiredDataHolder lowLevelOneDoFJointDesiredDataHolder;

   public DoNothingControllerState(OneDoFJointBasics[] controlledJoints, HighLevelControllerParameters highLevelControllerParameters)
   {
      super(controllerState, highLevelControllerParameters, controlledJoints);
      lowLevelOneDoFJointDesiredDataHolder = new LowLevelOneDoFJointDesiredDataHolder(controlledJoints.length);
      lowLevelOneDoFJointDesiredDataHolder.registerJointsWithEmptyData(controlledJoints);
   }

   @Override
   public void doAction(double timeInState)
   {
      for (int i = 0; i < controlledJoints.length; i++)
      {
         controlledJoints[i].setTau(0.0);
         lowLevelOneDoFJointDesiredDataHolder.getJointDesiredOutput(controlledJoints[i]).clear();
         lowLevelOneDoFJointDesiredDataHolder.setDesiredJointTorque(controlledJoints[i], 0.0);
         lowLevelOneDoFJointDesiredDataHolder.setDesiredJointPosition(controlledJoints[i], controlledJoints[i].getQ());
         lowLevelOneDoFJointDesiredDataHolder.setDesiredJointVelocity(controlledJoints[i], controlledJoints[i].getQd());
      }

      lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings());
   }

   @Override
   public void onEntry()
   {
      // Do nothing

   }

   @Override
   public void onExit()
   {
      // Do nothing

   }

   @Override
   public JointDesiredOutputListReadOnly getOutputForLowLevelController()
   {
      return lowLevelOneDoFJointDesiredDataHolder;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/JointPrivilegedConfigurationParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

/**
 * {@code JointPrivilegedConfigurationParameters} gathers the main parameters needed to compute a
 * secondary objective based on a provided privileged configuration.
 * <p>
 * Depending on whether the controller core is used in inverse dynamics or inverse kinematics mode,
 * it will computed a set of desired joint accelerations/velocities to bring the robot to a desired
 * privileged configuration.
 * </p>
 * <p>
 * This new jointspace objective is then projected into the nullspace of the principal objectives
 * already submitted by the high-level control. As a result, the privileged configuration objective
 * will have only a little affect onto the main objectives.
 * </p>
 */
public class JointPrivilegedConfigurationParameters
{
   public JointPrivilegedConfigurationParameters()
   {
   }

   /**
    * To compute the nullspace of the high-level control objectives, a matrix inversion has to be
    * calculated. To improve stability, a damped-least square pseudo-inverse is used, which in turn
    * uses this alpha parameter.
    * <p>
    * It should preferably be close to zero.
    * </p>
    * 
    * @return the parameter used for computing the damped-least squares pseudo-inverse.
    */
   public double getNullspaceProjectionAlpha()
   {
      return 0.005;
   }

   /**
    * <p>
    * Returns the desired default configuration gain for the joint privileged configuration handler.
    * </p>
    * <p>
    * This gain is used to track the privileged configuration when no other gain is provided.
    * </p>
    * 
    * @return configuration gain
    */
   public double getDefaultConfigurationGain()
   {
      return 40.0;
   }

   /**
    * <p>
    * Returns the desired default velocity gain for the joint privileged configuration handler.
    * </p>
    * <p>
    * This gain is used to damp the velocities when no other velocity gain is provided.
    * </p>
    * <p>
    * Note that when computing privileged velocities, this value is not used.
    * </p>
    * 
    * @return velocity gain
    */
   public double getDefaultVelocityGain()
   {
      return 6.0;
   }

   /**
    * <p>
    * Returns the default maximum velocity for the joint privileged configuration handler.
    * </p>
    * <p>
    * This limits the maximum velocity when computing privileged velocity commands that will be
    * allowed to attempt to reach the privileged configuration.
    * </p>
    * 
    * @return max joint velocity (rad / s)
    */
   public double getDefaultMaxVelocity()
   {
      return 2.0;
   }

   /**
    * <p>
    * Returns the default maximum acceleration for the joint privileged configuration handler.
    * </p>
    * <p>
    * This limits the maximum acceleration when computing privileged acceleration commands that will
    * be allowed to attempt to reach the privileged configuration.
    * </p>
    * 
    * @return max joint acceleration (rad / s^2)
    */
   public double getDefaultMaxAcceleration()
   {
      return Double.POSITIVE_INFINITY;
   }

   /**
    * <p>
    * Returns the default weight for the joint privileged configuration handler.
    * </p>
    * <p>
    * This is the weight placed on the privileged configuration command in the nullspace of the task
    * Jacobian
    * </p>
    * 
    * @return privileged command weight
    */
   public double getDefaultWeight()
   {
      return 5.0;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/properties/ConcaveHullFactoryParametersProperty.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.properties;

import javafx.beans.property.Property;
import us.ihmc.robotEnvironmentAwareness.geometry.ConcaveHullFactoryParameters;

public class ConcaveHullFactoryParametersProperty extends ParametersProperty<ConcaveHullFactoryParameters>
{
   private final DoubleField edgeLengthThreshold = new DoubleField(ConcaveHullFactoryParameters::getEdgeLengthThreshold, (p, v) -> p.setEdgeLengthThreshold(v));
   private final BooleanField removeAllTrianglesWithTwoBorderEdges = new BooleanField(ConcaveHullFactoryParameters::doRemoveAllTrianglesWithTwoBorderEdges, (p, v) -> p.setRemoveAllTrianglesWithTwoBorderEdges(v));
   private final BooleanField allowSplittingConcaveHull = new BooleanField(ConcaveHullFactoryParameters::isSplittingConcaveHullAllowed, (p, v) -> p.setAllowSplittingConcaveHull(v));
   private final IntegerField maxNumberOfIterations = new IntegerField(ConcaveHullFactoryParameters::getMaxNumberOfIterations, (p, v) -> p.setMaxNumberOfIterations(v));

   public ConcaveHullFactoryParametersProperty(Object bean, String name)
   {
      super(bean, name, new ConcaveHullFactoryParameters());
   }

   public void bindBidirectionalEdgeLengthThreshold(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, edgeLengthThreshold);
   }
   
   public void bindBidirectionalRemoveAllTrianglesWithTwoBorderEdges(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, removeAllTrianglesWithTwoBorderEdges);
   }
   
   public void bindBidirectionalAllowSplittingConcaveHull(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, allowSplittingConcaveHull);
   }
   
   public void bindBidirectionalMaxNumberOfIterations(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxNumberOfIterations);
   }

   @Override
   protected ConcaveHullFactoryParameters getValueCopy(ConcaveHullFactoryParameters valueToCopy)
   {
      return new ConcaveHullFactoryParameters(valueToCopy);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/setterUpper/StateMachineJPanelGuiSetterUpper.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.setterUpper;

import java.awt.Container;

import javax.swing.BoxLayout;
import javax.swing.JFrame;

import us.ihmc.robotics.stateMachine.core.State;
import us.ihmc.robotics.stateMachine.core.StateMachine;
import us.ihmc.robotics.stateMachine.extra.StateMachinesJPanel;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class StateMachineJPanelGuiSetterUpper<K extends Enum<K>> implements GUISetterUpper
{
   private final StateMachine<K, ? extends State> stateMachine;
   private final boolean inJFrame;
   public StateMachineJPanelGuiSetterUpper(StateMachine<K, ? extends State> stateMachine, boolean inJFrame)
   {
      this.stateMachine = stateMachine;
      this.inJFrame = inJFrame;
   }

   @Override
   public void setupGUI(SimulationConstructionSet scs)
   {
      StateMachinesJPanel<K> walkingStatePanel = new StateMachinesJPanel<>(stateMachine);
      String name = "State Machine";
      if (inJFrame)
      {
         JFrame jFrame = new JFrame(name);
         Container contentPane = jFrame.getContentPane();
         contentPane.setLayout(new BoxLayout(contentPane, BoxLayout.X_AXIS));

         //ScrollPane pane = new ScrollPane();
         //pane.add(walkingStatePanel);
         jFrame.getContentPane().add(walkingStatePanel);
         jFrame.pack();
         jFrame.setSize(450, 300);
         jFrame.setAlwaysOnTop(true);
         jFrame.setVisible(true);
      }
      else
      {
         scs.addExtraJpanel(walkingStatePanel, name, false);         
      }

      // Doing the following will cause redraw when the state changes, but not
      // during replay or rewind:
      stateMachine.addStateChangedListener(walkingStatePanel);
   }

}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ICPControlGains.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.capturePoint;

import us.ihmc.robotics.controllers.pidGains.implementations.IntegratorGains;

public class ICPControlGains extends IntegratorGains implements ICPControlGainsReadOnly
{
   private double kpParallelToMotion;
   private double kpOrthogonalToMotion;

   private double feedbackPartMaxRate = Double.POSITIVE_INFINITY;

   private double feedbackPartMaxValueParallelToMotion = Double.POSITIVE_INFINITY;
   private double feedbackPartMaxValueOrthogonalToMotion = Double.POSITIVE_INFINITY;

   @Override
   public double getKpParallelToMotion()
   {
      return kpParallelToMotion;
   }

   public void setKpParallelToMotion(double kpParallelToMotion)
   {
      this.kpParallelToMotion = kpParallelToMotion;
   }

   @Override
   public double getKpOrthogonalToMotion()
   {
      return kpOrthogonalToMotion;
   }

   public void setKpOrthogonalToMotion(double kpOrthogonalToMotion)
   {
      this.kpOrthogonalToMotion = kpOrthogonalToMotion;
   }

   @Override
   public double getFeedbackPartMaxRate()
   {
      return feedbackPartMaxRate;
   }

   public void setFeedbackPartMaxRate(double feedbackPartMaxRate)
   {
      this.feedbackPartMaxRate = feedbackPartMaxRate;
   }

   @Override
   public double getFeedbackPartMaxValueParallelToMotion()
   {
      return feedbackPartMaxValueParallelToMotion;
   }

   public void setFeedbackPartMaxValueParallelToMotion(double feedbackPartMaxValueParallelToMotion)
   {
      this.feedbackPartMaxValueParallelToMotion = feedbackPartMaxValueParallelToMotion;
   }

   @Override
   public double getFeedbackPartMaxValueOrthogonalToMotion()
   {
      return feedbackPartMaxValueOrthogonalToMotion;
   }

   public void setFeedbackPartMaxValueOrthogonalToMotion(double feedbackPartMaxValueOrthogonalToMotion)
   {
      this.feedbackPartMaxValueOrthogonalToMotion = feedbackPartMaxValueOrthogonalToMotion;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/eventBasedStateMachine/FiniteStateMachineCallback.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.eventBasedStateMachine;

/**
 * Defines a callback for a given state as triggered by an event. Note a callback does not necessarily result in a transition.
 *
 * @param <E> the event enum type.
 */
@Deprecated
public class FiniteStateMachineCallback<S extends Enum<S>, E extends Enum<E>>
{
   /**
    * The event on which to trigger.
    */
   private final E event;

   /**
    * The state on which to trigger.
    */
   private final S state;

   /**
    * The callback function.
    */
   private final Runnable callback;

   public FiniteStateMachineCallback(E event, S state, Runnable callback)
   {
      this.event = event;
      this.state = state;
      this.callback = callback;
   }

   /**
    * {@see #event}
    */
   public E getEvent()
   {
      return event;
   }

   /**
    * {@see #state}
    */
   public S getState()
   {
      return state;
   }

   /**
    * {@see #function}
    */
   public void call()
   {
      this.callback.run();
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/ChestOrientationTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.ChestTrajectoryMessage;
import us.ihmc.euclid.referenceFrame.FrameQuaternion;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.humanoidBehaviors.behaviors.primitives.ChestTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;

public class ChestOrientationTask extends BehaviorAction
{
   private final ChestTrajectoryMessage chestOrientationPacket;
   private final ChestTrajectoryBehavior chestOrientationBehavior;

   public ChestOrientationTask(ChestTrajectoryMessage chestTrajectoryMessage, ChestTrajectoryBehavior chestOrientationBehavior)
   {
      super(chestOrientationBehavior);
      this.chestOrientationBehavior = chestOrientationBehavior;
      this.chestOrientationPacket = chestTrajectoryMessage;
   }

   public ChestOrientationTask(FrameQuaternion desiredChestOrientation, ChestTrajectoryBehavior chestOrientationBehavior, double trajectoryTime,
                               ReferenceFrame trajectoryFrame)
   {
      super(chestOrientationBehavior);
      this.chestOrientationBehavior = chestOrientationBehavior;
      desiredChestOrientation.changeFrame(trajectoryFrame);
      Quaternion chestOrientation = new Quaternion(desiredChestOrientation);
      chestOrientationPacket = HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, chestOrientation, trajectoryFrame);
   }

   @Override
   protected void setBehaviorInput()
   {
      chestOrientationBehavior.setInput(chestOrientationPacket);
   }
}

<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/processManagement/FuzzyStringClassLauncher.java<|end_filename|>
package us.ihmc.tools.processManagement;

import java.awt.AWTException;
import java.awt.Component;
import java.awt.GridBagConstraints;
import java.awt.GridBagLayout;
import java.awt.Insets;
import java.awt.SystemTray;
import java.awt.TrayIcon;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.io.File;
import java.io.IOException;
import java.lang.reflect.Method;
import java.net.URL;
import java.util.ArrayList;
import java.util.Enumeration;
import java.util.HashMap;

import javax.swing.BorderFactory;
import javax.swing.BoxLayout;
import javax.swing.ImageIcon;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JMenuItem;
import javax.swing.JPanel;
import javax.swing.JPopupMenu;
import javax.swing.JTextField;
import javax.swing.JTree;
import javax.swing.SwingUtilities;
import javax.swing.event.DocumentEvent;
import javax.swing.event.DocumentListener;
import javax.swing.tree.DefaultMutableTreeNode;
import javax.swing.tree.DefaultTreeCellRenderer;

import org.apache.lucene.analysis.standard.StandardAnalyzer;
import org.apache.lucene.document.Document;
import org.apache.lucene.document.Field;
import org.apache.lucene.index.DirectoryReader;
import org.apache.lucene.index.IndexWriter;
import org.apache.lucene.index.IndexWriterConfig;
import org.apache.lucene.queryparser.classic.ParseException;
import org.apache.lucene.queryparser.classic.QueryParser;
import org.apache.lucene.search.IndexSearcher;
import org.apache.lucene.search.Query;
import org.apache.lucene.search.ScoreDoc;
import org.apache.lucene.search.TopScoreDocCollector;
import org.apache.lucene.store.Directory;
import org.apache.lucene.store.RAMDirectory;
import org.apache.lucene.util.Version;

/**
 * Because FuzzyStringLauncher utilizes JavaProcessSpawner under the hood, which in turn
 * works its magic by inheriting its classpath from wherever it is invoked, you should
 * "embed" the FuzzyStringClassLauncher somewhere that has dependencies on other projects
 * and jars containing the main methods that you'll most likely want to invoke.
 * 
 * An example would be subclassing, or even just putting an instance of the launcher inside
 * of another class as a field.  An example is the subclass DRCFuzzyStringClassLauncher, which
 * is in the DRC project.  This has dependencies on most of our other projects and so it has a huge
 * classpath, meaning most of our main classes can be launched from that instance of the launcher.
 * 
 * @author dstephen
 */

public class FuzzyStringClassLauncher
{
   private HashMap<String, Class<?>> classList = new HashMap<String, Class<?>>();

   private JFrame frame;
   private JPanel searchPanel;
   private JPanel resultsPanel;
   private JTextField searchField = new JTextField();

   private QueryParser queryParser;
   private IndexSearcher searcher;

   private TopScoreDocCollector collector;

   private String name;

   private ImageIcon javaIcon = new ImageIcon(FuzzyStringClassLauncher.class.getClassLoader().getResource("java.png"));

   public FuzzyStringClassLauncher(String name)
   {
      this.name = name;

      System.out.println("Indexing classes...");
      initClassList();
      System.out.println("Indexing complete.");
      try
      {
         initLucene();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }
   }

   public void start()
   {
      SwingUtilities.invokeLater(new Runnable()
      {
         @Override
         public void run()
         {
            setupJFrame();
            //            showJFrame();
         }
      });
   }

   private void setupJFrame()
   {
      frame = new JFrame(name);
      searchPanel = new JPanel(new GridBagLayout());
      searchPanel.setBorder(BorderFactory.createEmptyBorder());

      GridBagConstraints c = new GridBagConstraints();
      Insets oldInsets = c.insets;

      c.gridx = 0;
      c.gridy = 0;
      c.anchor = GridBagConstraints.PAGE_START;
      c.ipadx = 500;
      c.weightx = 0;
      c.insets = new Insets(30, 30, 30, 30);
      c.fill = GridBagConstraints.HORIZONTAL;

      searchField = new JTextField();
      searchField.setColumns(5);
      setupSearchFieldListener();

      searchPanel.add(searchField, c);

      c.insets = oldInsets;

      resultsPanel = new JPanel(new GridBagLayout());
      resultsPanel.setBorder(BorderFactory.createEmptyBorder());
      resultsPanel.setVisible(false);

      frame.getContentPane().setLayout(new BoxLayout(frame.getContentPane(), BoxLayout.Y_AXIS));
      frame.getContentPane().add(searchPanel);
      frame.getContentPane().add(resultsPanel);

      SystemTray tray = SystemTray.getSystemTray();
      TrayIcon icon = new TrayIcon(javaIcon.getImage());

      final JPopupMenu menu = new JPopupMenu("Fuzzy String Class Launcher");
      JMenuItem item = new JMenuItem("Quit");
      menu.add(item);

      item.addActionListener(new ActionListener()
      {
         @Override
         public void actionPerformed(ActionEvent e)
         {
            System.exit(0);
         }
      });      

      icon.addMouseListener(new MouseListener()
      {

         @Override
         public void mouseReleased(MouseEvent e)
         {

         }

         @Override
         public void mousePressed(MouseEvent e)
         {

         }

         @Override
         public void mouseExited(MouseEvent e)
         {

         }

         @Override
         public void mouseEntered(MouseEvent e)
         {

         }

         @Override
         public void mouseClicked(final MouseEvent e)
         {
            if (SwingUtilities.isRightMouseButton(e))
            {
               menu.setLocation(e.getX(), 0);
               menu.setInvoker(menu);
               menu.setVisible(true);
            }
            else if (SwingUtilities.isLeftMouseButton(e))
            {
               if (frame.isVisible())
               {
                  searchField.setText("");
                  frame.setVisible(false);
               }
               else
               {
                  SwingUtilities.invokeLater(new Runnable()
                  {
                     @Override
                     public void run()
                     {
                        frame.setLocation(e.getX() - frame.getWidth() / 2, 10);
                        frame.setVisible(true);                        
                     }
                  });
               }
            }
         }
      });

      try
      {
         tray.add(icon);
      }
      catch (AWTException e1)
      {
         e1.printStackTrace();
      }

//      frame.setAutoRequestFocus(true);
      frame.setUndecorated(true);
      frame.pack();
      frame.setResizable(false);
      frame.setDefaultCloseOperation(JFrame.HIDE_ON_CLOSE);
      frame.setLocationRelativeTo(null);
      frame.setAlwaysOnTop(true);
   }
   
   private void setupSearchFieldListener()
   {
      searchField.getDocument().addDocumentListener(new DocumentListener()
      {
         @Override
         public void removeUpdate(DocumentEvent e)
         {
            if (searchField.getText().length() == 0)
            {
               disposeSearchResults();
            }
            else
            {
               updateSearchResults();
            }
         }

         @Override
         public void insertUpdate(DocumentEvent e)
         {
            if (searchField.getText().length() == 0)
            {
               disposeSearchResults();
            }
            else
            {
               updateSearchResults();
            }
         }

         @Override
         public void changedUpdate(DocumentEvent e)
         {
            if (searchField.getText().length() == 0)
            {
               disposeSearchResults();
            }
            else
            {
               updateSearchResults();
            }
         }
      });
   }

   private void initClassList()
   {
      ArrayList<File> dirs = new ArrayList<File>();
      ArrayList<String> allClasses = new ArrayList<String>();
      try
      {
         getSubdirsFromRoots(dirs);

         getAllClasses(dirs, allClasses);
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }

      populateMainClassListFromClasses(allClasses);
   }

   private void populateMainClassListFromClasses(ArrayList<String> allClasses)
   {
      for (String s : allClasses)
      {
         try
         {
            Class<?> clazz = Class.forName(s, false, this.getClass().getClassLoader());

            for (Method m : clazz.getMethods())
            {
               if (m.getName().equals("main"))
               {
                  classList.put(clazz.getSimpleName(), clazz);
               }
            }
         }
         catch (ClassNotFoundException e)
         {
            e.printStackTrace();
         }
      }
   }

   private void getAllClasses(ArrayList<File> dirs, ArrayList<String> classList) throws IOException
   {
      final String binString = "bin" + File.separator;
      final String classesString = "classes" + File.separator;
      final String mainString = "main" + File.separator;
      final String testString = "test" + File.separator;

      for (File dir : dirs)
      {
         for (File f : dir.listFiles())
         {
            String fileName = f.getCanonicalFile().getAbsolutePath();
            if (fileName.contains(binString))
            {
               fileName = fileName.substring(fileName.indexOf(binString) +binString.length());
            }
            if (fileName.contains(classesString))
            {
               fileName = fileName.substring(fileName.indexOf(classesString) + classesString.length());
            }
            if(fileName.startsWith(mainString))
            {
               fileName = fileName.substring(fileName.indexOf(mainString) + mainString.length());
            }
            else if(fileName.startsWith(testString))
            {
               fileName = fileName.substring(fileName.indexOf(testString) + testString.length());
            }

            fileName = fileName.replace(File.separator, ".");

            if (!f.isDirectory() && !fileName.contains("$") && fileName.endsWith(".class"))
            {
               classList.add(fileName.substring(0, fileName.indexOf(".class")));
            }
         }
      }
   }

   private void getSubdirsFromRoots(ArrayList<File> dirs) throws IOException
   {
      Enumeration<URL> roots = this.getClass().getClassLoader().getResources("");
      while (roots.hasMoreElements())
      {
         String nextPath = roots.nextElement().getPath();
         dirs.add(new File(nextPath));
         getAllSubDirs(nextPath, dirs);
      }
   }

   private void getAllSubDirs(String path, ArrayList<File> dirs) throws IOException
   {
      File root = null;
      root = new File(path).getCanonicalFile();

      for (File f : root.listFiles())
      {
         if (f.isDirectory())
         {
            dirs.add(f);
            getAllSubDirs(f.getCanonicalPath(), dirs);
         }
      }
   }

   private void initLucene() throws IOException
   {
      StandardAnalyzer analyzer = new StandardAnalyzer(Version.LUCENE_43);
      Directory index = new RAMDirectory();

      IndexWriterConfig config = new IndexWriterConfig(Version.LUCENE_43, analyzer);
      IndexWriter writer = new IndexWriter(index, config);

      for (String s : classList.keySet())
      {
         Document doc = new Document();
         doc.add(new org.apache.lucene.document.TextField("classname", s, Field.Store.YES));
         writer.addDocument(doc);
      }

      writer.close();

      queryParser = new QueryParser(Version.LUCENE_43, "classname", analyzer);
      searcher = new IndexSearcher(DirectoryReader.open(index));
   }

   private void updateSearchResults()
   {
      collector = TopScoreDocCollector.create(15, true);
      resultsPanel.removeAll();
      final JTree tree = new JTree(new DefaultMutableTreeNode());
      tree.setRowHeight(0);
      tree.setCellRenderer(new DefaultTreeCellRenderer()
      {
         private static final long serialVersionUID = 1L;

         @Override
         public Component getTreeCellRendererComponent(JTree tree, Object value, boolean sel, boolean expanded, boolean leaf, int row, boolean hasFocus)
         {
            JLabel label = (JLabel) super.getTreeCellRendererComponent(tree, value, sel, expanded, leaf, row, hasFocus);
            label.setBorder(BorderFactory.createEmptyBorder((row == 1 ? 0 : 10), 10, 10, 20));
            return label;
         }
      });
      tree.setShowsRootHandles(false);
      ((DefaultTreeCellRenderer) tree.getCellRenderer()).setOpenIcon(null);
      ((DefaultTreeCellRenderer) tree.getCellRenderer()).setClosedIcon(null);
      ((DefaultTreeCellRenderer) tree.getCellRenderer()).setLeafIcon(javaIcon);
      tree.addMouseListener(new MouseListener()
      {

         @Override
         public void mouseReleased(MouseEvent e)
         {

         }

         @Override
         public void mousePressed(MouseEvent e)
         {

         }

         @Override
         public void mouseExited(MouseEvent e)
         {

         }

         @Override
         public void mouseEntered(MouseEvent e)
         {

         }

         @Override
         public void mouseClicked(MouseEvent e)
         {
            if (e.getClickCount() > 1)
            {
               String str = tree.getSelectionPath().getLastPathComponent().toString();
               if (str != null && !str.toLowerCase().contains("no results"))
               {
                  str = str.substring(str.indexOf("\">") + 2, str.indexOf("</"));
                  new JavaProcessSpawner(false, true).spawn(classList.get(str), new String[] { "-Xms2048m", "-Xmx2048m" }, null);
               }
            }
         }
      });
      try
      {
         String searchString = searchField.getText().substring(0, 1) + searchField.getText().substring(1, searchField.getText().length()).replace("", "*");
         Query q = queryParser.parse(searchString + "~");
         searcher.search(q, collector);

         ScoreDoc[] hits = collector.topDocs().scoreDocs;

         for (ScoreDoc hit : hits)
         {
            Document d = searcher.doc(hit.doc);
            DefaultMutableTreeNode newNode = new DefaultMutableTreeNode("<html><body><span style=\"font-weight:bold;font-size:1.2em;\">" + d.get("classname")
                  + "</span></body></html>");
            ((DefaultMutableTreeNode) tree.getModel().getRoot()).add(newNode);
         }
         if (hits.length == 0)
         {
            DefaultMutableTreeNode newNode = new DefaultMutableTreeNode(
                  "<html><body><span style=\"font-weight:bold;font-size:1.2em;\">No Results</span></body></html>");
            ((DefaultMutableTreeNode) tree.getModel().getRoot()).add(newNode);
         }
         GridBagConstraints c = new GridBagConstraints();
         tree.setBorder(BorderFactory.createEtchedBorder());
         tree.expandRow(0);
         c.insets = new Insets(10, 10, 10, 10);
         //         c.ipadx = 100;
         c.fill = GridBagConstraints.BOTH;
         c.weightx = 1;
         resultsPanel.add(tree, c);
         resultsPanel.validate();
         resultsPanel.repaint();
         resultsPanel.setVisible(true);
         frame.pack();
         frame.validate();
         frame.repaint();
      }
      catch (ParseException e)
      {
         e.printStackTrace();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }

   }

   private void disposeSearchResults()
   {
      resultsPanel.setVisible(false);
      frame.pack();
      frame.validate();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/groundTruthinator/GroundTruthinatorSensor.java<|end_filename|>
package us.ihmc.exampleSimulations.groundTruthinator;

import us.ihmc.euclid.tuple3D.Point3D;

public class GroundTruthinatorSensor
{
   private final Point3D sensorPositionInWorldFrame = new Point3D();
   private final Point3D attachmentPositionInRobotFrame = new Point3D();
   private double sensedCableLength, estimatedCableLength;
   private double sensedCableVelocity, estimatedCableVelocity;


   public GroundTruthinatorSensor(Point3D sensorPosition, Point3D attachmentPosition)
   {
      sensorPositionInWorldFrame.set(sensorPosition);
      attachmentPositionInRobotFrame.set(attachmentPosition);
   }

   public void setSensorPositionInWorldFrame(Point3D sensorPositionInWorld)
   {
      this.sensorPositionInWorldFrame.set(sensorPositionInWorld);
   }

   public void setAttachmentPositionInRobotFrame(Point3D attachmentPositionInRobotFrame)
   {
      this.attachmentPositionInRobotFrame.set(attachmentPositionInRobotFrame);
   }

   public void setSensedCableLength(double cableLength)
   {
      this.sensedCableLength = cableLength;
   }

   public void setEstimatedCableLength(double cableLength)
   {
      this.estimatedCableLength = cableLength;
   }

   public void setSensedCableVelocity(double cableVelocity)
   {
      this.sensedCableVelocity = cableVelocity;
   }

   public void setEstimatedCableVelocity(double cableVelocity)
   {
      this.estimatedCableVelocity = cableVelocity;
   }

   public void getSensorPositionInWorldFrame(Point3D sensorPositionInWorldToPack)
   {
      sensorPositionInWorldToPack.set(sensorPositionInWorldFrame);
   }

   public void getAttachmentPositionInRobotFrame(Point3D attachmentPositionInRobotFrameToPack)
   {
      attachmentPositionInRobotFrameToPack.set(attachmentPositionInRobotFrame);
   }

   public double getSensedCableLength()
   {
      return sensedCableLength;
   }

   public double getEstimatedCableLength()
   {
      return estimatedCableLength;
   }

   public double getSensedCableVelocity()
   {
      return sensedCableVelocity;
   }

   public double getEstimatedCableVelocity()
   {
      return estimatedCableVelocity;
   }


}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AutomaticManipulationAbortMessage.java<|end_filename|>
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.
       * Request the controller to enable the automatic manipulation abort feature.
       * When enabled, any arm trajectory will get aborted as soon as the balance controller has a large tracking error.
       */
public class AutomaticManipulationAbortMessage extends Packet<AutomaticManipulationAbortMessage> implements Settable<AutomaticManipulationAbortMessage>, EpsilonComparable<AutomaticManipulationAbortMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean enable_;

   public AutomaticManipulationAbortMessage()
   {
   }

   public AutomaticManipulationAbortMessage(AutomaticManipulationAbortMessage other)
   {
      this();
      set(other);
   }

   public void set(AutomaticManipulationAbortMessage other)
   {
      sequence_id_ = other.sequence_id_;

      enable_ = other.enable_;

   }

   /**
            * 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 void setEnable(boolean enable)
   {
      enable_ = enable;
   }
   public boolean getEnable()
   {
      return enable_;
   }


   public static Supplier<AutomaticManipulationAbortMessagePubSubType> getPubSubType()
   {
      return AutomaticManipulationAbortMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AutomaticManipulationAbortMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AutomaticManipulationAbortMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_, other.enable_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AutomaticManipulationAbortMessage)) return false;

      AutomaticManipulationAbortMessage otherMyClass = (AutomaticManipulationAbortMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.enable_ != otherMyClass.enable_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AutomaticManipulationAbortMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("enable=");
      builder.append(this.enable_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/AtlasFlatGroundWalkingTrack.java<|end_filename|>
package us.ihmc.atlas;

import com.martiansoftware.jsap.JSAPException;

import us.ihmc.avatar.DRCFlatGroundWalkingTrack;
import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.avatar.initialSetup.DRCGuiInitialSetup;
import us.ihmc.avatar.initialSetup.DRCRobotInitialSetup;
import us.ihmc.avatar.initialSetup.DRCSCSInitialSetup;
import us.ihmc.jMonkeyEngineToolkit.GroundProfile3D;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.simulationConstructionSetTools.util.HumanoidFloatingRootJointRobot;
import us.ihmc.simulationToolkit.controllers.OscillateFeetPerturber;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.util.ground.BumpyGroundProfile;
import us.ihmc.simulationconstructionset.util.ground.FlatGroundProfile;

public class AtlasFlatGroundWalkingTrack
{
   private static final DRCRobotModel defaultModelForGraphicSelector = new AtlasRobotModel(AtlasRobotVersion.ATLAS_UNPLUGGED_V5_NO_HANDS, RobotTarget.SCS, false);

   private static final boolean USE_BUMPY_GROUND = false;
   private static final boolean USE_FEET_PERTURBER = false;

   public static void main(String[] args) throws JSAPException
   {

      DRCRobotModel model = null;
      model = AtlasRobotModelFactory.selectSimulationModelFromFlag(args);

      if (model == null)
         model = AtlasRobotModelFactory.selectModelFromGraphicSelector(defaultModelForGraphicSelector);

      if (model == null)
         throw new RuntimeException("No robot model selected");

      DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false);

      final double groundHeight = 0.0;
      GroundProfile3D groundProfile;
      if (USE_BUMPY_GROUND)
      {
         groundProfile = createBumpyGroundProfile();
      }
      else
      {
         groundProfile = new FlatGroundProfile(groundHeight);
      }

      DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, model.getSimulateDT());
      scsInitialSetup.setDrawGroundProfile(true);
      scsInitialSetup.setInitializeEstimatorToActual(true);

      double initialYaw = 0.3;
      DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = model.getDefaultRobotInitialSetup(groundHeight, initialYaw);

      boolean useVelocityAndHeadingScript = true;
      boolean cheatWithGroundHeightAtForFootstep = false;

      DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup,
            useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, model);

      if (USE_FEET_PERTURBER)
         createOscillateFeetPerturber(drcFlatGroundWalkingTrack);
   }

   private static void createOscillateFeetPerturber(DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack)
   {
      SimulationConstructionSet simulationConstructionSet = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
      HumanoidFloatingRootJointRobot robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot();

      int ticksPerPerturbation = 10;
      OscillateFeetPerturber oscillateFeetPerturber = new OscillateFeetPerturber(robot, simulationConstructionSet.getDT() * ((double) ticksPerPerturbation));
      oscillateFeetPerturber.setTranslationMagnitude(new double[] { 0.01, 0.015, 0.005 });
      oscillateFeetPerturber.setRotationMagnitudeYawPitchRoll(new double[] { 0.017, 0.012, 0.011 });

      oscillateFeetPerturber.setTranslationFrequencyHz(RobotSide.LEFT, new double[] { 0.0, 0, 3.3 });
      oscillateFeetPerturber.setTranslationFrequencyHz(RobotSide.RIGHT, new double[] { 0.0, 0, 1.3 });

      oscillateFeetPerturber.setRotationFrequencyHzYawPitchRoll(RobotSide.LEFT, new double[] { 0.0, 0, 7.3 });
      oscillateFeetPerturber.setRotationFrequencyHzYawPitchRoll(RobotSide.RIGHT, new double[] { 0., 0, 1.11 });

      robot.setController(oscillateFeetPerturber, ticksPerPerturbation);
   }

   private static BumpyGroundProfile createBumpyGroundProfile()
   {
      double xAmp1 = 0.05, xFreq1 = 0.5, xAmp2 = 0.01, xFreq2 = 0.5;
      double yAmp1 = 0.01, yFreq1 = 0.07, yAmp2 = 0.05, yFreq2 = 0.37;
      BumpyGroundProfile groundProfile = new BumpyGroundProfile(xAmp1, xFreq1, xAmp2, xFreq2, yAmp1, yFreq1, yAmp2, yFreq2);
      return groundProfile;
   }
}


<|start_filename|>ihmc-ros-tools/generated-src/ihmc_msgs/HandDesiredConfigurationRosMessage.java<|end_filename|>
package ihmc_msgs;

public interface HandDesiredConfigurationRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_msgs/HandDesiredConfigurationRosMessage";
  static final java.lang.String _DEFINITION = "## HandDesiredConfigurationRosMessage\n# Packet for commanding the hands to perform various predefined grasps. A message with a unique id\n# equals to 0 will be interpreted as invalid and will not be processed by the controller.\n\n# Specifies the side of the robot that will execute the trajectory\nint8 robot_side\n\n# Specifies the grasp to perform\nint8 desired_hand_configuration\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  byte getRobotSide();
  void setRobotSide(byte value);
  byte getDesiredHandConfiguration();
  void setDesiredHandConfiguration(byte value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/PositionController.java<|end_filename|>
package us.ihmc.robotics.controllers;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;

/**
 * @author twan
 *         Date: 6/15/13
 */
public interface PositionController
{
   public abstract void compute(FrameVector3D output, FramePoint3D desiredPosition, FrameVector3D desiredVelocity, FrameVector3D currentVelocity,
                FrameVector3D feedForward);

   public abstract ReferenceFrame getBodyFrame();
}


<|start_filename|>simulation-construction-set-tools/src/test/java/us/ihmc/simulationConstructionSetTools/simulationTesting/SimpleRewindabilityComparisonScript.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationTesting;

import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class SimpleRewindabilityComparisonScript implements SimulationComparisonScript
{
   private final boolean DEBUG = false;
   
   private final int nTicksInitial;
   private final int nTicksCompare;
   private final int nTicksFinal;

   public SimpleRewindabilityComparisonScript(int nTicksInitial, int nTicksCompare, int nTicksFinal)
   {
      this.nTicksInitial = nTicksInitial;
      this.nTicksCompare = nTicksCompare;
      this.nTicksFinal = nTicksFinal;
   }

   @Override
   public void doInitialAction(SimulationConstructionSet scs0, SimulationConstructionSet scs1)
   {
      if (scs0.getRecordFreq() != scs1.getRecordFreq())
      {
         throw new RuntimeException("scs0.getRecordFreq() != scs1.getRecordFreq()");
      }
      
      if (nTicksInitial % scs0.getRecordFreq() != 0)
      {
         throw new RuntimeException("nTicksInitial must be divisible by record frequency!");
      }
      
      SimulationConstructionSet[] scss = {scs0, scs1};

      for (SimulationConstructionSet scs : scss)
      {
         scs.simulate(nTicksInitial);
      }
      for (SimulationConstructionSet scs : scss)
      {
         waitForSimulationToFinish(scs);
      }
   }

   @Override
   public void doFinalAction(SimulationConstructionSet scs0, SimulationConstructionSet scs1)
   {
      if (scs0.getRecordFreq() != scs1.getRecordFreq())
      {
         throw new RuntimeException("scs0.getRecordFreq() != scs1.getRecordFreq()");
      }
      
      if (nTicksCompare % scs0.getRecordFreq() != 0)
      {
         throw new RuntimeException("nTicksCompare must be divisible by record frequency!");
      }
      
      if (nTicksFinal % scs0.getRecordFreq() != 0)
      {
         throw new RuntimeException("nTicksFinal must be divisible by record frequency!");
      }
      
      SimulationConstructionSet[] scss = {scs0, scs1};
      
      if (DEBUG) System.err.println("Setting In Points");
      for (SimulationConstructionSet scs : scss)
      {
         scs.setInPoint();
      }

      if (DEBUG) System.err.println("Ticking Simulation 0");

      scs0.simulate(nTicksCompare);
      waitForSimulationToFinish(scs0);


      int index = 0;
      for (SimulationConstructionSet scs : scss)
      {
         if (DEBUG) System.err.println("Going to InPoint And Simulating on " + index++);

         scs.gotoInPointNow();
         scs.simulate(nTicksFinal);
         waitForSimulationToFinish(scs);
      }

   }


   private void waitForSimulationToFinish(SimulationConstructionSet scs)
   {
      while (scs.isSimulating())
      {
         try
         {
            Thread.sleep(10);
         }
         catch (InterruptedException e)
         {
            e.printStackTrace();
         }
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PelvisPoseErrorPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "PelvisPoseErrorPacket" defined in "PelvisPoseErrorPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from PelvisPoseErrorPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit PelvisPoseErrorPacket_.idl instead.
*
*/
public class PelvisPoseErrorPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.PelvisPoseErrorPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::PelvisPoseErrorPacket_";

   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.PelvisPoseErrorPacket 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.PelvisPoseErrorPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.PelvisPoseErrorPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.PelvisPoseErrorPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.PelvisPoseErrorPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_5(data.getResidualError());

      cdr.write_type_5(data.getTotalError());

      cdr.write_type_7(data.getHasMapBeenReset());

   }

   public static void read(controller_msgs.msg.dds.PelvisPoseErrorPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setResidualError(cdr.read_type_5());
      	
      data.setTotalError(cdr.read_type_5());
      	
      data.setHasMapBeenReset(cdr.read_type_7());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.PelvisPoseErrorPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_5("residual_error", data.getResidualError());
      ser.write_type_5("total_error", data.getTotalError());
      ser.write_type_7("has_map_been_reset", data.getHasMapBeenReset());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.PelvisPoseErrorPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setResidualError(ser.read_type_5("residual_error"));
      data.setTotalError(ser.read_type_5("total_error"));
      data.setHasMapBeenReset(ser.read_type_7("has_map_been_reset"));
   }

   public static void staticCopy(controller_msgs.msg.dds.PelvisPoseErrorPacket src, controller_msgs.msg.dds.PelvisPoseErrorPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.PelvisPoseErrorPacket createData()
   {
      return new controller_msgs.msg.dds.PelvisPoseErrorPacket();
   }
   @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.PelvisPoseErrorPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.PelvisPoseErrorPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.PelvisPoseErrorPacket src, controller_msgs.msg.dds.PelvisPoseErrorPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public PelvisPoseErrorPacketPubSubType newInstance()
   {
      return new PelvisPoseErrorPacketPubSubType();
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/contactPoints/InteriorAngleFrictionConeCalculator.java<|end_filename|>
package us.ihmc.wholeBodyController.contactPoints;

import java.util.List;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;

public class InteriorAngleFrictionConeCalculator implements FrictionConeRotationCalculator
{
   private final ConvexPolygon2D supportPolygon = new ConvexPolygon2D();

   @Override
   public double computeYawOffset(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx)
   {
      supportPolygon.set(Vertex2DSupplier.asVertex2DSupplier(contactPoints));
      Point2D contactPoint = contactPoints.get(contactIdx);
      int vertexIndex = supportPolygon.getClosestVertexIndex(contactPoint);

      double angleOfEdge = getAngleOfEdgeAfterPoint(vertexIndex);
      double interiorAngle = getInteriorAngle(vertexIndex);
      return angleOfEdge - 0.5 * interiorAngle;
   }

   private final LineSegment2D edge1 = new LineSegment2D();
   private final LineSegment2D edge2 = new LineSegment2D();
   private final Vector2D edgeDirection1 = new Vector2D();
   private final Vector2D edgeDirection2 = new Vector2D();

   public double getInteriorAngle(int vertexIndex)
   {
      supportPolygon.getEdge(supportPolygon.getPreviousVertexIndex(vertexIndex), edge1);
      supportPolygon.getEdge(vertexIndex, edge2);
      edgeDirection1.sub(edge1.getSecondEndpoint(), edge1.getFirstEndpoint());
      edgeDirection2.sub(edge2.getSecondEndpoint(), edge2.getFirstEndpoint());
      double angle = edgeDirection2.angle(edgeDirection1);
      double dotProduct = edgeDirection2.dot(edgeDirection1);
      if (dotProduct > 0.0)
      {
         angle = Math.PI - angle;
      }
      return angle;
   }

   private final LineSegment2D edge = new LineSegment2D();
   private final Vector2D edgeDirection = new Vector2D();
   private final Vector2D xAxis = new Vector2D();

   public double getAngleOfEdgeAfterPoint(int vertexIndex)
   {
      supportPolygon.getEdge(vertexIndex, edge);
      edgeDirection.sub(edge.getSecondEndpoint(), edge.getFirstEndpoint());
      xAxis.set(1.0, 0.0);
      return xAxis.angle(edgeDirection);
   }
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/fingers/ValkyrieHandFingerTrajectoryMessageConversion.java<|end_filename|>
package us.ihmc.valkyrie.fingers;

import controller_msgs.msg.dds.HandDesiredConfigurationMessage;
import controller_msgs.msg.dds.ValkyrieHandFingerTrajectoryMessage;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HandConfiguration;
import us.ihmc.robotics.robotSide.RobotSide;

/**
 * This class converts desired HandConfiguration into a ValkyrieHandFingerTrajectoryMessage.
 * Finger controller will stop finger motors which of trajectory is not specified.
 */

public class ValkyrieHandFingerTrajectoryMessageConversion
{
   /*
    * TODO Somehow make it YoDoubles again.
    * The fingers seem to be pretty slow, so kinda pointless reducing this one.
    * In addition, the comms to the fingers seems flaky, going faster increases
    * the chances to trigger a limit fault.
    */
   public static double trajectoryTime = 5.0;
   public static double extendedTimeRatioForThumb = 1.25; // Making sure the thumb does not crush the finger when closing.
   public static double basicGripThumbRollClosingTime = 1.0; // TODO.

   public static double trajectoryTimeForSim = 0.5;

   public static final void convertHandDesiredConfigurationMessage(HandDesiredConfigurationMessage handDesiredConfigurationMessage,
                                                                   ValkyrieHandFingerTrajectoryMessage messageToPack)
   {
      messageToPack.setRobotSide(handDesiredConfigurationMessage.getRobotSide());
      HandConfiguration desiredHandConfiguration = HandConfiguration.fromByte(handDesiredConfigurationMessage.getDesiredHandConfiguration());
      convertHandConfiguration(desiredHandConfiguration, messageToPack);
   }

   public static final void convertHandConfiguration(RobotSide robotSide, HandConfiguration desiredHandConfiguration,
                                                     ValkyrieHandFingerTrajectoryMessage messageToPack)
   {
      messageToPack.setRobotSide(robotSide.toByte());
      convertHandConfiguration(desiredHandConfiguration, messageToPack);
   }

   public static final void convertHandConfiguration(HandConfiguration desiredHandConfiguration, ValkyrieHandFingerTrajectoryMessage messageToPack)
   {
      ValkyrieFingerMotorName[] valkyrieFingerMotorNames = null;
      double[] trajectoryTimes = null;
      double[] desiredJointPositions = null;

      switch (desiredHandConfiguration)
      {
      case STOP:
         break;

      case OPEN:
         valkyrieFingerMotorNames = new ValkyrieFingerMotorName[ValkyrieFingerMotorName.values.length];
         valkyrieFingerMotorNames[0] = ValkyrieFingerMotorName.ThumbMotorRoll;
         valkyrieFingerMotorNames[1] = ValkyrieFingerMotorName.ThumbMotorPitch1;
         valkyrieFingerMotorNames[2] = ValkyrieFingerMotorName.ThumbMotorPitch2;
         valkyrieFingerMotorNames[3] = ValkyrieFingerMotorName.IndexFingerMotorPitch1;
         valkyrieFingerMotorNames[4] = ValkyrieFingerMotorName.MiddleFingerMotorPitch1;
         valkyrieFingerMotorNames[5] = ValkyrieFingerMotorName.PinkyMotorPitch1;

         trajectoryTimes = new double[] {trajectoryTime, trajectoryTime, trajectoryTime, trajectoryTime * extendedTimeRatioForThumb,
               trajectoryTime * extendedTimeRatioForThumb, trajectoryTime * extendedTimeRatioForThumb};
         desiredJointPositions = new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
         break;

      case CLOSE:
         valkyrieFingerMotorNames = new ValkyrieFingerMotorName[ValkyrieFingerMotorName.values.length];
         valkyrieFingerMotorNames[0] = ValkyrieFingerMotorName.ThumbMotorRoll;
         valkyrieFingerMotorNames[1] = ValkyrieFingerMotorName.ThumbMotorPitch1;
         valkyrieFingerMotorNames[2] = ValkyrieFingerMotorName.ThumbMotorPitch2;
         valkyrieFingerMotorNames[3] = ValkyrieFingerMotorName.IndexFingerMotorPitch1;
         valkyrieFingerMotorNames[4] = ValkyrieFingerMotorName.MiddleFingerMotorPitch1;
         valkyrieFingerMotorNames[5] = ValkyrieFingerMotorName.PinkyMotorPitch1;

         trajectoryTimes = new double[] {trajectoryTime * extendedTimeRatioForThumb, trajectoryTime * extendedTimeRatioForThumb,
               trajectoryTime * extendedTimeRatioForThumb, trajectoryTime, trajectoryTime, trajectoryTime};
         desiredJointPositions = new double[] {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
         break;

      case OPEN_FINGERS:
         valkyrieFingerMotorNames = new ValkyrieFingerMotorName[3];
         valkyrieFingerMotorNames[0] = ValkyrieFingerMotorName.IndexFingerMotorPitch1;
         valkyrieFingerMotorNames[1] = ValkyrieFingerMotorName.MiddleFingerMotorPitch1;
         valkyrieFingerMotorNames[2] = ValkyrieFingerMotorName.PinkyMotorPitch1;

         trajectoryTimes = new double[] {trajectoryTime, trajectoryTime, trajectoryTime};
         desiredJointPositions = new double[] {0.0, 0.0, 0.0};
         break;

      case OPEN_THUMB:
         valkyrieFingerMotorNames = new ValkyrieFingerMotorName[3];
         valkyrieFingerMotorNames[0] = ValkyrieFingerMotorName.ThumbMotorRoll;
         valkyrieFingerMotorNames[1] = ValkyrieFingerMotorName.ThumbMotorPitch1;
         valkyrieFingerMotorNames[2] = ValkyrieFingerMotorName.ThumbMotorPitch2;

         trajectoryTimes = new double[] {trajectoryTime, trajectoryTime, trajectoryTime};
         desiredJointPositions = new double[] {0.0, 0.0, 0.0};
         break;

      case CLOSE_FINGERS:
         valkyrieFingerMotorNames = new ValkyrieFingerMotorName[3];
         valkyrieFingerMotorNames[0] = ValkyrieFingerMotorName.IndexFingerMotorPitch1;
         valkyrieFingerMotorNames[1] = ValkyrieFingerMotorName.MiddleFingerMotorPitch1;
         valkyrieFingerMotorNames[2] = ValkyrieFingerMotorName.PinkyMotorPitch1;

         trajectoryTimes = new double[] {trajectoryTime, trajectoryTime, trajectoryTime};
         desiredJointPositions = new double[] {1.0, 1.0, 1.0};
         break;

      case CLOSE_THUMB:
         valkyrieFingerMotorNames = new ValkyrieFingerMotorName[3];
         valkyrieFingerMotorNames[0] = ValkyrieFingerMotorName.ThumbMotorRoll;
         valkyrieFingerMotorNames[1] = ValkyrieFingerMotorName.ThumbMotorPitch1;
         valkyrieFingerMotorNames[2] = ValkyrieFingerMotorName.ThumbMotorPitch2;

         trajectoryTimes = new double[] {trajectoryTime, trajectoryTime, trajectoryTime};
         desiredJointPositions = new double[] {1.0, 1.0, 1.0};
         break;

      default:
         throw new IllegalArgumentException("Message conversion for the desired HandConfiguration is not implemented");
      }

      if (valkyrieFingerMotorNames != null)
      {
         int dimension = valkyrieFingerMotorNames.length;
         messageToPack.getJointspaceTrajectory().set(HumanoidMessageTools.createJointspaceTrajectoryMessage(trajectoryTimes, desiredJointPositions));
         for (int i = 0; i < dimension; i++)
         {
            messageToPack.getValkyrieFingerMotorNames().add(valkyrieFingerMotorNames[i].toByte());
         }
      }
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/KryoObjectCommunicator.java<|end_filename|>
package us.ihmc.communication.net;

import java.awt.Container;
import java.io.IOException;
import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.atomic.AtomicBoolean;

import javax.swing.JFrame;
import javax.swing.JScrollPane;
import javax.swing.JTable;
import javax.swing.table.DefaultTableModel;

import com.esotericsoftware.kryonet.Connection;
import com.esotericsoftware.kryonet.EndPoint;
import com.esotericsoftware.kryonet.FrameworkMessage.KeepAlive;
import com.esotericsoftware.kryonet.Listener;

import us.ihmc.commons.FormattingTools;
import us.ihmc.commons.thread.ThreadTools;

public abstract class KryoObjectCommunicator implements NetworkedObjectCommunicator
{
   private final AtomicBoolean throwExceptionForUnregisteredPackets = new AtomicBoolean(true);
   private final LinkedHashMap<Class<?>, ExecutorService> listenerExecutors = new LinkedHashMap<Class<?>, ExecutorService>();
   private final LinkedHashMap<Class<?>, ArrayList<ObjectConsumer<?>>> listeners = new LinkedHashMap<Class<?>, ArrayList<ObjectConsumer<?>>>();

   private final ArrayList<TcpNetStateListener> tcpStateListeners = new ArrayList<TcpNetStateListener>();
   private final ArrayList<ConnectionStateListener> stateListeners = new ArrayList<ConnectionStateListener>();
   
   private final ArrayList<GlobalObjectConsumer> globalListeners = new ArrayList<GlobalObjectConsumer>();
   
   // Stuff for data count table 
   private DefaultTableModel dataRateTable;
   private LinkedHashMap<Class<?>, TableData> dataCounts;
   private long startTime = 0;
   
   
   public void showBandwidthDialog(String name)
   {
      if(dataRateTable == null)
      {
         dataCounts = new LinkedHashMap<Class<?>, TableData>();
         JFrame jFrame = new JFrame(name);
         Container contentPane = jFrame.getContentPane();
         
         dataRateTable = new DefaultTableModel(new Object[]{"Object", "Total bits", "bps"}, 0);
         
         int rowNumber = 0;
         for(Class<?> clazz : listeners.keySet())
         {
            dataCounts.put(clazz, new TableData(rowNumber));
            dataRateTable.addRow(new Object[] { clazz.getSimpleName(), 0, 0 });
            rowNumber++;
         }
         
         JTable table = new JTable(dataRateTable);
         JScrollPane scrollPane = new JScrollPane(table);
         contentPane.add(scrollPane);
         
         
         jFrame.pack();
         jFrame.setSize(400, 500);
         jFrame.setLocationByPlatform(true);
         jFrame.setVisible(true);
      }
   }
   
   protected void registerClassList(NetClassList classList)
   {
	   if(!listeners.containsKey(Object.class))
	   {
		   listeners.put(Object.class, new ArrayList<ObjectConsumer<?>>());
	   }
	   
	   for(Class<?> clazz : classList.getPacketClassList())
	   {
		   listeners.put(clazz, new ArrayList<ObjectConsumer<?>>());
		   listenerExecutors.put(clazz, Executors.newFixedThreadPool(1, ThreadTools.getNamedThreadFactory("Kryo" + clazz.getSimpleName() + "Listener")));
	   }
   }
   
   @Override
   public void attachStateListener(ConnectionStateListener stateListener)
   {
      stateListeners.add(stateListener);
   }

   @Override
   public void attachStateListener(TcpNetStateListener stateListener)
   {
      tcpStateListeners.add(stateListener);
   }
   
   @Override
   public <T> void attachListener(Class<T> clazz, ObjectConsumer<T> listener)
   {
      if(listeners.containsKey(clazz))
      {
         listeners.get(clazz).add(listener);
      }
      else
      {
         throw new RuntimeException("Class " + clazz.getSimpleName() + " is not registered with ObjectCommunicator");
      }
   }
   
   @Override
   public <T> void detachListener(Class<T> clazz, ObjectConsumer<T> listener)
   {
      if(listeners.containsKey(clazz))
      {
         ArrayList<ObjectConsumer<?>> listenerList = listeners.get(clazz);
         if (listenerList.contains(listener))
         {
            listenerList.remove(listener);
         }
      }
   }
   
   @Override
   public void attachGlobalListener(GlobalObjectConsumer listener)
   {
	   globalListeners.add(listener);
   }
   
   @Override
   public void detachGlobalListener(GlobalObjectConsumer listener)
   {
      if (globalListeners.contains(listener))
      {
         globalListeners.remove(listener);
      }
   }
   
   @Override
   public synchronized void consumeObject(Object object)
   {
      send(object);
   }

   @Override
   public synchronized int send(Object object)
   {

      if (!listeners.containsKey(object.getClass()))
      {
         if (throwExceptionForUnregisteredPackets.get())
            throw new RuntimeException(object.getClass().getSimpleName() + " not registered with ObjectCommunicator");
         else
            return -1;
      }
      int bytesSend = sendTCP(object);
      updateDataRateTable(object, bytesSend);
      return bytesSend;
   }

   public void throwExceptionForUnregisteredPackets(boolean value)
   {
      throwExceptionForUnregisteredPackets.set(value);
   }

   private void updateDataRateTable(Object object, int bytesSend)
   {
      if(dataRateTable != null)
      {
         if(startTime == 0)
         {
            startTime = System.nanoTime();
         }
         
         TableData tableData = dataCounts.get(object.getClass());
         double wallTime = (System.nanoTime() - startTime) / 1e9;
         tableData.addData(bytesSend * 8, wallTime);
         dataRateTable.setValueAt(FormattingTools.toHumanReadable(tableData.getTotalBits()), tableData.getRow(), 1);
         dataRateTable.setValueAt(FormattingTools.toHumanReadable(tableData.getBitsPerSecond()), tableData.getRow(), 2);         
      }
   }
   
   protected final void createConnectionListener(EndPoint endPoint)
   {
      Listener listener = new Listener()
      {
         @Override
         @SuppressWarnings("unchecked")
         public void received(Connection connection, final Object object)
         {
            final Class<? extends Object> classType = object.getClass();
            ExecutorService executorService = listenerExecutors.get(classType);
            if(executorService != null)
            {
               executorService.execute(new Runnable()
               {
                  
                  @Override
                  public void run()
                  {
                     for(int i = 0; i < globalListeners.size(); i++)
                     {
                        globalListeners.get(i).consumeObject(object);
                     }
                     
                     ArrayList<ObjectConsumer<?>> objectListeners = listeners.get(classType);
                     if (objectListeners != null)
                     {
                        for (int i = 0; i < objectListeners.size(); i++)
                        {
                           @SuppressWarnings("rawtypes")
                           ObjectConsumer listener = objectListeners.get(i);
                           listener.consumeObject(object);
                        }
                     }
                  }
               });
            }
            
            else if (!(object instanceof KeepAlive))
            {
               System.err.println("Received unkown object of class " + classType);
            }
         }
         
         
         @Override
         public void connected(Connection connection)
         {
            for (int i = 0; i < tcpStateListeners.size(); i++)
            {
               tcpStateListeners.get(i).connected(connection);
            }

            for(int i = 0; i< stateListeners.size(); i++)
            {
               stateListeners.get(i).connected();
            }
         }
         
         @Override
         public void disconnected(Connection connection)
         {
            for (int i = 0; i < tcpStateListeners.size(); i++)
            {
               tcpStateListeners.get(i).disconnected(connection);
            }

            for(int i = 0; i< stateListeners.size(); i++)
            {
               stateListeners.get(i).disconnected();
            }
         }

      };
      endPoint.addListener(listener);
   }

   @Override
   public final void connect() throws IOException
   {
      openConnection();
   }
   
   @Override
   public final void disconnect()
   {
      closeConnection();
      for(ExecutorService executor : listenerExecutors.values())
      {
         executor.shutdownNow();
      }
   }
   
   protected abstract void openConnection() throws IOException;
   
   protected abstract int sendUDP(Object object);

   protected abstract int sendTCP(Object object);
   
   @Override
   public abstract boolean isConnected();

   /**
    * Disconnect the connection, but leave the executor listeners alive. This allows re-connecting at a later moment.
    */
   @Override
   public abstract void closeConnection();

   private class TableData
   {
      private final int row;
      private long totalBits = 0;
      private double bitsPerSecond = 0;
      
      public TableData(int row)
      {
         this.row = row;
      }
      
      public void addData(int bits, double wallTime)
      {
         totalBits += bits;
         bitsPerSecond = totalBits/wallTime;
      }
      
      public long getTotalBits()
      {
         return totalBits;
      }
      
      public double getBitsPerSecond()
      {
         return bitsPerSecond;
      }
      
      public int getRow()
      {
         return row;
      }
   
   }

}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/subscriber/RequestControllerStopSubscriber.java<|end_filename|>
package us.ihmc.avatar.ros.subscriber;

import controller_msgs.msg.dds.BDIBehaviorCommandPacket;
import std_msgs.Empty;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.PacketDestination;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.utilities.ros.subscriber.AbstractRosTopicSubscriber;


public class RequestControllerStopSubscriber extends AbstractRosTopicSubscriber<std_msgs.Empty>
{
   private final PacketCommunicator packetCommunicator;
   
   public RequestControllerStopSubscriber(PacketCommunicator packetCommunicator)
   {
      super(std_msgs.Empty._TYPE);
      
      this.packetCommunicator = packetCommunicator;
   }

   @Override
   public void onNewMessage(Empty message)
   {
      BDIBehaviorCommandPacket packet = HumanoidMessageTools.createBDIBehaviorCommandPacket(true);
      packet.setDestination(PacketDestination.CONTROLLER.ordinal());
      packetCommunicator.send(packet);
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/TwoSegmentFootstepGenerator.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import java.util.ArrayList;

import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.humanoidRobotics.footstep.Footstep;
import us.ihmc.humanoidRobotics.footstep.FootstepUtils;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.dataStructures.HeightMapWithPoints;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class TwoSegmentFootstepGenerator implements FootstepGenerator
{
   private AbstractSimpleParametersFootstepGenerator footstepGenerator;
   private AbstractSimpleParametersFootstepGenerator footstepGenerator2;
   private final SideDependentList<RigidBodyBasics> feet;
   private final SideDependentList<ReferenceFrame> soleFrames;
   private SideDependentList<Footstep> midStanceFeet = new SideDependentList<Footstep>();
   private boolean secondSegmentManuallySpecifiesStart = false;

   public TwoSegmentFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FramePose2D midPose, FramePose2D finalPose, PathTypeStepParameters pathType,
                                      PathTypeStepParameters pathType2)
   {
      this.feet = feet;
      this.soleFrames = soleFrames;
      footstepGenerator = new TurnStraightTurnFootstepGenerator(feet, soleFrames, midPose, pathType);
      footstepGenerator2 = new TurnStraightTurnFootstepGenerator(feet, soleFrames, finalPose, pathType2);
   }

   // TurnStraightTurn used despite the ring being set to auto-align before this to ensure that the orientation is reached even for
   // small displacement paths that could otherwise end early before fully aligning with the direction of the path in TurningThenStraightFootstepGenerator.
   public TwoSegmentFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FramePose2D midPose, FramePoint2D finalPoint, PathTypeStepParameters pathType,
                                      PathTypeStepParameters pathType2)
   {
      this.feet = feet;
      this.soleFrames = soleFrames;
      footstepGenerator = new TurnStraightTurnFootstepGenerator(feet, soleFrames, midPose, pathType);
      footstepGenerator2 = new TurningThenStraightFootstepGenerator(feet, soleFrames, finalPoint, pathType2);
   }

   public TwoSegmentFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FramePose2D midPose, FramePoint2D finalPoint, PathTypeStepParameters pathType,
                                      PathTypeStepParameters pathType2, RobotSide stanceStart)
   {
      this.feet = feet;
      this.soleFrames = soleFrames;
      footstepGenerator = new TurnStraightTurnFootstepGenerator(feet, soleFrames, midPose, pathType, stanceStart);
      footstepGenerator2 = new TurningThenStraightFootstepGenerator(feet, soleFrames, finalPoint, pathType2);
   }

   public TwoSegmentFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FramePose2D midPose, FramePoint2D finalPoint, PathTypeStepParameters pathType,
                                      PathTypeStepParameters pathType2, RobotSide stanceStart, RobotSide midStanceStart)
   {
      this.feet = feet;
      this.soleFrames = soleFrames;
      footstepGenerator = new TurnStraightTurnFootstepGenerator(feet, soleFrames, midPose, pathType, stanceStart);
      secondSegmentManuallySpecifiesStart = true;
      footstepGenerator2 = new TurningThenStraightFootstepGenerator(feet, soleFrames, finalPoint, pathType2, midStanceStart);
   }

   @Override
   public ArrayList<Footstep> generateDesiredFootstepList()
   {
      ArrayList<Footstep> footstepsFirstSegment = new ArrayList<Footstep>(footstepGenerator.generateDesiredFootstepList());

      initializeStanceFeetOfSecondSegment(footstepsFirstSegment);
      if (footstepGenerator2.hasDisplacement())
      {
         ArrayList<Footstep> footstepsSecondSegment = new ArrayList<Footstep>(footstepGenerator2.generateDesiredFootstepList());

         return concatenateFootstepPaths(footstepsFirstSegment, footstepsSecondSegment);
      }
      else
         return footstepsFirstSegment;
   }

   private void initializeStanceFeetOfSecondSegment(ArrayList<Footstep> footstepsToControlRing)
   {
      int numFootsteps = footstepsToControlRing.size();
      RobotSide lastStepSide = RobotSide.RIGHT;
      if (numFootsteps >= 1)
      {
         Footstep lastFootstep = footstepsToControlRing.get(numFootsteps - 1);
         lastStepSide = lastFootstep.getRobotSide();
         setMidstep(lastFootstep);
      }
      else
      {
         System.err.println("Why are there no feet generated for the first segment of TwoSegmentFootstepGenerator?");
         setMidstep(FootstepUtils.generateStandingFootstep(lastStepSide, feet.get(lastStepSide), soleFrames.get(lastStepSide)));
      }

      if (numFootsteps >= 2)
      {
         setMidstep(footstepsToControlRing.get(numFootsteps - 2));
      }
      else
      {
         RobotSide lastStepOppositeSide = lastStepSide.getOppositeSide();
         setMidstep(FootstepUtils.generateStandingFootstep(lastStepOppositeSide, feet.get(lastStepOppositeSide), soleFrames.get(lastStepOppositeSide)));
      }

      if (!secondSegmentManuallySpecifiesStart)
         footstepGenerator2.setStanceStartPreference(lastStepSide);
      footstepGenerator2.setPriorFootposes(midStanceFeet);

      footstepGenerator2.initialize();
   }

   private void setMidstep(Footstep footstep)
   {
      midStanceFeet.set(footstep.getRobotSide(), footstep);
   }

   private ArrayList<Footstep> concatenateFootstepPaths(ArrayList<Footstep> firstSetOfSteps, ArrayList<Footstep> secondSetOfSteps)
   {
      int indexOfLastStepOfFirstSegment = firstSetOfSteps.size() - 1;
      RobotSide sideOfLastFootstepOfFirstSegment = firstSetOfSteps.get(indexOfLastStepOfFirstSegment).getRobotSide();
      RobotSide sideOfFirstFootstepOfSecondSegment = secondSetOfSteps.get(0).getRobotSide();

      if (sideOfLastFootstepOfFirstSegment == sideOfFirstFootstepOfSecondSegment)
         firstSetOfSteps.remove(indexOfLastStepOfFirstSegment);

      for (Footstep footstep : secondSetOfSteps)
      {
         firstSetOfSteps.add(footstep);
      }

      return firstSetOfSteps;
   }

   public void setPoseFinderParams(double footstepFittingBufferSize, double boundingBoxForFootstepHeightFindingSideLength)
   {
      footstepGenerator.setPoseFinderParams(footstepFittingBufferSize, boundingBoxForFootstepHeightFindingSideLength);
      footstepGenerator2.setPoseFinderParams(footstepFittingBufferSize, boundingBoxForFootstepHeightFindingSideLength);
   }

   public void setHeightMap(HeightMapWithPoints heightMap, SideDependentList<? extends ContactablePlaneBody> contactableFeet)
   {
      footstepGenerator.setHeightMap(heightMap, contactableFeet);
      footstepGenerator2.setHeightMap(heightMap, contactableFeet);
   }

   public boolean hasDisplacement()
   {
      return footstepGenerator.hasDisplacement() || footstepGenerator2.hasDisplacement();
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/ParameterChangeListener.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements;

public interface ParameterChangeListener
{
   public abstract void changed(GuiParameter parameter);
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/geometry/IntersectionPlaneBoxCalculator.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.geometry;

import gnu.trove.list.array.TDoubleArrayList;
import javafx.util.Pair;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.commons.lists.SupplierBuilder;

import java.util.ArrayList;
import java.util.List;

public class IntersectionPlaneBoxCalculator
{
   private static final double EPSILON = 1.0e-3;

   private final Point3D[] boxVertices = new Point3D[8];
   private final List<Pair<Point3D, Point3D>> boxEdges = new ArrayList<>();

   private final Vector3D boxSize = new Vector3D();
   private final Point3D boxCenter = new Point3D();

   private final Point3D pointOnPlane = new Point3D();
   private final Vector3D planeNormal = new Vector3D();

   public IntersectionPlaneBoxCalculator()
   {
      // Index ordering as in http://paulbourke.net/geometry/polygonise/
      boxVertices[0] = new Point3D( 0.5,  0.5, -0.5);
      boxVertices[1] = new Point3D( 0.5, -0.5, -0.5);
      boxVertices[2] = new Point3D(-0.5, -0.5, -0.5);
      boxVertices[3] = new Point3D(-0.5,  0.5, -0.5);
      boxVertices[4] = new Point3D( 0.5,  0.5,  0.5);
      boxVertices[5] = new Point3D( 0.5, -0.5,  0.5);
      boxVertices[6] = new Point3D(-0.5, -0.5,  0.5);
      boxVertices[7] = new Point3D(-0.5,  0.5,  0.5);

      boxEdges.add(new Pair<>(boxVertices[0], boxVertices[1]));
      boxEdges.add(new Pair<>(boxVertices[1], boxVertices[2]));
      boxEdges.add(new Pair<>(boxVertices[2], boxVertices[3]));
      boxEdges.add(new Pair<>(boxVertices[3], boxVertices[0]));
      boxEdges.add(new Pair<>(boxVertices[4], boxVertices[5]));
      boxEdges.add(new Pair<>(boxVertices[5], boxVertices[6]));
      boxEdges.add(new Pair<>(boxVertices[6], boxVertices[7]));
      boxEdges.add(new Pair<>(boxVertices[7], boxVertices[4]));
      boxEdges.add(new Pair<>(boxVertices[0], boxVertices[4]));
      boxEdges.add(new Pair<>(boxVertices[1], boxVertices[5]));
      boxEdges.add(new Pair<>(boxVertices[2], boxVertices[6]));
      boxEdges.add(new Pair<>(boxVertices[3], boxVertices[7]));
   }

   public void setCube(double size, Point3D center)
   {
      setBox(size, size, size, center);
   }

   public void setCube(double size, double centerX, double centerY, double centerZ)
   {
      setBox(size, size, size, centerX, centerY, centerZ);
   }

   public void setBox(double lx, double ly, double lz, Point3D center)
   {
      setBox(lx, ly, lz, center.getX(), center.getY(), center.getZ());
   }

   public void setBox(double lx, double ly, double lz, double centerX, double centerY, double centerZ)
   {
      boxSize.set(lx, ly, lz);
      boxCenter.set(centerX, centerY, centerZ);
   }

   public void setPlane(Point3D pointOnPlane, Vector3D planeNormal)
   {
      this.pointOnPlane.set(pointOnPlane);
      this.planeNormal.set(planeNormal);
   }

   private final Vector3D edgeVector = new Vector3D();
   private final Vector3D fromPlaneCenterToEdgeStart = new Vector3D();
   private final RecyclingArrayList<Point3D> unorderedIntersections = new RecyclingArrayList<>(0, SupplierBuilder.createFromEmptyConstructor(Point3D.class));

   public List<Point3D> computeIntersections()
   {
      RecyclingArrayList<Point3D> intersections = new RecyclingArrayList<>(Point3D.class);
      computeIntersections(intersections);
      return intersections;
   }

   private final Point3D intersection = new Point3D();

   public void computeIntersections(RecyclingArrayList<Point3D> intersectionsToPack)
   {
      unorderedIntersections.clear();

      for (int i = 0; i < 12; i++)
      {
         Point3D edgeStart = boxEdges.get(i).getKey();
         Point3D edgeEnd = boxEdges.get(i).getValue();
         edgeVector.sub(edgeEnd, edgeStart);

         fromPlaneCenterToEdgeStart.sub(pointOnPlane, boxCenter);
         fromPlaneCenterToEdgeStart.setX(fromPlaneCenterToEdgeStart.getX() / boxSize.getX());
         fromPlaneCenterToEdgeStart.setY(fromPlaneCenterToEdgeStart.getY() / boxSize.getY());
         fromPlaneCenterToEdgeStart.setZ(fromPlaneCenterToEdgeStart.getZ() / boxSize.getZ());
         fromPlaneCenterToEdgeStart.sub(edgeStart);

         double dotNormalEdge = planeNormal.dot(edgeVector);

         if (Math.abs(dotNormalEdge) < 1.0e-5)
            continue;

         double scaleFactor = planeNormal.dot(fromPlaneCenterToEdgeStart) / dotNormalEdge;
         if (scaleFactor < 0.0 || scaleFactor > 1.0)
            continue;

         intersection.scaleAdd(scaleFactor, edgeVector, edgeStart);
         intersection.setX(intersection.getX() * boxSize.getX());
         intersection.setY(intersection.getY() * boxSize.getY());
         intersection.setZ(intersection.getZ() * boxSize.getZ());
         intersection.add(boxCenter);
         if (!listContains(unorderedIntersections, intersection))
            unorderedIntersections.add().set(intersection);

         if (unorderedIntersections.size() == 6) // That's the max number of possible intersections
            break;
      }
      reorderIntersections(unorderedIntersections, intersectionsToPack);
   }

   private final TDoubleArrayList orderedAngles = new TDoubleArrayList();
   private final Vector3D v0 = new Vector3D();
   private final Vector3D vi = new Vector3D();
   private final Vector3D vCross = new Vector3D();
   private final Point3D average = new Point3D();

   private void reorderIntersections(List<Point3D> unorderedIntersections, RecyclingArrayList<Point3D> intersectionsToPack)
   {
      intersectionsToPack.clear();
      if (unorderedIntersections.isEmpty())
         return;

      orderedAngles.reset();
      intersectionsToPack.add().set(unorderedIntersections.get(0));
      orderedAngles.add(0.0);

      average.set(0.0, 0.0, 0.0);
      for (int i = 0; i < unorderedIntersections.size(); i++)
         average.add(unorderedIntersections.get(i));
      average.scale(1.0 / unorderedIntersections.size());

      v0.sub(unorderedIntersections.get(0), average);
      v0.normalize();

      for (int i = 1; i < unorderedIntersections.size(); i++)
      {
         vi.sub(unorderedIntersections.get(i), average);
         vi.normalize();

         double angle = v0.dot(vi); // Gives [1.0, -1.0] for an angle in [0, Pi]

         vCross.cross(v0, vi);
         if (vCross.dot(planeNormal) < 0.0) // Allows to make a difference between the two angle ranges: [0, Pi] and [Pi, 2*Pi]
            angle = -2 - angle; // We're in the range [Pi, 2*Pi], this transform the dot original range [1, -1] to [-1, -3] where -3 is when vi & v0 point in the same direction. 
         // We obtained an "angle" that goes from 1 down to -3. It is transformed to be in the range [0, 4].
         angle -= 1.0;
         angle *= -1.0;

         // Binary search to figure out where the vertex should go in the list.
         int index = angleBinarySearch(orderedAngles, angle);
         if (index < 0)
            index = -index - 1;

         intersectionsToPack.insertAtIndex(index).set(unorderedIntersections.get(i));
         orderedAngles.insert(index, angle);
      }
   }

   private static int angleBinarySearch(TDoubleArrayList l, double key)
   {
      int low = 0;
      int high = l.size() - 1;

      while (low <= high)
      {
         int mid = (low + high) >>> 1;
         double midVal = l.get(mid);
         int cmp = compare(midVal, key);

         if (cmp < 0)
            low = mid + 1;
         else if (cmp > 0)
            high = mid - 1;
         else
            return mid; // key found
      }
      return -(low + 1); // key not found
   }

   private static int compare(double angle1, double angle2)
   {
      if (angle1 == angle2)
         return 0;
      else if (angle1 < angle2)
         return -1;
      // (angle1 > angle2)
      return 1;
   }

   private boolean listContains(List<Point3D> listOfPoints, Point3D pointToCheck)
   {
      for (int i = 0; i < listOfPoints.size(); i++)
      {
         if (epsilonEquals(listOfPoints.get(i), pointToCheck))
            return true;
      }
      return false;
   }

   private boolean epsilonEquals(Point3D point1, Point3D point2)
   {
      double diff;

      diff = point1.getX() - point2.getX();
      if (Double.isNaN(diff))
         return false;
      if ((diff < 0 ? -diff : diff) > EPSILON * boxSize.getX())
         return false;

      diff = point1.getY() - point2.getY();
      if (Double.isNaN(diff))
         return false;
      if ((diff < 0 ? -diff : diff) > EPSILON * boxSize.getY())
         return false;

      diff = point1.getZ() - point2.getZ();
      if (Double.isNaN(diff))
         return false;
      if ((diff < 0 ? -diff : diff) > EPSILON * boxSize.getZ())
         return false;

      return true;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/PlanOffsetStatus.java<|end_filename|>
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.
       * The controller sends this message to notify that it has shifted the remaining footsteps to be executed due to some execution error.
       */
public class PlanOffsetStatus extends Packet<PlanOffsetStatus> implements Settable<PlanOffsetStatus>, EpsilonComparable<PlanOffsetStatus>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The amount by which the remaining footsteps have been translated.
            */
   public us.ihmc.euclid.tuple3D.Vector3D offset_vector_;

   public PlanOffsetStatus()
   {
      offset_vector_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public PlanOffsetStatus(PlanOffsetStatus other)
   {
      this();
      set(other);
   }

   public void set(PlanOffsetStatus other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.offset_vector_, offset_vector_);
   }

   /**
            * 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_;
   }


   /**
            * The amount by which the remaining footsteps have been translated.
            */
   public us.ihmc.euclid.tuple3D.Vector3D getOffsetVector()
   {
      return offset_vector_;
   }


   public static Supplier<PlanOffsetStatusPubSubType> getPubSubType()
   {
      return PlanOffsetStatusPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return PlanOffsetStatusPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(PlanOffsetStatus other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.offset_vector_.epsilonEquals(other.offset_vector_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof PlanOffsetStatus)) return false;

      PlanOffsetStatus otherMyClass = (PlanOffsetStatus) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.offset_vector_.equals(otherMyClass.offset_vector_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("PlanOffsetStatus {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("offset_vector=");
      builder.append(this.offset_vector_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/handControl/HandFingerTrajectoryMessagePublisher.java<|end_filename|>
package us.ihmc.avatar.handControl;

import gnu.trove.list.array.TDoubleArrayList;
import us.ihmc.robotics.robotSide.RobotSide;

public interface HandFingerTrajectoryMessagePublisher
{
   public abstract void sendFingerTrajectoryMessage(RobotSide robotSide, TDoubleArrayList desiredPositions, TDoubleArrayList trajectoryTimes);
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/JointLoadStatusProvider.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates;

public interface JointLoadStatusProvider
{
   public abstract boolean isJointLoadBearing(String jointName);
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/TurnThenStraightOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FrameOrientation2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.robotics.geometry.AngleTools;

public class TurnThenStraightOverheadPath extends CompositeOverheadPath
{
   private TurningOverheadPath turningPath;
   private StraightLineOverheadPath straightLinePath;
   private final static double defaultNoTranslationTolerance = 1e-14;

   public TurnThenStraightOverheadPath(FramePose2D startPose, FramePoint2D endPoint, double headingOffsetFromPath)
   {
      this(startPose, endPoint, headingOffsetFromPath, defaultNoTranslationTolerance);
   }

   public TurnThenStraightOverheadPath(FramePose2D startPose, FramePoint2D endPoint, double headingOffsetFromPath, double noTranslationTolerance)
   {
      super(calculatePaths(startPose, endPoint, headingOffsetFromPath, noTranslationTolerance));
      turningPath = (TurningOverheadPath) this.paths.get(0);
      straightLinePath = (StraightLineOverheadPath) this.paths.get(1);
   }

   private static List<OverheadPath> calculatePaths(FramePose2D startPose, FramePoint2D endPoint, double headingOffset, double noTranslationTolerance)
   {
      startPose.checkReferenceFrameMatch(endPoint);
      double heading = AngleTools.calculateHeading(startPose, endPoint, headingOffset, noTranslationTolerance);
      FrameOrientation2D intermediateOrientation = new FrameOrientation2D(startPose.getReferenceFrame(), heading);
      TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation);
      FramePose2D intermediatePose = turningPath.getPoseAtS(1.0);
      StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPoint);
      List<OverheadPath> paths = new ArrayList<OverheadPath>();
      paths.add(turningPath);
      paths.add(straightPath);

      return paths;
   }

   public double getSignedTurningAmount()
   {
      return turningPath.getDeltaYaw();//signed
   }

   public double getDistance()
   {
      return straightLinePath.getDistance();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/wholeBodyTrajectoryToolboxModule/SpatialNodePlotter.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.wholeBodyTrajectoryToolboxModule;

import java.awt.Color;
import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.graphicsDescription.plotting.artifact.CircleArtifact;
import us.ihmc.graphicsDescription.plotting.artifact.LineArtifact;
import us.ihmc.manipulation.planning.exploringSpatial.SpatialNode;
import us.ihmc.plotting.Plotter;

public class SpatialNodePlotter
{
   private int dimensionOfConfigurations;
   private double trajectoryTime;

   private List<Plotter> plotters = new ArrayList<Plotter>();
   private double[] upperLimits;
   private double[] lowerLimits;

   private int cnt;

   private boolean isFrameEnabled;

   public SpatialNodePlotter(WholeBodyTrajectoryToolboxData toolboxData, boolean enabled)
   {
      this.dimensionOfConfigurations = toolboxData.getExplorationDimension();

      this.upperLimits = new double[dimensionOfConfigurations];
      this.lowerLimits = new double[dimensionOfConfigurations];
      this.trajectoryTime = toolboxData.getTrajectoryTime();

      for (int i = 0; i < dimensionOfConfigurations; i++)
      {
         Plotter plotter = new Plotter();

         plotter.setPreferredSize(400, 600);

         plotter.setViewRange(2.0);
         plotter.setXYZoomEnabled(true);
         plotter.setShowLabels(true);
         plotter.setFocusPointX(0.5);
         plotter.setFocusPointY(0.0);
         plotters.add(plotter);
         upperLimits[i] = Double.NEGATIVE_INFINITY;
         lowerLimits[i] = Double.POSITIVE_INFINITY;

         String plotterName = toolboxData.createRandomSpatialData().getConfigurationNames().get(i);
         isFrameEnabled = enabled;
         if (enabled)
            plotter.showInNewWindow(plotterName, false);
      }
      cnt = 0;
   }

   public void closeAll()
   {
      for (int i = 0; i < plotters.size(); i++)
      {
         // TODO : close panels.
         if (isFrameEnabled)
            plotters.get(i).getJFrame().dispose();
      }
   }

   /**
    * 1::node
    * 2::path
    * 3::shortcut
    */
   public void update(SpatialNode node, int type)
   {
      double normalizedTime = node.getTime() / trajectoryTime;
      Color color;
      double diameter = 0.01;

      for (int nodeIndex = 0; nodeIndex < dimensionOfConfigurations; nodeIndex++)
      {
         String prefix;
         switch (type)
         {
         case 1:
            if (node.isValid())
            {
               prefix = "" + cnt + "_valid_" + nodeIndex;
               color = Color.blue;
            }
            else
            {
               prefix = "" + cnt + "_invalid_" + nodeIndex;
               diameter = 0.01;
               color = Color.red;
            }
            break;
         case 2:
            prefix = "" + cnt + "_path_" + nodeIndex;
            color = Color.black;
            break;
         case 3:
            prefix = "" + cnt + "_shortcut_" + nodeIndex;
            color = Color.green;
            break;
         default:
            prefix = "";
            color = Color.white;
            break;
         }

         double configurationData = node.getConfigurationData(nodeIndex);

         if (node.getParent() != null && node.isValid())
         {
            SpatialNode parentNode = node.getParent();
            double parentTime = parentNode.getTime() / trajectoryTime;
            double parentConfigurationData = parentNode.getConfigurationData(nodeIndex);

            LineArtifact lineArtifact = new LineArtifact(prefix + "_line", new Point2D(parentTime, parentConfigurationData),
                                                         new Point2D(normalizedTime, configurationData));

            lineArtifact.setColor(color);
            plotters.get(nodeIndex).addArtifact(lineArtifact);
         }

         CircleArtifact nodeArtifact = new CircleArtifact(prefix + "_node", normalizedTime, configurationData, diameter, true);
         nodeArtifact.setColor(color);

         plotters.get(nodeIndex).addArtifact(nodeArtifact);
         plotters.get(nodeIndex).update();
      }

      cnt++;
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/setterUpper/GUISetterUpperRegistry.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.setterUpper;

import java.util.ArrayList;

import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class GUISetterUpperRegistry
{

   private final ArrayList<GUISetterUpper> guiSetterUppers = new ArrayList<GUISetterUpper>();
   
   public void registerGUISetterUpper(GUISetterUpper guiSetterUpper)
   {
      this.guiSetterUppers.add(guiSetterUpper);
      
   }
   
   public void setupGUIs(SimulationConstructionSet scs)
   {
      for (GUISetterUpper guiSetterUpper : guiSetterUppers)
      {
         guiSetterUpper.setupGUI(scs);
      }
   }
}


<|start_filename|>ihmc-ros-tools/generated-src/ihmc_msgs/JointspaceTrajectoryRosMessage.java<|end_filename|>
package ihmc_msgs;

public interface JointspaceTrajectoryRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_msgs/JointspaceTrajectoryRosMessage";
  static final java.lang.String _DEFINITION = "## JointspaceTrajectoryRosMessage\n# \n\n# List of points in the trajectory.\nihmc_msgs/OneDoFJointTrajectoryRosMessage[] joint_trajectory_messages\n\n# Properties for queueing trajectories.\nihmc_msgs/QueueableRosMessage queueing_properties\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  java.util.List<ihmc_msgs.OneDoFJointTrajectoryRosMessage> getJointTrajectoryMessages();
  void setJointTrajectoryMessages(java.util.List<ihmc_msgs.OneDoFJointTrajectoryRosMessage> value);
  ihmc_msgs.QueueableRosMessage getQueueingProperties();
  void setQueueingProperties(ihmc_msgs.QueueableRosMessage value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/dataProcessors/ValueDataCheckerParameters.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.dataProcessors;

public class ValueDataCheckerParameters
{
   private double maximumDerivative = Double.POSITIVE_INFINITY;
   private double maximumSecondDerivative = Double.POSITIVE_INFINITY;
   private double maximumValue = Double.POSITIVE_INFINITY;
   private double minimumValue = Double.NEGATIVE_INFINITY;
   private double errorThresholdOnDerivativeComparison = Double.POSITIVE_INFINITY;
   
   public ValueDataCheckerParameters()
   {
      
   }
 
   public ValueDataCheckerParameters getDefensiveCopy()
   {
      ValueDataCheckerParameters ret = new ValueDataCheckerParameters();
      ret.setErrorThresholdOnDerivativeComparison(this.getErrorThresholdOnDerivativeComparison());
      ret.setMaximumDerivative(this.getMaximumDerivative());
      ret.setMaximumSecondDerivative(this.getMaximumSecondDerivative());
      ret.setMaximumValue(this.getMaximumValue());
      ret.setMinimumValue(this.getMinimumValue());
      
      return ret;
   }
 
   public double getErrorThresholdOnDerivativeComparison()
   {
      return errorThresholdOnDerivativeComparison;
   }

   public void setErrorThresholdOnDerivativeComparison(double errorThresholdOnDerivativeComparison)
   {
      this.errorThresholdOnDerivativeComparison = Math.abs(errorThresholdOnDerivativeComparison);
   }

 
   public double getMaximumDerivative()
   {
      return maximumDerivative;
   }

   public void setMaximumDerivative(double maximumDerivative)
   {
      this.maximumDerivative = Math.abs(maximumDerivative);
   }

   public double getMaximumSecondDerivative()
   {
      return maximumSecondDerivative;
   }

   public void setMaximumSecondDerivative(double maximumSecondDerivative)
   {
      this.maximumSecondDerivative = Math.abs(maximumSecondDerivative);
   }

   public double getMaximumValue()
   {
      return maximumValue;
   }

   public void setMaximumValue(double maximumValue)
   {
      if (maximumValue < minimumValue)
         throw new RuntimeException("maximumValue must be greater than minimumValue. maximumValue=" + maximumValue + ", minimumValue=" + minimumValue);
      
      this.maximumValue = maximumValue;
   }

   public double getMinimumValue()
   {
      return minimumValue;
   }

   public void setMinimumValue(double minimumValue)
   {
      if (minimumValue > maximumValue)
         throw new RuntimeException("maximumValue must be greater than minimumValue. maximumValue=" + maximumValue + ", minimumValue=" + minimumValue);
      
      this.minimumValue = minimumValue;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WholeBodyTrajectoryToolboxMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "WholeBodyTrajectoryToolboxMessage" defined in "WholeBodyTrajectoryToolboxMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from WholeBodyTrajectoryToolboxMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit WholeBodyTrajectoryToolboxMessage_.idl instead.
*
*/
public class WholeBodyTrajectoryToolboxMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::WholeBodyTrajectoryToolboxMessage_";

   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.WholeBodyTrajectoryToolboxMessage 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.WholeBodyTrajectoryToolboxMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 10; ++i0)
      {
          current_alignment += controller_msgs.msg.dds.WaypointBasedTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 10; ++i0)
      {
          current_alignment += controller_msgs.msg.dds.RigidBodyExplorationConfigurationMessagePubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 10; ++i0)
      {
          current_alignment += controller_msgs.msg.dds.ReachingManifoldMessagePubSubType.getMaxCdrSerializedSize(current_alignment);}

      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.getCdrSerializedSize(data.getConfiguration(), current_alignment);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getEndEffectorTrajectories().size(); ++i0)
      {
          current_alignment += controller_msgs.msg.dds.WaypointBasedTrajectoryMessagePubSubType.getCdrSerializedSize(data.getEndEffectorTrajectories().get(i0), current_alignment);}

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getExplorationConfigurations().size(); ++i0)
      {
          current_alignment += controller_msgs.msg.dds.RigidBodyExplorationConfigurationMessagePubSubType.getCdrSerializedSize(data.getExplorationConfigurations().get(i0), current_alignment);}

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getReachingManifolds().size(); ++i0)
      {
          current_alignment += controller_msgs.msg.dds.ReachingManifoldMessagePubSubType.getCdrSerializedSize(data.getReachingManifolds().get(i0), current_alignment);}


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.write(data.getConfiguration(), cdr);
      if(data.getEndEffectorTrajectories().size() <= 10)
      cdr.write_type_e(data.getEndEffectorTrajectories());else
          throw new RuntimeException("end_effector_trajectories field exceeds the maximum length");

      if(data.getExplorationConfigurations().size() <= 10)
      cdr.write_type_e(data.getExplorationConfigurations());else
          throw new RuntimeException("exploration_configurations field exceeds the maximum length");

      if(data.getReachingManifolds().size() <= 10)
      cdr.write_type_e(data.getReachingManifolds());else
          throw new RuntimeException("reaching_manifolds field exceeds the maximum length");

   }

   public static void read(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.read(data.getConfiguration(), cdr);	
      cdr.read_type_e(data.getEndEffectorTrajectories());	
      cdr.read_type_e(data.getExplorationConfigurations());	
      cdr.read_type_e(data.getReachingManifolds());	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_a("configuration", new controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType(), data.getConfiguration());

      ser.write_type_e("end_effector_trajectories", data.getEndEffectorTrajectories());
      ser.write_type_e("exploration_configurations", data.getExplorationConfigurations());
      ser.write_type_e("reaching_manifolds", data.getReachingManifolds());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_a("configuration", new controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType(), data.getConfiguration());

      ser.read_type_e("end_effector_trajectories", data.getEndEffectorTrajectories());
      ser.read_type_e("exploration_configurations", data.getExplorationConfigurations());
      ser.read_type_e("reaching_manifolds", data.getReachingManifolds());
   }

   public static void staticCopy(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage src, controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage createData()
   {
      return new controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage();
   }
   @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.WholeBodyTrajectoryToolboxMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage src, controller_msgs.msg.dds.WholeBodyTrajectoryToolboxMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public WholeBodyTrajectoryToolboxMessagePubSubType newInstance()
   {
      return new WholeBodyTrajectoryToolboxMessagePubSubType();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/RobotTarget.java<|end_filename|>
package us.ihmc.avatar.drcRobot;

public enum RobotTarget
{
   SCS, GAZEBO, REAL_ROBOT, HEAD_ON_A_STICK
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/algorithms/FrameConvexPolygonWithLineIntersector2d.java<|end_filename|>
package us.ihmc.robotics.geometry.algorithms;

import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
import us.ihmc.euclid.referenceFrame.FrameLine2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.interfaces.FrameConvexPolygon2DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.FrameLine2DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder;

public class FrameConvexPolygonWithLineIntersector2d
{
   private final FramePoint2D intersectionPointOne;
   private final FramePoint2D intersectionPointTwo;

   private IntersectionResult intersectionResult;

   public enum IntersectionResult
   {
      LINE_SEGMENT_INTERSECTION, POINT_INTERSECTION, NO_INTERSECTION;
   }

   public FrameConvexPolygonWithLineIntersector2d()
   {
      intersectionPointOne = new FramePoint2D();
      intersectionPointTwo = new FramePoint2D();

      intersectionResult = IntersectionResult.NO_INTERSECTION;
   }

   public void intersectWithLine(FrameConvexPolygon2DReadOnly frameConvexPolygon2d, FrameLine2DReadOnly frameLine2d)
   {
      checkAndSetFrames(frameConvexPolygon2d, frameLine2d);
      int intersectionTypeInt = frameConvexPolygon2d.intersectionWith(frameLine2d, intersectionPointOne, intersectionPointTwo);

      packIntersectionType(intersectionTypeInt);
   }

   /**
    * There is actually no ray class at the moment, so we use a FrameLine2d.
    *
    * TODO: Make ray classes and use them. @dcalvert
    */
   public void intersectWithRay(FrameConvexPolygon2D frameConvexPolygon2d, FrameLine2D frameRay2d)
   {
      checkAndSetFrames(frameConvexPolygon2d, frameRay2d);
      int intersectionTypeInt = frameConvexPolygon2d.intersectionWithRay(frameRay2d, intersectionPointOne, intersectionPointTwo);

      packIntersectionType(intersectionTypeInt);
   }

   private void packIntersectionType(int intersectionTypeInt)
   {
      switch (intersectionTypeInt)
      {
      case 0:
         intersectionResult = IntersectionResult.NO_INTERSECTION;
         break;
      case 1:
         intersectionResult = IntersectionResult.POINT_INTERSECTION;
         break;
      case 2:
         intersectionResult = IntersectionResult.LINE_SEGMENT_INTERSECTION;
         break;
      }
   }

   private void checkAndSetFrames(ReferenceFrameHolder frameObject1, ReferenceFrameHolder frameObject2)
   {
      frameObject1.checkReferenceFrameMatch(frameObject2);
      intersectionPointOne.setToZero(frameObject1.getReferenceFrame());
      intersectionPointTwo.setToZero(frameObject1.getReferenceFrame());
   }

   public FramePoint2D getIntersectionPointOne()
   {
      return intersectionPointOne;
   }

   public FramePoint2D getIntersectionPointTwo()
   {
      return intersectionPointTwo;
   }

   public IntersectionResult getIntersectionResult()
   {
      return intersectionResult;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/bipedSupportPolygons/ListOfPointsContactableFoot.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.bipedSupportPolygons;

import java.util.List;

import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.referenceFrame.FrameLineSegment2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.humanoidRobotics.bipedSupportPolygons.ContactableFoot;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public class ListOfPointsContactableFoot extends ListOfPointsContactablePlaneBody implements ContactableFoot
{
   private final FramePoint2D toeOffContactPoint;
   private final FrameLineSegment2D toeOffContactLine;

   public ListOfPointsContactableFoot(RigidBodyBasics rigidBody, ReferenceFrame soleFrame, List<Point2D> contactPointsInSoleFrame, Point2D toeOffContactPointInSoleFrame,
         LineSegment2D toeOffContactLineInSoleFrame)
   {
      super(rigidBody, soleFrame, contactPointsInSoleFrame);

      this.toeOffContactPoint = new FramePoint2D(soleFrame, toeOffContactPointInSoleFrame);
      this.toeOffContactLine = new FrameLineSegment2D(soleFrame, toeOffContactLineInSoleFrame);
   }

   @Override
   public void getToeOffContactPoint(FramePoint2D contactPointToPack)
   {
      contactPointToPack.setIncludingFrame(toeOffContactPoint);
   }

   @Override
   public void getToeOffContactLine(FrameLineSegment2D contactLineToPack)
   {
      contactLineToPack.setIncludingFrame(toeOffContactLine);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ComVelocityPacket.java<|end_filename|>
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 quadruped controller API.
       */
public class ComVelocityPacket extends Packet<ComVelocityPacket> implements Settable<ComVelocityPacket>, EpsilonComparable<ComVelocityPacket>
{
   public us.ihmc.euclid.tuple3D.Vector3D velocity_;

   public ComVelocityPacket()
   {
      velocity_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public ComVelocityPacket(ComVelocityPacket other)
   {
      this();
      set(other);
   }

   public void set(ComVelocityPacket other)
   {
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.velocity_, velocity_);   }


   public us.ihmc.euclid.tuple3D.Vector3D getVelocity()
   {
      return velocity_;
   }


   public static Supplier<ComVelocityPacketPubSubType> getPubSubType()
   {
      return ComVelocityPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ComVelocityPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ComVelocityPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!this.velocity_.epsilonEquals(other.velocity_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ComVelocityPacket)) return false;

      ComVelocityPacket otherMyClass = (ComVelocityPacket) other;

      if (!this.velocity_.equals(otherMyClass.velocity_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ComVelocityPacket {");
      builder.append("velocity=");
      builder.append(this.velocity_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/behaviors/BehaviorControlModeEnum.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.behaviors;

public enum BehaviorControlModeEnum
{
   STOP, PAUSE, RESUME;

   public static final BehaviorControlModeEnum[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static BehaviorControlModeEnum fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/CTTreeTools.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import java.util.Random;

import us.ihmc.robotics.robotSide.RobotSide;

public class CTTreeTools
{
   /*
    * 50 okay . why 1 has problem? angular velocity problem on chest.
    */
   static Random randomManager = new Random(3);
   static double timeCoefficient = 3.0;

   public static void setRandomNormalizedNodeData(CTTaskNode node, boolean isUniform, double treeReachingTime)
   {
      for (int i = 0; i < node.getDimensionOfNodeData(); i++)
         setRandomNormalizedNodeData(node, i, isUniform, treeReachingTime);
   }

   public static void setRandomNormalizedNodeData(CTTaskNode node, int index, boolean isUniform, double treeReachingTime)
   {
      //double exceedIntentionalTimeRatio = 3.0;
      double exceedIntentionalTimeRatio = timeCoefficient * treeReachingTime;
      // double exceedIntentionalTimeRatio = 3.0 * treeReachingTime;

      double exceedIntentionalRatio = 0.5;

      if (isUniform)
         exceedIntentionalRatio = 0.0;
      else
         exceedIntentionalRatio = 1.0;

      double value;

      if (index == 0)
         value = randomManager.nextDouble() * (1.0 + exceedIntentionalTimeRatio);
      else
      {
         value = randomManager.nextDouble() * (1.0 + exceedIntentionalRatio);
         value = value - 0.5 * exceedIntentionalRatio;

         if (value >= 1)
            value = 1;
         if (value <= 0)
            value = 0;
      }

      node.setNormalizedNodeData(index, value);
   }

   public static void setRandomConfigurationData(CTTaskNode node, int index, double lowerLimit, double upperLimit, boolean isUniform)
   {
      double exceedIntentionalRatio = 0.5;

      if (isUniform)
         exceedIntentionalRatio = 0.0;
      else
         exceedIntentionalRatio = 1.0;

      double value;

      value = randomManager.nextDouble() * (1.0 + exceedIntentionalRatio);
      value = value - 0.5 * exceedIntentionalRatio;

      if (value >= 1)
         value = 1;
      if (value <= 0)
         value = 0;

      double randomConfigurationData = lowerLimit + value * (upperLimit - lowerLimit);
      node.setNodeData(index, randomConfigurationData);
   }

   public static void setRandomTimeData(CTTaskNode node, double trajectoryTime, double treeReachingTime)
   {
      double exceedIntentionalTimeRatio = timeCoefficient * treeReachingTime;

      double value;

      value = randomManager.nextDouble() * (1.0 + exceedIntentionalTimeRatio);

      if (value > 1)
         value = 1;

      double randomTimeData = value * trajectoryTime;
      node.setTimeData(randomTimeData);
   }
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/Vertex3DSupplierList.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Iterator;
import java.util.List;
import java.util.ListIterator;

import us.ihmc.euclid.geometry.interfaces.Vertex3DSupplier;
import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;

public class Vertex3DSupplierList<T extends Point3DReadOnly> implements Vertex3DSupplier, List<T>
{
   private int firstVertexIndex;
   private int lastVertexIndex;
   private final boolean useVertexIndexingAsVertexSupplier;
   private final List<T> vertexList;

   public Vertex3DSupplierList(boolean useVertexIndexing)
   {
      vertexList = new ArrayList<>();
      useVertexIndexingAsVertexSupplier = useVertexIndexing;
   }
   
   public Vertex3DSupplierList()
   {
      this(false);
   }

   public Vertex3DSupplierList(List<T> backingList, boolean useVertexIndexing)
   {
      vertexList = backingList;
      useVertexIndexingAsVertexSupplier = useVertexIndexing;
   }
   
   public Vertex3DSupplierList(List<T> backingList)
   {
      this(backingList, false);
   }

   public List<T> getVertexList()
   {
      return vertexList;
   }
   
   public void setIndicesForVerticesToSupply(int firstIndex, int lastIndex)
   {
      checkIfVertexIndexingIsEnabled();
      if (firstIndex > lastIndex)
         throw new RuntimeException("The first index (" + firstIndex +") cannot be less than last index (" + lastIndex + ")");
      firstVertexIndex = firstIndex;
      lastVertexIndex = lastIndex;
   }

   public void setIndicesFromList()
   {
      checkIfVertexIndexingIsEnabled();
      firstVertexIndex = 0;
      lastVertexIndex = vertexList.size() - 1;
   }
   
   public void checkIfVertexIndexingIsEnabled()
   {
      if(!useVertexIndexingAsVertexSupplier)
         throw new RuntimeException("Cannot use vertex index. Option was disabled at instantiation");
   }
   
   private void checkIfVertexIndexIsValid(int indexToCheck)
   {
      checkIfVertexIndexingIsEnabled();
      if(indexToCheck < 0 && indexToCheck >= getNumberOfVertices())
         throw new IndexOutOfBoundsException("Index (" + indexToCheck + ") is not in bounds");
   }

   @Override
   public T getVertex(int index)
   {
      if(useVertexIndexingAsVertexSupplier)
      {
         checkIfVertexIndexIsValid(index);
         return vertexList.get(firstVertexIndex + index);
      }
      else
         return vertexList.get(index);
   }

   @Override
   public int getNumberOfVertices()
   {
      if (useVertexIndexingAsVertexSupplier)
         return lastVertexIndex - firstVertexIndex + 1;
      else
         return vertexList.size();
   }

   @Override
   public boolean add(T pointToAdd)
   {
      return vertexList.add(pointToAdd);
   }

   @Override
   public void add(int indexToInsertAt, T pointToInsert)
   {
      vertexList.add(indexToInsertAt, pointToInsert);
   }

   @Override
   public void clear()
   {
      vertexList.clear();
   }

   @Override
   public boolean contains(Object obejctToCheck)
   {
      return vertexList.contains(obejctToCheck);
   }

   @Override
   public boolean containsAll(Collection<?> collection)
   {
      return vertexList.contains(collection);
   }

   @Override
   public T get(int index)
   {
      return vertexList.get(index);
   }

   @Override
   public int indexOf(Object object)
   {
      return vertexList.indexOf(object);
   }

   @Override
   public boolean isEmpty()
   {
      return vertexList.isEmpty();
   }

   @Override
   public Iterator<T> iterator()
   {
      return vertexList.iterator();
   }

   @Override
   public int lastIndexOf(Object objectToGetIndexOf)
   {
      return vertexList.lastIndexOf(objectToGetIndexOf);
   }

   @Override
   public ListIterator<T> listIterator()
   {
      return vertexList.listIterator();
   }

   @Override
   public ListIterator<T> listIterator(int index)
   {
      return null;
   }

   @Override
   public boolean remove(Object objectToRemove)
   {
      return vertexList.remove(objectToRemove);
   }

   @Override
   public T remove(int index)
   {
      return vertexList.remove(index);
   }

   @Override
   public boolean removeAll(Collection<?> collection)
   {
      return vertexList.removeAll(collection);
   }

   @Override
   public boolean retainAll(Collection<?> collection)
   {
      return vertexList.retainAll(collection);
   }

   @Override
   public T set(int index, T element)
   {
      return vertexList.set(index, element);
   }

   @Override
   public int size()
   {
      return vertexList.size();
   }

   @Override
   public List<T> subList(int fromIndex, int toIndex)
   {
      return vertexList.subList(fromIndex, toIndex);
   }

   @Override
   public Object[] toArray()
   {
      return vertexList.toArray();
   }

   @Override
   public <T> T[] toArray(T[] a)
   {
      return vertexList.toArray(a);
   }

   @Override
   public boolean addAll(Collection<? extends T> collection)
   {
      return vertexList.addAll(collection);
   }

   @Override
   public boolean addAll(int index, Collection<? extends T> collection)
   {
      return vertexList.addAll(index, collection);
   }
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/RosLocalizationStatusSubscriber.java<|end_filename|>
package us.ihmc.ihmcPerception;

import controller_msgs.msg.dds.LocalizationStatusPacket;
import std_msgs.Float64;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.humanoidRobotics.kryo.PPSTimestampOffsetProvider;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.subscriber.AbstractRosTopicSubscriber;

/**
 * Created by dstephen on 3/20/15.
 */
public class RosLocalizationStatusSubscriber
{
   private double overlap = RosLocalizationConstants.DEFAULT_OVERLAP;

   public RosLocalizationStatusSubscriber(final RosMainNode rosMainNode, final PacketCommunicator packetCommunicator,
         final PPSTimestampOffsetProvider ppsTimeOffsetProvider)
   {
      AbstractRosTopicSubscriber<Float64> overlapSubscriber = new AbstractRosTopicSubscriber<std_msgs.Float64>(std_msgs.Float64._TYPE) {
         @Override
         public void onNewMessage(std_msgs.Float64 message) {
            overlap = message.getData();
            LocalizationStatusPacket localizationOverlapPacket = HumanoidMessageTools.createLocalizationStatusPacket(overlap, null);
            packetCommunicator.send(localizationOverlapPacket);
         }
      };
      rosMainNode.attachSubscriber(RosLocalizationConstants.OVERLAP_UPDATE_TOPIC, overlapSubscriber);

      AbstractRosTopicSubscriber<std_msgs.String> statusSubscriber = new AbstractRosTopicSubscriber<std_msgs.String>(std_msgs.String._TYPE) {

         @Override
         public void onNewMessage(std_msgs.String message) {
            String status = message.getData();
            LocalizationStatusPacket localizationOverlapPacket = HumanoidMessageTools.createLocalizationStatusPacket(overlap, status);
            packetCommunicator.send(localizationOverlapPacket);
         }

      };

      rosMainNode.attachSubscriber(RosLocalizationConstants.STATUS_UPDATE_TOPIC, statusSubscriber);
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerAPI/input/MessageCollector.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controllerAPI.input;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import controller_msgs.msg.dds.MessageCollection;
import controller_msgs.msg.dds.MessageCollectionNotification;
import gnu.trove.set.hash.TLongHashSet;
import us.ihmc.commons.PrintTools;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.idl.IDLSequence;

/**
 * A {@code MessageCollector} can be added to a {@link ControllerNetworkSubscriber}.
 * <p>
 * It works with {@link MessageCollection} and allows the user to ensure that messages sent in group
 * are also processed in group.
 * </p>
 * <p>
 * See {@link MessageCollectionMessenger} for setting up and sending a collection of messages that
 * are to be processed together.
 * </p>
 * 
 * @author <NAME>
 */
public class MessageCollector
{
   /** Whether this collector is currently waiting for additional messages of a collection. */
   private boolean isCollecting = false;
   /**
    * The ID set of the messages that are part of a collection. This set is empty when this
    * collector is not active, when active the set contains the ID of the messages that are still to
    * be collected.
    */
   private final TLongHashSet expectedMessageIDs;
   /** The list of the collection messages that have already been collected. */
   private final List<Settable<?>> interceptedMessages;
   /** A view list of {@link #interceptedMessages}, it is for read-only purposes. */
   private final List<Settable<?>> interceptedMessagesView;
   /**
    * The internal buffer used to save a copy of the intercepted messages without holding onto the
    * reference of a message that was created outside this collector.
    */
   private final MessagePool messagePool;
   /** Extractor used to get the ID of the messages. */
   private final MessageIDExtractor messageIDExtractor;
   /**
    * Notification that sent to notify the user that this collector has just received a
    * {@code MessageCollection} and is now active and waiting to collect the messages.
    */
   private final MessageCollectionNotification notification;

   /**
    * Dummy collector that is never active, for convenience purposes.
    * 
    * @return the dummy collector.
    */
   public static MessageCollector createDummyCollector()
   {
      return new MessageCollector();
   }

   private MessageCollector()
   {
      messageIDExtractor = null;
      messagePool = null;
      expectedMessageIDs = null;
      interceptedMessages = null;
      interceptedMessagesView = Collections.emptyList();
      notification = null;
   }

   /**
    * Creates a message collector.
    * <p>
    * A message collector is necessary to support {@code MessageCollection}.
    * </p>
    * 
    * @param messageIDExtractor provides the method for getting the IDs from the messages.
    * @param supportedMessages the list of messages that this collector can handle.
    */
   public MessageCollector(MessageIDExtractor messageIDExtractor, List<Class<? extends Settable<?>>> supportedMessages)
   {
      this.messageIDExtractor = messageIDExtractor;
      messagePool = new MessagePool(supportedMessages);
      expectedMessageIDs = new TLongHashSet();
      interceptedMessages = new ArrayList<>();
      interceptedMessagesView = Collections.unmodifiableList(interceptedMessages);
      notification = new MessageCollectionNotification();
   }

   /**
    * Disables this collector and marks its internal memory as available.
    * <p>
    * Should be called only once the intercepted messages have been processed.
    * </p>
    */
   public void reset()
   {
      if (messagePool == null)
         return;

      isCollecting = false;
      expectedMessageIDs.clear();
      interceptedMessages.clear();
      messagePool.reset();
   }

   /**
    * Enables this collector.
    * <p>
    * This collector is now waiting for the messages referred by the IDs contained in the given
    * {@code collection}.
    * </p>
    * 
    * @param collection the message holding onto the IDs of the messages to collect.
    * @return a notification ready to be sent back to the user.
    */
   public MessageCollectionNotification startCollecting(MessageCollection collection)
   {
      if (messagePool == null)
      {
         PrintTools.error("This is a dummy collector, it cannot collect messages.");
         return null;
      }
      reset();
      isCollecting = true;
      notification.setMessageCollectionSequenceId(collection.getSequenceId());

      IDLSequence.Long sequences = collection.getSequences();
      for (int i = 0; i < sequences.size(); i++)
      {
         expectedMessageIDs.add(sequences.get(i));
      }
      return notification;
   }

   /**
    * Tests the given message and hold onto it if this collector is active and was waiting for the
    * given message.
    * <p>
    * If this collector is inactive or not waiting for the given message, the message is not saved
    * and this method returns {@code false}.
    * </p>
    * 
    * @param message the message to possibly intercept.
    * @return {@code true} if the message is part of a collection that this collector is expecting,
    *         {@code false} if this collector is inactive or if the message is not part of the
    *         current collection gathered by this collector.
    */
   @SuppressWarnings({"unchecked", "rawtypes"})
   public boolean interceptMessage(Settable<?> message)
   {
      if (messagePool == null)
         return false;

      long messageID = messageIDExtractor.getMessageID(message);

      if (messageID == MessageIDExtractor.NO_ID)
         return false;

      boolean intercept = expectedMessageIDs.remove(messageID);

      if (intercept)
      {
         Settable copy = messagePool.requestMessage(message.getClass());
         copy.set(message);
         interceptedMessages.add(copy);
      }

      if (expectedMessageIDs.isEmpty())
         isCollecting = false;

      return intercept;
   }

   /**
    * Whether this collector is currently gathering messages.
    * 
    * @return {@code true} if collecting, {@code false} otherwise.
    */
   public boolean isCollecting()
   {
      if (messagePool == null)
         return true;

      return isCollecting;
   }

   /**
    * Gets a view of the collected messages.
    * <p>
    * These messages should only be processed once this collector is done collecting, i.e.
    * {@link #isCollecting()} returns {@code false}.
    * </p>
    * 
    * @return the collected messages.
    */
   public List<Settable<?>> getCollectedMessages()
   {
      return interceptedMessagesView;
   }

   /**
    * Implement this interface to provide a method for getting the ID of messages.
    * 
    * @author <NAME>
    */
   public static interface MessageIDExtractor
   {
      /**
       * Default value for marking a message as without an ID. A message without an ID can never be
       * part of a collection.
       */
      static final long NO_ID = 0;

      /**
       * Gets the ID of the given message.
       * 
       * @param message the message to get the ID of.
       * @return the ID or {@link #NO_ID} if the message should not be processed.
       */
      public long getMessageID(Object message);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/bambooTools/videoWall/BambooVideoWall.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.bambooTools.videoWall;

import java.nio.file.FileVisitResult;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import javax.swing.ImageIcon;
import javax.swing.JFrame;

import org.apache.commons.lang3.SystemUtils;

import us.ihmc.commons.nio.BasicPathVisitor;
import us.ihmc.commons.nio.PathTools;
import us.ihmc.simulationConstructionSetTools.bambooTools.BambooTools;
import us.ihmc.commons.thread.ThreadTools;

public class BambooVideoWall
{
   private static final double PLAYBACKER_HEIGHT = 1100;
   private static final int NUMBER_OF_ROWS_OF_VIDEO = 5;
   private static final int NUMBER_OF_COLUMNS_OF_VIDEO = 5;
   private static final int NUMBER_OF_SCREENS = NUMBER_OF_ROWS_OF_VIDEO * NUMBER_OF_COLUMNS_OF_VIDEO;
   private static final int VIDEO_WIDTH = 1280;
   private static final int VIDEO_HEIGHT = 720;

   private JFrame jFrame;
   private VideoPlayerGrid videoPlayerGrid;

   public BambooVideoWall()
   {
      jFrame = new JFrame(BambooVideoWall.class.getSimpleName());

      double adjustedVideoHeight = PLAYBACKER_HEIGHT / (double) NUMBER_OF_ROWS_OF_VIDEO;
      double adjustedVideoWidth = adjustedVideoHeight * VIDEO_WIDTH / VIDEO_HEIGHT;
      double width = adjustedVideoWidth * NUMBER_OF_COLUMNS_OF_VIDEO;
      
      videoPlayerGrid = new VideoPlayerGrid(NUMBER_OF_ROWS_OF_VIDEO, NUMBER_OF_COLUMNS_OF_VIDEO);
      jFrame.getContentPane().add(videoPlayerGrid);

      jFrame.setSize((int) width, (int) PLAYBACKER_HEIGHT + jFrame.getInsets().top);
      jFrame.setIconImage(new ImageIcon(ClassLoader.getSystemResource("us/ihmc/tools/icons/running-man-32x32.png").getPath()).getImage());
      jFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
      jFrame.setVisible(true);
   }

   public void startVideoWallSourcedFromErasableDirectory()
   {
      startVideoWall(new LatestDirectoryProvider()
      {
         @Override
         public Path checkForLatestDirectory()
         {
            return BambooTools.getEraseableDirectoryWithMostRecentBambooDataAndVideos();
         }
      });
   }

   public void startVideoWallSourcedFromSamson()
   {
      startVideoWall(new LatestDirectoryProvider()
      {
         @Override
         public Path checkForLatestDirectory()
         {
            if (SystemUtils.IS_OS_WINDOWS)
            {
               return BambooTools.getDirectoryWithMostRecentBambooDataAndVideos(Paths.get("//samson/BambooVideos"));
            }
            else if (SystemUtils.IS_OS_MAC)
            {
               return BambooTools.getDirectoryWithMostRecentBambooDataAndVideos(Paths.get("/Users/unknownid/BambooVideos"));
            }
            else
            {
               return null;
            }
         }
      });
   }

   private void startVideoWall(LatestDirectoryProvider latestDirectoryProvider)
   {
      Path latestDirectory = null;
      List<Path> videoList = null;

//      while (true)
//      {
         Path checkDirectory = latestDirectoryProvider.checkForLatestDirectory();

         if (latestDirectory == null || !latestDirectory.equals(checkDirectory))
         {
            latestDirectory = checkDirectory;
            videoList = getVideoPathListFromDirectory(latestDirectory);
         }
         
         showVideosOnce(videoList);

         ThreadTools.sleep(200);
//      }
   }

   private interface LatestDirectoryProvider
   {
      Path checkForLatestDirectory();
   }

   private List<Path> getVideoPathListFromDirectory(Path videoDirectory)
   {
      final List<Path> videoPaths = new ArrayList<>();

      PathTools.walkFlat(videoDirectory, new BasicPathVisitor()
      {
         @Override
         public FileVisitResult visitPath(Path path, PathType pathType)
         {
            if (pathType == PathType.FILE)
            {
               if (PathTools.getExtension(path).matches("mpg|mov|mp4"))
               {
                  videoPaths.add(path);
               }
            }

            return FileVisitResult.CONTINUE;
         }
      });

      Collections.sort(videoPaths);

      return videoPaths;
   }

   private void showVideosOnce(List<Path> videoPaths)
   {
      for (int videoIndex = 0, playerSlotIndex = 0; videoIndex < videoPaths.size(); videoIndex++, playerSlotIndex++)
      {
         if (playerSlotIndex == NUMBER_OF_SCREENS)
         {
            playerSlotIndex = 0;
         }

         while (!videoPlayerGrid.getVideoPlayers().get(playerSlotIndex).readyToUpdateVideoPath())
         {
            ThreadTools.sleep(200);
         }

         videoPlayerGrid.updateVideoPath(playerSlotIndex, videoPaths.get(videoIndex));
      }
   }

   public static void main(String[] args)
   {
      new BambooVideoWall().startVideoWallSourcedFromSamson();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/joystick/DoubleYoVariableDPADJoystickEventListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.joystick;

import net.java.games.input.Component;
import net.java.games.input.Event;
import us.ihmc.yoVariables.variable.YoDouble;
import us.ihmc.commons.MathTools;
import us.ihmc.tools.inputDevices.joystick.JoystickEventListener;

public class DoubleYoVariableDPADJoystickEventListener implements JoystickEventListener
{
   private final YoDouble variable;
   private final Component component;
   private final double min;
   private final double max;
   private final double increment;
   private final int sign;

   public DoubleYoVariableDPADJoystickEventListener(YoDouble variable, Component component, double min, double max, double increment, boolean signFlip)
   {
      if (variable == null)
         throw new RuntimeException("variable is null");
      if (min > max)
         throw new RuntimeException("min > max");
      this.variable = variable;
      this.component = component;
      this.min = min;
      this.max = max;
      this.increment = increment;
      this.sign = signFlip ? -1 : 1;
   }

   @Override
   public void processEvent(Event event)
   {
      if (event.getComponent() == component)
      {
         double value = getDPadValue(event);
         double newValue = (sign * value * increment) + variable.getDoubleValue();
         
         newValue = MathTools.clamp(newValue, min, max);
         variable.set(newValue);
      }
   }

   private double getDPadValue(Event event)
   {
      double dPadValue = event.getValue();
      double value = 0.0;
      if (MathTools.epsilonEquals(dPadValue, 0.25, 1e-3))
      {
         value = 1.0;
      }
      if (MathTools.epsilonEquals(dPadValue, 0.75, 1e-3))
      {
         value = -1.0;
      }
      return value;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/robotController/RawSensorReader.java<|end_filename|>
package us.ihmc.robotics.robotController;

import us.ihmc.simulationconstructionset.util.RobotControlElement;

public interface RawSensorReader extends RobotControlElement
{
   public abstract void read();
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/simpleArm/SimpleArmController.java<|end_filename|>
package us.ihmc.exampleSimulations.simpleArm;


import java.util.Random;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.algorithms.InverseDynamicsCalculator;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.simulationConstructionSetTools.robotController.SimpleRobotController;
import us.ihmc.yoVariables.variable.YoDouble;

/**
 * Random controller to test state estimation.
 */
public class SimpleArmController extends SimpleRobotController
{
   private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

   private final SimpleRobotInputOutputMap robot;
   private final YoDouble time;

   private static final Random random = new Random(94929438248L);
   private YoDouble noiseMagnitude = new YoDouble("NoiseMagnitude", registry);
   private YoDouble decay = new YoDouble("Decay", registry);
   private YoDouble frequency = new YoDouble("Frequency", registry);
   private YoDouble magnitude = new YoDouble("Magnitude", registry);
   private YoDouble offset = new YoDouble("Offset", registry);

   private final InverseDynamicsCalculator inverseDynamicsCalculator;

   private RigidBodyBasics endEffectorBody;

   public SimpleArmController(SimpleRobotInputOutputMap robot, RigidBodyBasics endEffectorBody, YoDouble time)
   {
      this.endEffectorBody = endEffectorBody;
      this.time = time;
      this.robot = robot;

      noiseMagnitude.set(0.5);
      decay.set(0.05);
      frequency.set(0.5);

      inverseDynamicsCalculator = new InverseDynamicsCalculator(endEffectorBody);
      inverseDynamicsCalculator.setGravitionalAcceleration(-SimpleArmRobot.gravity);
   }

   @Override
   public void doControl()
   {
      robot.readFromSimulation();

      magnitude.add(noiseMagnitude.getDoubleValue() * (random.nextDouble() - 0.5));
      offset.add(noiseMagnitude.getDoubleValue() * (random.nextDouble() - 0.5));

      double angle = 2.0 * Math.PI * frequency.getDoubleValue() * time.getDoubleValue();
      double randomTorque = magnitude.getDoubleValue() * Math.sin(angle) + offset.getDoubleValue();

      inverseDynamicsCalculator.compute();
      endEffectorBody.childrenSubtreeIterable().forEach(inverseDynamicsCalculator::writeComputedJointWrench);

      robot.addYawTorque(randomTorque);
      robot.addPitch1Torque(randomTorque);
      robot.addPitch2Torque(randomTorque);

      magnitude.mul(1.0 - decay.getDoubleValue());
      offset.mul(1.0 - decay.getDoubleValue());

      robot.writeToSimulation();
   }

}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasLeapOfFaithParameters.java<|end_filename|>
package us.ihmc.atlas.parameters;

import us.ihmc.commonWalkingControlModules.configurations.LeapOfFaithParameters;

public class AtlasLeapOfFaithParameters extends LeapOfFaithParameters
{
   private final boolean runningOnRealRobot;

   public AtlasLeapOfFaithParameters(boolean runningOnRealRobot)
   {
      this.runningOnRealRobot = runningOnRealRobot;
   }

   /** {@inheritDoc} */
   @Override
   public double getFractionOfSwingToScaleFootWeight()
   {
      return runningOnRealRobot ? 1.0 : 0.92;
   }

   /** {@inheritDoc} */
   @Override
   public double getPelvisReachingFractionOfSwing()
   {
      return runningOnRealRobot ? 1.0 : 0.90;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/robotController/RobotControllerExecutor.java<|end_filename|>
package us.ihmc.robotics.robotController;

public interface RobotControllerExecutor
{

   void waitAndWriteData(long tick);

   void readData(long tick);

   public void executeForSimulationTick(long tick);

   void initialize();

   long getTicksPerSimulationTick();
   
   void stop();

   void updateYoGraphicsListRegistry();

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/planarRegionEnvironments/TwoBollardEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.planarRegionEnvironments;

import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;

public class TwoBollardEnvironment extends PlanarRegionEnvironmentInterface
{
   private final double bollardWidth = 0.2;
   private final double bollardHeight = 0.75;

   public TwoBollardEnvironment(double barrelSeparation)
   {
      // ground plane
      generator.addRectangle(1.0, barrelSeparation + 0.5);
      generator.translate(2.0, 0.0, 0.0);
      generator.addRectangle(3.0, 3.0);
      generator.translate(-4.0, 0.0, 0.0);
      generator.addRectangle(3.0, 3.0);
      addPlanarRegionsToTerrain(YoAppearance.RGBColor(110 / 256.0, 121 / 256.0, 121 / 256.0));

      // first bollard
      generator.identity();
      generator.translate(0.0, 0.5 * barrelSeparation, 0.0);
      generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight);
      addPlanarRegionsToTerrain(YoAppearance.Yellow());

      // second bollard
      generator.identity();
      generator.translate(0.0, -0.5 * barrelSeparation, 0.0);
      generator.addCubeReferencedAtBottomMiddle(bollardWidth, bollardWidth, bollardHeight);
      addPlanarRegionsToTerrain(YoAppearance.Yellow());
   }

   public double getBollardHeight()
   {
      return bollardHeight;
   }

   public double getBollardWidth()
   {
      return bollardWidth;
   }

   public static void main(String[] args)
   {
      SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("robot"));
      scs.addStaticLinkGraphics(new TwoBollardEnvironment(0.75).getTerrainObject3D().getLinkGraphics());
      scs.setGroundVisible(false);
      scs.startOnAThread();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/ScrewTools.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.Arrays;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Stream;

import gnu.trove.list.array.TIntArrayList;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.SpatialAcceleration;
import us.ihmc.mecano.tools.MultiBodySystemTools;
import us.ihmc.robotics.geometry.TransformTools;

public class ScrewTools
{
   public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis,
                                                              boolean isPartOfClosedKinematicLoop)
   {
      return addPassiveRevoluteJoint(jointName, parentBody, TransformTools.createTranslationTransform(jointOffset), jointAxis, isPartOfClosedKinematicLoop);
   }

   public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBodyBasics parentBody, RigidBodyTransform transformToParent,
                                                              Vector3D jointAxis, boolean isPartOfClosedKinematicLoop)
   {
      return new PassiveRevoluteJoint(jointName, parentBody, transformToParent, jointAxis, isPartOfClosedKinematicLoop);
   }

   public static RigidBodyBasics[] computeSubtreeSuccessors(RigidBodyBasics... bodies)
   {
      return MultiBodySystemTools.collectSuccessors(MultiBodySystemTools.collectSubtreeJoints(bodies));
   }

   public static RigidBodyBasics[] computeSupportAndSubtreeSuccessors(RigidBodyBasics... bodies)
   {
      return MultiBodySystemTools.collectSuccessors(MultiBodySystemTools.collectSupportAndSubtreeJoints(bodies));
   }

   /**
    * Compute and pack the joint path between two RigidBody in the jointPathToPack. Use the method
    * {@link #computeDistanceToAncestor(RigidBodyBasics, RigidBodyBasics)} to get the size of the Array
    * to provide.
    * 
    * @param jointPathToPack
    * @param start
    * @param end
    * @return the length of the joint path, returns -1 if the the given jointPathToPack is too small.
    */
   public static int createJointPath(JointBasics[] jointPathToPack, RigidBodyBasics start, RigidBodyBasics end)
   {
      boolean flip = false;
      RigidBodyBasics descendant = start;
      RigidBodyBasics ancestor = end;
      int pathLength = MultiBodySystemTools.computeDistanceToAncestor(descendant, ancestor);
      if (pathLength < 0)
      {
         flip = true;
         descendant = end;
         ancestor = start;
         pathLength = MultiBodySystemTools.computeDistanceToAncestor(end, start);
      }

      if (jointPathToPack == null || jointPathToPack.length < pathLength)
         return -1;

      RigidBodyBasics currentBody = descendant;
      int i = 0;
      while (currentBody != ancestor)
      {
         int j = flip ? pathLength - 1 - i : i;
         JointBasics parentJoint = currentBody.getParentJoint();
         jointPathToPack[j] = parentJoint;
         currentBody = parentJoint.getPredecessor();
         i++;
      }

      for (int k = pathLength; k < jointPathToPack.length; k++)
         jointPathToPack[k] = null;

      return pathLength;
   }

   public static SpatialAcceleration createGravitationalSpatialAcceleration(RigidBodyBasics rootBody, double gravity)
   {
      Vector3D gravitationalAcceleration = new Vector3D(0.0, 0.0, gravity);
      Vector3D zero = new Vector3D();
      SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame(),
                                                                     zero, gravitationalAcceleration);

      return rootAcceleration;
   }

   public static void computeIndicesForJoint(JointBasics[] jointsInOrder, TIntArrayList listToPackIndices, JointBasics... jointsToComputeIndicesFor)
   {
      int startIndex = 0;
      for (int i = 0; i < jointsInOrder.length; i++)
      {
         int nDegreesOfFreedom = jointsInOrder[i].getDegreesOfFreedom();

         for (int j = 0; j < jointsToComputeIndicesFor.length; j++)
         {
            if (jointsInOrder[i] == jointsToComputeIndicesFor[j])
            {
               for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++)
               {
                  listToPackIndices.add(k);
               }
            }
         }

         startIndex += nDegreesOfFreedom;
      }
   }

   public static void computeIndexForJoint(List<? extends JointReadOnly> jointsInOrder, TIntArrayList listToPackIndices, JointReadOnly jointToComputeIndicesFor)
   {
      int startIndex = 0;
      for (int i = 0; i < jointsInOrder.size(); i++)
      {
         JointReadOnly joint = jointsInOrder.get(i);
         int nDegreesOfFreedom = joint.getDegreesOfFreedom();

         if (joint == jointToComputeIndicesFor)
         {
            for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++)
            {
               listToPackIndices.add(k);
            }
         }

         startIndex += nDegreesOfFreedom;
      }
   }

   public static void computeIndexForJoint(JointReadOnly[] jointsInOrder, TIntArrayList listToPackIndices, JointReadOnly jointToComputeIndicesFor)
   {
      int startIndex = 0;
      for (int i = 0; i < jointsInOrder.length; i++)
      {
         int nDegreesOfFreedom = jointsInOrder[i].getDegreesOfFreedom();

         if (jointsInOrder[i] == jointToComputeIndicesFor)
         {
            for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++)
            {
               listToPackIndices.add(k);
            }
         }

         startIndex += nDegreesOfFreedom;
      }
   }

   public static JointBasics[] findJointsWithNames(JointBasics[] allJoints, String... jointNames)
   {
      Set<String> jointNameSet = new HashSet<>(Arrays.asList(jointNames));
      JointBasics[] result = Stream.of(allJoints).distinct().filter(joint -> jointNameSet.contains(joint.getName())).toArray(JointBasics[]::new);

      if (result.length != jointNames.length)
         throw new RuntimeException("Not all joints could be found");

      return result;
   }

   public static RigidBodyBasics[] findRigidBodiesWithNames(RigidBodyBasics[] allBodies, String... bodyNames)
   {
      Set<String> bodyNameSet = new HashSet<>(Arrays.asList(bodyNames));
      RigidBodyBasics[] result = Stream.of(allBodies).distinct().filter(body -> bodyNameSet.contains(body.getName())).toArray(RigidBodyBasics[]::new);

      if (result.length != bodyNames.length)
         throw new RuntimeException("Not all bodies could be found");

      return result;
   }

   public static int computeGeometricJacobianHashCode(JointBasics joints[], ReferenceFrame jacobianFrame, boolean allowChangeFrame)
   {
      int jointsHashCode = 1;
      for (JointBasics joint : joints)
      {
         jointsHashCode = 31 * jointsHashCode + joint.hashCode();
      }
      if (!allowChangeFrame)
         return 31 * jointsHashCode + jacobianFrame.hashCode();
      else
         return jointsHashCode;
   }

   public static int computeGeometricJacobianHashCode(JointBasics joints[], int firstIndex, int lastIndex, ReferenceFrame jacobianFrame,
                                                      boolean allowChangeFrame)
   {
      int jointsHashCode = 1;
      for (int i = firstIndex; i <= lastIndex; i++)
      {
         jointsHashCode = 31 * jointsHashCode + joints[i].hashCode();
      }
      if (!allowChangeFrame)
         return 31 * jointsHashCode + jacobianFrame.hashCode();
      else
         return jointsHashCode;
   }

   /**
    * Will return the {@code numberOfBodies}'eth parent of the provided {@code startBody}. E.g. if
    * {@code numberOfBodies == 1} this will return the parent of the {@code startBody} and so on.
    * 
    * @throws RuntimeException if the body chain is not long enough to reach the desired parent.
    * @param startBody the body to start at.
    * @param numberOfBodies the amount of steps to go up the body chain.
    * @return the {@link RigidBodyBasics} that is {@code numberOfBodies} higher up the rigid body chain
    *         then the {@code startBody}.
    */
   public static RigidBodyBasics goUpBodyChain(RigidBodyBasics startBody, int numberOfBodies)
   {
      if (numberOfBodies == 0)
      {
         return startBody;
      }
      JointBasics parentJoint = startBody.getParentJoint();
      if (parentJoint == null)
      {
         throw new RuntimeException("Reached root body. Can not move up the chain any further.");
      }
      return goUpBodyChain(parentJoint.getPredecessor(), numberOfBodies - 1);
   }
}

<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/robotController/SliderBoardRobotController.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.robotController;

import us.ihmc.simulationconstructionset.util.RobotController;

public abstract class SliderBoardRobotController implements RobotController
{
   public abstract void initializeSliders();

   public abstract void register();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/quadTree/QuadTreeForGroundPoint.java<|end_filename|>
package us.ihmc.robotics.quadTree;

import us.ihmc.euclid.tuple3D.Point3D;

public class QuadTreeForGroundPoint extends Point3D
{
   private static final long serialVersionUID = 8682882199962837728L;
   
   private boolean registered = false;
   private QuadTreeForGroundLeaf parent;

   public QuadTreeForGroundPoint(double x, double y, double z)
   {
      super(x, y, z);
   }

   public void setParent(QuadTreeForGroundLeaf parent)
   {
      this.parent = parent;
   }
   
   public boolean isRegistered()
   {
      return registered;
   }
   
   public void setRegistered(boolean registered)
   {
      this.registered = registered;
   }
   
   public QuadTreeForGroundLeaf getParent()
   {
      return parent;
   }

   public void removeFromParent()
   {
      if(parent != null)
      {
         parent.removePoint(this);
      }
   }

   @Override
   public int hashCode()
   {
      return super.hashCode(); // Ignore the existence of the parent field. Only 2 equal points with different parent will exist at a given time anyway.
   }

   // Make sure to override, otherwise it uses Tuple3d.equals
   @Override
   public boolean equals(Object obj)
   {
      if (this == obj)
         return true;
      else
         return false;
   }

}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/clouds/detect/shape/GenerateCylinderPointVector.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.clouds.detect.shape;

import java.util.List;

import georegression.geometry.GeometryMath_F64;
import georegression.metric.ClosestPoint3D_F64;
import georegression.metric.Distance3D_F64;
import georegression.struct.line.LineParametric3D_F64;
import georegression.struct.shapes.Cylinder3D_F64;
import us.ihmc.sensorProcessing.bubo.clouds.detect.alg.ModelGeneratorCheck;
import us.ihmc.sensorProcessing.bubo.clouds.detect.alg.PointVectorNN;

/**
 * Cylinder estimation for use in {@link us.ihmc.sensorProcessing.bubo.clouds.detect.alg.PointCloudShapeDetectionSchnabel2007}.  The sphere is estimated
 * using two points and their normal vectors.  A point in the sphere's axis is found by finding the closest
 * point between the lines defined by the two points + vectors.  The axis is found by taking the cross
 * product of the two normals.  Radius is distance from cylinder's axis.  An extra point is used to validate
 * the found cylinder.
 *
 * @author <NAME>
 */
public class GenerateCylinderPointVector implements ModelGeneratorCheck<Cylinder3D_F64, PointVectorNN> {

	// tolerance cos(angle) for vector normals
	private double tolAngleCosine;
	// tolerance for each point from the sphere
	private double tolDistance;

	// used to accept or reject a model
	private CheckShapeParameters<Cylinder3D_F64> check = new CheckShapeAcceptAll<Cylinder3D_F64>();

	// line defined by two lines.  used to find sphere center
	private LineParametric3D_F64 lineA = new LineParametric3D_F64(false);
	private LineParametric3D_F64 lineB = new LineParametric3D_F64(false);

	public GenerateCylinderPointVector(double tolAngle, double tolDistance) {
		this.tolAngleCosine = Math.cos(Math.PI / 2.0 - tolAngle);
		this.tolDistance = tolDistance;
	}

	@Override
	public void setCheck(CheckShapeParameters<Cylinder3D_F64> check) {
		this.check = check;
	}

	@Override
	public boolean generate(List<PointVectorNN> dataSet, Cylinder3D_F64 output) {

		PointVectorNN pa = dataSet.get(0);
		PointVectorNN pb = dataSet.get(1);
		PointVectorNN pc = dataSet.get(2);

		lineA.p = pa.p;
		lineA.slope = pa.normal;

		lineB.p = pb.p;
		lineB.slope = pb.normal;

		// With perfect data, the closest point between the two lines defined by a point and its normal
		// will lie on the axis of the cylinder
		ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p);
		// The cylinder's axis will be along the cross product of the two normal vectors
		GeometryMath_F64.cross(pa.normal, pb.normal, output.line.slope);

		// take the normal of the slope so that it can detect if it has a value of zero, which happens if the
		// two input vectors are the same
		double n = output.line.slope.norm();
		// n should actually always be one since the point normals are normalized to one.
		if (n < 1e-8)
			return false;
		output.line.slope.x /= n;
		output.line.slope.y /= n;
		output.line.slope.z /= n;

		// set the radius to be the average distance point is from the cylinder's axis
		double ra = Distance3D_F64.distance(output.line, pa.p);
		double rb = Distance3D_F64.distance(output.line, pb.p);
		double rc = Distance3D_F64.distance(output.line, pc.p);

		output.radius = (ra + rb) / 2.0;

		// sanity check using the model parameters
		if (!check.valid(output))
			return false;

		// sanity check the model using points
		return checkModel(output, pc, ra, rb, rc);
	}

	protected final boolean checkModel(Cylinder3D_F64 output, PointVectorNN pc, double ra, double rb, double rc) {
		// check the solution
		if (Math.abs(ra - output.radius) > tolDistance)
			return false;
		if (Math.abs(rb - output.radius) > tolDistance)
			return false;
		if (Math.abs(rc - output.radius) > tolDistance)
			return false;

		// only need to check to see if one angle is off since the other two are within tolerance by definition
		double cosAngle = output.line.slope.dot(pc.normal);
		if (Math.abs(cosAngle) > tolAngleCosine)
			return false;

		return true;
	}

	@Override
	public int getMinimumPoints() {
		return 3;
	}
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/stateTransitionConditions/StartWalkingCondition.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.stateTransitionConditions;

import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.stateMachine.core.StateTransitionCondition;

public class StartWalkingCondition implements StateTransitionCondition
{
   private final RobotSide transferToSide;
   private final WalkingMessageHandler walkingMessageHandler;

   public StartWalkingCondition(RobotSide transferToSide, WalkingMessageHandler walkingMessageHandler)
   {
      this.transferToSide = transferToSide;
      this.walkingMessageHandler = walkingMessageHandler;
   }

   @Override
   public boolean testCondition(double timeInState)
   {
      return walkingMessageHandler.isNextFootstepFor(transferToSide.getOppositeSide());
   }
}

<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/HandTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.HandTrajectoryMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.HandTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class HandTrajectoryTask extends BehaviorAction
{
   private final HandTrajectoryMessage handTrajectoryMessage;
   private final HandTrajectoryBehavior handTrajectoryBehavior;

   public HandTrajectoryTask(HandTrajectoryMessage handTrajectoryMessage, HandTrajectoryBehavior handTrajectoryBehavior)
   {
      super(handTrajectoryBehavior);
      this.handTrajectoryBehavior = handTrajectoryBehavior;
      this.handTrajectoryMessage = handTrajectoryMessage;
   }

   @Override
   protected void setBehaviorInput()
   {
      handTrajectoryBehavior.setInput(handTrajectoryMessage);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/eventBased/EventType.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.eventBased;

/**
 * Event type
 */
public enum EventType {CREATED, CHANGED}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/StateEstimatorSensorDefinitions.java<|end_filename|>
package us.ihmc.sensorProcessing.simulatedSensors;

import java.util.ArrayList;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.robotics.sensors.ForceSensorDefinition;
import us.ihmc.robotics.sensors.IMUDefinition;

public class StateEstimatorSensorDefinitions
{
   private final ArrayList<OneDoFJointBasics> oneDoFJointSensorDefinitions = new ArrayList<OneDoFJointBasics>();
   private final ArrayList<IMUDefinition> imuSensorDefinitions = new ArrayList<IMUDefinition>();
   private final ArrayList<ForceSensorDefinition> forceSensorDefinitions = new ArrayList<ForceSensorDefinition>();

   public StateEstimatorSensorDefinitions()
   {
   }

   public ArrayList<OneDoFJointBasics> getJointSensorDefinitions()
   {
      return oneDoFJointSensorDefinitions;
   }

   public ArrayList<IMUDefinition> getIMUSensorDefinitions()
   {
      return imuSensorDefinitions;
   }

   public ArrayList<ForceSensorDefinition> getForceSensorDefinitions()
   {
      return forceSensorDefinitions;
   }

   public void addJointSensorDefinitions(OneDoFJointBasics[] oneDoFJoints)
   {
      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
         addJointSensorDefinition(oneDoFJoint);
   }
   public void addJointSensorDefinition(OneDoFJointBasics oneDoFJoint)
   {
      oneDoFJointSensorDefinitions.add(oneDoFJoint);
   }

   public void addIMUSensorDefinition(IMUDefinition imuDefinition)
   {
      imuSensorDefinitions.add(imuDefinition);
   }

   public void addIMUSensorDefinition(IMUDefinition[] imuDefinitions)
   {
      for (IMUDefinition imuDefinition : imuDefinitions)
         addIMUSensorDefinition(imuDefinition);
   }

   public void addForceSensorDefinition(ForceSensorDefinition forceSensorDefinition)
   {
      forceSensorDefinitions.add(forceSensorDefinition);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sliderboard/SliderboardListener.java<|end_filename|>
package us.ihmc.robotics.sliderboard;

public interface SliderboardListener
{
   /**
    * Will be called when the slider on a slider board was moved.
    *
    * @param sliderPercentage the new slider value between 0.0 and 1.0
    */
   public void sliderMoved(double sliderPercentage);
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/example/RobotParameters.java<|end_filename|>
package us.ihmc.avatar.reachabilityMap.example;

import com.google.common.base.CaseFormat;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.YoAppearance;

public class RobotParameters
{
//   private static final RigidBodyTransform endEffectorTransformToWrist = new RigidBodyTransform(new AxisAngle4d(0.0, 1.0, 0.0, Math.PI / 2.0), new Vector3d(0.0, 0.0, -0.08));
   private static final RigidBodyTransform endEffectorTransformToWrist = new RigidBodyTransform();

   public RigidBodyTransform getEndEffectorTransformToWrist()
   {
      return endEffectorTransformToWrist;
   }

   public enum RobotArmJointParameters
   {
      SHOULDER_YAW, SHOULDER_PITCH, SHOULDER_ROLL, ELBOW_YAW, ELBOW_PITCH, WRIST_YAW, WRIST_PITCH;

      public static RobotArmJointParameters getRootJoint()
      {
         return SHOULDER_YAW;
      }

      public Vector3D getJointAxis()
      {
         String name = this.toString();
         if (name.endsWith("YAW"))
            return new Vector3D(0.0, 0.0, 1.0);
         else if (name.endsWith("PITCH"))
            return new Vector3D(0.0, 1.0, 0.0);
         else if (name.endsWith("ROLL"))
            return new Vector3D(1.0, 0.0, 0.0);
         else
            throw new RuntimeException("Should not get there.");
      }

      public String getJointName(boolean capitalizeFirstLetter)
      {
         return CaseFormat.UPPER_UNDERSCORE.to(capitalizeFirstLetter ? CaseFormat.UPPER_CAMEL : CaseFormat.LOWER_CAMEL, toString());
      }

      public Vector3D getJointOffset()
      {
         switch (this)
         {
            case SHOULDER_YAW:   return new Vector3D(0.0, 0.0, 1.5);
            case SHOULDER_PITCH: return new Vector3D(0.0, 0.0, 0.0);
            case SHOULDER_ROLL:  return new Vector3D(0.0, 0.0, 0.0);
            case ELBOW_YAW:      return new Vector3D(0.0, 0.0, -0.5);
            case ELBOW_PITCH:    return new Vector3D(0.0, 0.0, 0.0);
            case WRIST_YAW:      return new Vector3D(0.0, 0.0, -0.5);
            case WRIST_PITCH:    return new Vector3D(0.0, 0.0, 0.0);
            default:             throw new RuntimeException("Should not get there.");
         }
      }

      public double getJointLowerLimit()
      {
         switch (this)
         {
            case SHOULDER_YAW:   return -Math.PI; // -1.57;
            case SHOULDER_PITCH: return -Math.PI;
            case SHOULDER_ROLL:  return -Math.PI; // 0.0;
            case ELBOW_YAW:      return -Math.PI; // -Math.PI;
            case ELBOW_PITCH:    return -Math.PI; // -2.356;
            case WRIST_YAW:      return -Math.PI; // 0.0;
            case WRIST_PITCH:    return -Math.PI; // -Math.PI / 2.0;
            default:             throw new RuntimeException("Should not get there.");
         }
      }

      public double getJointUpperLimit()
      {
         switch (this)
         {
            case SHOULDER_YAW:   return Math.PI; // 0.785;
            case SHOULDER_PITCH: return Math.PI;
            case SHOULDER_ROLL:  return Math.PI; // Math.PI;
            case ELBOW_YAW:      return Math.PI; // Math.PI;
            case ELBOW_PITCH:    return Math.PI; // 0.0;
            case WRIST_YAW:      return Math.PI; // Math.PI;
            case WRIST_PITCH:    return Math.PI; // Math.PI / 2.0;
            default:             throw new RuntimeException("Should not get there.");
         }
      }

      public RobotArmJointParameters getParentJoint()
      {
         switch (this)
         {
            case SHOULDER_YAW:   return null;
            case SHOULDER_PITCH: return SHOULDER_YAW;
            case SHOULDER_ROLL:  return SHOULDER_PITCH;
            case ELBOW_YAW:      return SHOULDER_ROLL;
            case ELBOW_PITCH:    return ELBOW_YAW;
            case WRIST_YAW:      return ELBOW_PITCH;
            case WRIST_PITCH:    return WRIST_YAW;
            default:             throw new RuntimeException("Should not get there.");
         }
      }

      public RobotArmJointParameters getChildJoint()
      {
         switch (this)
         {
            case SHOULDER_YAW:   return SHOULDER_PITCH;
            case SHOULDER_PITCH: return SHOULDER_ROLL;
            case SHOULDER_ROLL:  return ELBOW_YAW;
            case ELBOW_YAW:      return ELBOW_PITCH;
            case ELBOW_PITCH:    return WRIST_YAW;
            case WRIST_YAW:      return WRIST_PITCH;
            case WRIST_PITCH:    return null;
            default:             throw new RuntimeException("Should not get there.");
         }
      }

      public RobotArmLinkParameters getAttachedLink()
      {
         switch (this)
         {
            case SHOULDER_YAW:   return RobotArmLinkParameters.UPPER_SHOULDER;
            case SHOULDER_PITCH: return RobotArmLinkParameters.LOWER_SHOULDER;
            case SHOULDER_ROLL:  return RobotArmLinkParameters.UPPER_ARM;
            case ELBOW_YAW:      return RobotArmLinkParameters.ELBOW;
            case ELBOW_PITCH:    return RobotArmLinkParameters.LOWER_ARM;
            case WRIST_YAW:      return RobotArmLinkParameters.WRIST;
            case WRIST_PITCH:    return RobotArmLinkParameters.HAND;
            default:             throw new RuntimeException("Should not get there.");
         }
      }
   }

   public enum RobotArmLinkParameters
   {
      UPPER_SHOULDER, LOWER_SHOULDER, UPPER_ARM, ELBOW, LOWER_ARM, WRIST, HAND;

      public static RobotArmLinkParameters getEndEffector()
      {
         return HAND;
      }

      public String getLinkName()
      {
         return CaseFormat.UPPER_UNDERSCORE.to(CaseFormat.UPPER_CAMEL, toString()) + "Link";
      }

      public Graphics3DObject getLinkGraphics()
      {
         switch (this)
         {
            case UPPER_SHOULDER:
               Graphics3DObject upperShoulderGraphics = new Graphics3DObject();
               upperShoulderGraphics.addSphere(0.05, YoAppearance.White());
               return upperShoulderGraphics;
            case LOWER_SHOULDER:
               Graphics3DObject lowerShoulderGraphics = new Graphics3DObject();
               lowerShoulderGraphics.addSphere(0.05, YoAppearance.White());
               return lowerShoulderGraphics;
            case UPPER_ARM:
               Graphics3DObject upperArmGraphics = new Graphics3DObject();
               double zOffsetUpperArm = getChildJoint().getJointOffset().getZ() / 2.0;
               upperArmGraphics.translate(0.0, 0.0, zOffsetUpperArm);
               upperArmGraphics.addEllipsoid(0.05, 0.05, Math.abs(zOffsetUpperArm), YoAppearance.DarkBlue());
               return upperArmGraphics;
            case ELBOW:
               Graphics3DObject elbowGraphics = new Graphics3DObject();
               elbowGraphics.addSphere(0.04, YoAppearance.White());
               return elbowGraphics;
            case LOWER_ARM:
               Graphics3DObject lowerArmGraphics = new Graphics3DObject();
               double zOffsetLowerArm = getChildJoint().getJointOffset().getZ() / 2.0;
               lowerArmGraphics.translate(0.0, 0.0, zOffsetLowerArm);
               lowerArmGraphics.addEllipsoid(0.05, 0.05, Math.abs(zOffsetLowerArm), YoAppearance.DarkGreen());
               return lowerArmGraphics;
            case WRIST:
               Graphics3DObject wristGraphics = new Graphics3DObject();
               wristGraphics.addSphere(0.03, YoAppearance.White());
               return wristGraphics;
            case HAND:
               Graphics3DObject handGraphics = new Graphics3DObject();
               handGraphics.transform(endEffectorTransformToWrist);
               handGraphics.addEllipsoid(0.08, 0.03, 0.06, YoAppearance.Gray());
               handGraphics.addCoordinateSystem(0.25);
               return handGraphics;
            default:
               throw new RuntimeException("Should not get there.");
         }
      }

      public RobotArmJointParameters getChildJoint()
      {
         switch (this)
         {
            case UPPER_SHOULDER: return RobotArmJointParameters.SHOULDER_PITCH;
            case LOWER_SHOULDER: return RobotArmJointParameters.SHOULDER_ROLL;
            case UPPER_ARM:      return RobotArmJointParameters.ELBOW_YAW;
            case ELBOW:          return RobotArmJointParameters.ELBOW_PITCH;
            case LOWER_ARM:      return RobotArmJointParameters.WRIST_YAW;
            case WRIST:          return RobotArmJointParameters.WRIST_PITCH;
            case HAND:           return null;
            default:             throw new RuntimeException("Should not get there.");
         }
      }
   }
}

<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/CTTreeVisualizer.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import java.util.ArrayList;

public class CTTreeVisualizer
{
   private int configurationDimension;
   
   private ArrayList<CTNodeVisualizer> nodeVisualizers;
   
   public CTTreeVisualizer(CTTaskNodeTree tree, boolean showUp)
   {
      this.configurationDimension = tree.getDimensionOfTask();
      
      this.nodeVisualizers = new ArrayList<CTNodeVisualizer>();
      for(int i=0;i<configurationDimension;i++)
      {
         CTNodeVisualizer nodeVisualizer = new CTNodeVisualizer(tree.getTaskName(i+1), i+1, showUp);
         this.nodeVisualizers.add(nodeVisualizer);
      }
   }
   
   public void initialize()
   {
      for(int i=0;i<configurationDimension;i++)
      {
         nodeVisualizers.get(i).initialize();
      }
   }
   
   public void update(CTTaskNode newNode)
   {
      for(int i=0;i<configurationDimension;i++)
      {
         nodeVisualizers.get(i).updateVisualizer(newNode);
      }
   }
   
   public void update(ArrayList<CTTaskNode> path)
   {
      for(int i=0;i<configurationDimension;i++)
      {
         nodeVisualizers.get(i).updateVisualizer(path, false);
      }
   }
   
   public void showUpPath(ArrayList<CTTaskNode> path)
   {
      for(int i=0;i<configurationDimension;i++)
      {
         nodeVisualizers.get(i).updateVisualizer(path, true);
      }
   }
}

<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisOrientationControlMode.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controlModules.pelvis;

public enum PelvisOrientationControlMode
{
   WALKING_CONTROLLER,
   USER
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/importing/NeuralNetworkConfiguration.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork.importing;

import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.ActivationFunction;
import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.PassThrough;
import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.Relu;
import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.Sigmoid;

public class NeuralNetworkConfiguration
{
   private String[] inputVariableNames;
   private String[] outputVariableNames;
   private String[] activationFunctionsPerLayer;
   private int[] numberOfNeuronsPerLayer;
   private double[][] bias;
   private double[][][] weights;

   public NeuralNetworkConfiguration()
   {

   }

   public String[] getActivationFunctionsPerLayer()
   {
      return activationFunctionsPerLayer;
   }

   public void setActivationFunctionsPerLayer(String[] activationFunctionsPerLayer)
   {
      this.activationFunctionsPerLayer = activationFunctionsPerLayer;
   }

   public int[] getNumberOfNeuronsPerLayer()
   {
      return numberOfNeuronsPerLayer;
   }

   public void setNumberOfNeuronsPerLayer(int[] numberOfNeuronsPerLayer)
   {
      this.numberOfNeuronsPerLayer = numberOfNeuronsPerLayer;
   }

   public double[][] getBias()
   {
      return bias;
   }

   public void setBias(double[][] bias)
   {
      this.bias = bias;
   }

   public double[][][] getWeights()
   {
      return weights;
   }

   public void setWeights(double[][][] weights)
   {
      this.weights = weights;
   }

   public ActivationFunction[] getActivationFunctions()
   {
      ActivationFunction[] activationFunctions = new ActivationFunction[activationFunctionsPerLayer.length];
      for (int i = 0; i < activationFunctionsPerLayer.length; i++)
      {
         switch (activationFunctionsPerLayer[i])
         {
         case "RELU":
            activationFunctions[i] = new Relu();
            break;
         case "SIGMOID":
            activationFunctions[i] = new Sigmoid();
            break;
         default:
            activationFunctions[i] = new PassThrough();
            break;
         }
      }
      return activationFunctions;
   }

   public String[] getInputVariableNames()
   {
      return inputVariableNames;
   }

   public void setInputVariableNames(String[] inputVariableNames)
   {
      this.inputVariableNames = inputVariableNames;
   }

   public String[] getOutputVariableNames()
   {
      return outputVariableNames;
   }

   public void setOutputVariableNames(String[] outputVariableNames)
   {
      this.outputVariableNames = outputVariableNames;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SoleTrajectoryMessage.java<|end_filename|>
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 commands the controller to move in taskspace a foot to the desired position  while going through the specified trajectory points.
       * To execute a single straight line trajectory to reach a desired foot pose, set only one trajectory point with zero velocity and its time to be equal to the desired trajectory time.
       */
public class SoleTrajectoryMessage extends Packet<SoleTrajectoryMessage> implements Settable<SoleTrajectoryMessage>, EpsilonComparable<SoleTrajectoryMessage>
{
   public static final byte FRONT_LEFT = (byte) 0;
   public static final byte FRONT_RIGHT = (byte) 1;
   public static final byte HIND_RIGHT = (byte) 2;
   public static final byte HIND_LEFT = (byte) 3;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Specifies which foot will execute the trajectory.
            */
   public byte robot_quadrant_ = (byte) 255;
   /**
            * The position trajectory information.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage position_trajectory_;

   public SoleTrajectoryMessage()
   {
      position_trajectory_ = new controller_msgs.msg.dds.EuclideanTrajectoryMessage();
   }

   public SoleTrajectoryMessage(SoleTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(SoleTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_quadrant_ = other.robot_quadrant_;

      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.staticCopy(other.position_trajectory_, position_trajectory_);
   }

   /**
            * 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_;
   }

   /**
            * Specifies which foot will execute the trajectory.
            */
   public void setRobotQuadrant(byte robot_quadrant)
   {
      robot_quadrant_ = robot_quadrant;
   }
   /**
            * Specifies which foot will execute the trajectory.
            */
   public byte getRobotQuadrant()
   {
      return robot_quadrant_;
   }


   /**
            * The position trajectory information.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage getPositionTrajectory()
   {
      return position_trajectory_;
   }


   public static Supplier<SoleTrajectoryMessagePubSubType> getPubSubType()
   {
      return SoleTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SoleTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SoleTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_quadrant_, other.robot_quadrant_, epsilon)) return false;

      if (!this.position_trajectory_.epsilonEquals(other.position_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SoleTrajectoryMessage)) return false;

      SoleTrajectoryMessage otherMyClass = (SoleTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_quadrant_ != otherMyClass.robot_quadrant_) return false;

      if (!this.position_trajectory_.equals(otherMyClass.position_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SoleTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_quadrant=");
      builder.append(this.robot_quadrant_);      builder.append(", ");
      builder.append("position_trajectory=");
      builder.append(this.position_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/util/CustomProgressMonitor.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger.util;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Font;
import java.awt.Insets;
import java.awt.Window;
import java.io.OutputStream;
import java.io.PrintStream;

import javax.swing.BorderFactory;
import javax.swing.BoxLayout;
import javax.swing.JDialog;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.JProgressBar;
import javax.swing.JScrollPane;
import javax.swing.JTextArea;

import org.apache.commons.io.output.TeeOutputStream;

import us.ihmc.tools.gui.TextAreaOutputStream;
import us.ihmc.commons.thread.ThreadTools;


public class CustomProgressMonitor extends JDialog implements ProgressMonitorInterface
{
   private static final long serialVersionUID = 2256156304298596266L;
   private JProgressBar progressBar;
   private JTextArea commandOutput;
   private JLabel noteLabel;
   
   private TextAreaOutputStream outputStream ;
   private PrintStream printStream;
   
   private boolean echoToConsole = false;
   
   public CustomProgressMonitor()
   {
      super((Window) null, "Progress");
      setDefaultCloseOperation(JFrame.HIDE_ON_CLOSE);  
   }
   
   public CustomProgressMonitor(String message, String note, int min, int max)
   {
      this();
      initialize(message, note, min, max);
   }
   
   public void setEchoToConsole(boolean echoToConsole)
   {
      if(printStream != null)
      {
         throw new RuntimeException("Call setEchoToConsole before initialize()");
      }
      this.echoToConsole = echoToConsole;
   }
   
   @Override
   public void initialize(String message, String note, int min, int max)
   {
      JPanel mainPanel = new JPanel(new BorderLayout());
      mainPanel.setOpaque(true);;
      
      if (note != null)
      {
         noteLabel = new JLabel(note);
      }
      else
      {
         noteLabel = null;
      }

      progressBar = new JProgressBar(min, max);
      progressBar.setValue(min);
      progressBar.setStringPainted(true);

      commandOutput = new JTextArea(5, 20);
      commandOutput.setMargin(new Insets(5, 5, 5, 5));
      commandOutput.setEditable(false);
      outputStream = new TextAreaOutputStream(commandOutput);
      
      OutputStream stream;
      if(echoToConsole)
      {
         stream = new TeeOutputStream(outputStream, System.out);
      }
      else
      {
         stream = outputStream;
      }
      printStream = new PrintStream(stream, true);

      JPanel panel = new JPanel();
      panel.setLayout(new BoxLayout(panel, BoxLayout.Y_AXIS));
      if (message != null)
      {
         panel.add(new JLabel(message));
      }
      if (noteLabel != null)
      {
         panel.add(noteLabel);
      }
      panel.add(progressBar);

      mainPanel.add(panel, BorderLayout.PAGE_START);
      mainPanel.add(new JScrollPane(commandOutput), BorderLayout.CENTER);
      mainPanel.setBorder(BorderFactory.createEmptyBorder(20, 20, 20, 20));
      
      setContentPane(mainPanel);
      pack();
      setSize(800, getHeight());
      setLocationByPlatform(true);
      setVisible(true);

   }
   
   @Override
   public void setNote(String note)
   {
      noteLabel.setText(note);
   }

   @Override
   public void setProgress(int i)
   {
      progressBar.setValue(i);
   }
   
   @Override
   public PrintStream getPrintStream()
   {
      return printStream;
   }
   
   @Override
   public void close()
   {
      setVisible(false);
      dispose();
   }
   
   public static void main(String[] args)
   {
      ProgressMonitorInterface monitor = new CustomProgressMonitor("test", "testNote", 0, 100);
      PrintStream writer = monitor.getPrintStream();
      for(int i = 0; i < 100; i++)
      {
         monitor.setProgress(i);
         monitor.setNote("Progress: " + i);
         writer.println(i);
         ThreadTools.sleep(100);
      }
      
      monitor.close();

   }

   @Override
   public void setError(String string)
   {
      noteLabel.setText(string);
      noteLabel.setForeground(Color.red);
      Font font = noteLabel.getFont();
      noteLabel.setFont(new Font(font.getFontName(), Font.BOLD, font.getSize()));
      
   }

}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/modules/mocap/MocapToHeadFrameConverter.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.modules.mocap;

import java.util.HashMap;

import controller_msgs.msg.dds.RobotConfigurationData;
import optiTrack.MocapRigidBody;
import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.humanoidRobotics.communication.subscribers.HumanoidRobotDataReceiver;
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotics.partNames.NeckJointName;


/**
 * This module converts MocapRigidBodies to robot world and sends them as packets the UI understands
 * This has an Atlas specific assumption!!!!!!!! 
 * This module uses the neck_ry joint as the bridge between mocap and robot
 */
public class MocapToHeadFrameConverter
{
   HashMap<Integer, ReferenceFrame> mocapReferenceFrames = new HashMap<Integer, ReferenceFrame>();
   HashMap<Integer, RigidBodyTransform> mocapRigidBodyTransforms = new HashMap<Integer, RigidBodyTransform>();
   
   /** Get robot configuration from controller **/
   private final HumanoidRobotDataReceiver robotDataReceiver;

   /** Atlas specific assumption!!! head frame for atlas coincides with neck_ry joint */
   private final ReferenceFrame robotHeadFrame;
   
   /** in world frame, uses mocapHeadPoseInZUp as it's transform to parent */
   private final ReferenceFrame mocapHeadFrame;
   
   /** in world frame, uses mocapHeadPoseInZUp as it's transform to parent */
   private final ReferenceFrame mocapOffsetFrame;
   
   /** the transform from mocap head in z-up to mocap origin */
   private final RigidBodyTransform mocapHeadPoseInZUp = new RigidBodyTransform();
   
   /** the change in frame between head in mocap to head in robot world */
   private final RigidBodyTransform transformFromMocapHeadToRobotHead = new RigidBodyTransform();
   
   /** the transform from the mocap centroid to robot head root */
   private final RigidBodyTransform transformFromMocapHeadCentroidToHeadRoot = new RigidBodyTransform();
   
   /** the calibration transform found by aligning the point cloud with the mocap objects*/
   private final RigidBodyTransform mocapJigCalibrationTransform = new RigidBodyTransform();
   
   /** used to pack the mocap pose and multiply the calibration and transform to headroot */
   private final RigidBodyTransform workingRigidBodyTransform = new RigidBodyTransform();

   private boolean enableMocapUpdates = false;

   private ReferenceFrame mocapOrigin = new ReferenceFrame("mocapOrigin", ReferenceFrame.getWorldFrame())
   {
      
      @Override
      protected void updateTransformToParent(RigidBodyTransform transformToParent)
      {
         transformToParent.setIdentity();
      }
   };
   
   public MocapToHeadFrameConverter(DRCRobotModel robotModel, PacketCommunicator mocapModulePacketCommunicator)
   {
      FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
      robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, null);
      HumanoidReferenceFrames referenceFrames = robotDataReceiver.getReferenceFrames();
      robotHeadFrame = referenceFrames.getNeckFrame(NeckJointName.PROXIMAL_NECK_PITCH);
      

      mocapHeadFrame = new ReferenceFrame("headInMocapFrame", mocapOrigin)
      {
         @Override
         protected void updateTransformToParent(RigidBodyTransform transformToParent)
         {
            transformToParent.set(mocapHeadPoseInZUp);
         }
      };
      
      mocapOffsetFrame = new ReferenceFrame("mocapOffsetFrame", mocapOrigin)
      {
         
         @Override
         protected void updateTransformToParent(RigidBodyTransform transformToParent)
         {
            transformToParent.set(transformFromMocapHeadToRobotHead);
            
         }
      };
      
      mocapModulePacketCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver);
   }
   
   public void setTransformFromMocapCentroidToHeadRoot(RigidBodyTransform transformFromMocapCentroidToHeadRoot)
   {
      this.transformFromMocapHeadCentroidToHeadRoot.set(transformFromMocapCentroidToHeadRoot);
   }
   
   public void setMocapJigCalibrationTransform(RigidBodyTransform headJigCalibrationTransform)
   {
      this.mocapJigCalibrationTransform.set(headJigCalibrationTransform);
   }
   
   public void update(MocapRigidBody mocapObject)
   {
      if(enableMocapUpdates)
      {
         mocapObject.packPose(workingRigidBodyTransform);
         workingRigidBodyTransform.multiply(mocapJigCalibrationTransform);
         workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot);
         mocapHeadPoseInZUp.set(workingRigidBodyTransform);
         
         mocapHeadFrame.update();
         robotDataReceiver.updateRobotModel();
         mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame);
         mocapOffsetFrame.update();
      }
   }

   public RigidBodyTransform convertMocapPoseToRobotFrame(MocapRigidBody mocapRigidBody)
   {
      int id = mocapRigidBody.getId();
      
      if(!mocapReferenceFrames.containsKey(id))
      {
         ReferenceFrame mocapObjectFrame = createReferenceFrameForMocapObject(id);
         mocapReferenceFrames.put(id, mocapObjectFrame);
      }
      
      mocapRigidBody.packPose(mocapRigidBodyTransforms.get(id));
      
      ReferenceFrame referenceFrame = mocapReferenceFrames.get(id);
      referenceFrame.update();
      
      return referenceFrame.getTransformToDesiredFrame(mocapOffsetFrame);
   }

   private ReferenceFrame createReferenceFrameForMocapObject(int id)
   {
      final RigidBodyTransform mocapRigidBodyTransform = new RigidBodyTransform();
      mocapRigidBodyTransforms.put(id, mocapRigidBodyTransform);
      ReferenceFrame mocapObjectFrame = new ReferenceFrame("mocapObject" + id, mocapOrigin )
      {
         @Override
         protected void updateTransformToParent(RigidBodyTransform transformToParent)
         {
            transformToParent.set(mocapRigidBodyTransform);
         }
      };
      return mocapObjectFrame;
   }

   public void enableMocapUpdates(boolean freezeMocapUpdates)
   {
      this.enableMocapUpdates  = freezeMocapUpdates;
      
   }
}


<|start_filename|>simulation-construction-set-tools/src/test/java/us/ihmc/simulationConstructionSetTools/simulationTesting/NothingChangedVerifier.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationTesting;

import java.io.File;
import java.net.InetAddress;
import java.util.ArrayList;

import javax.swing.JOptionPane;

import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.util.ControllerFailureException;
import us.ihmc.simulationconstructionset.util.simulationRunner.BlockingSimulationRunner;
import us.ihmc.simulationconstructionset.util.simulationRunner.BlockingSimulationRunner.SimulationExceededMaximumTimeException;
import us.ihmc.simulationconstructionset.util.simulationRunner.StateFileComparer;
import us.ihmc.simulationconstructionset.util.simulationRunner.VariableDifference;
import us.ihmc.commons.thread.ThreadTools;

public class NothingChangedVerifier
{
   private final String baseFilename;
   private final String newFilename;
   private final boolean writeNewBaseFile;
   
   private final SimulationConstructionSet scs;
   private final BlockingSimulationRunner blockingSimulationRunner;
   
   public NothingChangedVerifier(String runName, SimulationConstructionSet scs)
   {
      this.scs = scs;
      blockingSimulationRunner = new BlockingSimulationRunner(scs, 600.0);

      String pathName = "StateComparison";
      File dir = new File(pathName);
      if (!dir.exists())
      {
         dir.mkdir();
      }

      String computerName = getComputerName();
      baseFilename = pathName + "/" + runName + "_base." + computerName + ".state";
      newFilename = pathName + "/" + runName + "_new." + computerName + ".state";

      File baseFile = new File(baseFilename);

      writeNewBaseFile = !baseFile.exists();
      if (writeNewBaseFile)
      {
         JOptionPane.showMessageDialog(null, "Warning! " + baseFilename + " doesn't exist. Creating it.");
      }
      
      //TODO: Not sure if we still need this sleep or not.
      ThreadTools.sleep(1000); // give scs time to start, otherwise isRunning() will return false.
   }
   
   public boolean getWriteNewBaseFile()
   {
      return writeNewBaseFile;
   }
   
   public void runSimulation(double simTime) throws SimulationExceededMaximumTimeException, ControllerFailureException
   {
      if (simTime > 0.0)
         blockingSimulationRunner.simulateAndBlock(simTime);
   }
   
   public void runAndVerifySameResultsAsPreviously(double simTime, double maxPercentDifference, ArrayList<String> stringsToIgnore) throws SimulationExceededMaximumTimeException, ControllerFailureException
   {
      runSimulation(simTime);
      verifySameResultsAsPreviously(maxPercentDifference, stringsToIgnore);
   }
   
   public void verifySameResultsAsPreviously(double maxPercentDifference, ArrayList<String> stringsToIgnore)
   {     
      if (writeNewBaseFile)
      {
         scs.writeState(baseFilename);
      }
      else
      {
         scs.writeState(newFilename);
         ArrayList<VariableDifference> variableDifferences = StateFileComparer.percentualCompareStateFiles(baseFilename, newFilename, maxPercentDifference, stringsToIgnore);

         if (variableDifferences.isEmpty())
            System.out.println("Sims are the same!");

         else
         {
            String message = baseFilename + " and " + newFilename + " are not the same.";
            
            System.err.println(message + "Changed variables:\n" + VariableDifference.allVariableDifferencesToString(variableDifferences));
            JOptionPane.showMessageDialog(null, message);
            System.err.flush();
         }
      }
   }
   
   private static String getComputerName()
   {
      try
      {
         String computerName = InetAddress.getLocalHost().getHostName();
         return computerName;
      }
      catch (Exception e)
      {
         System.out.println("Exception caught ="+e.getMessage());
         return "unknown";
      }

   }

}


<|start_filename|>ihmc-simulation-toolkit/src/main/java/us/ihmc/simulationToolkit/outputWriters/PerfectPositionControlSimulatedOutputWriter.java<|end_filename|>
package us.ihmc.simulationToolkit.outputWriters;

import java.util.ArrayList;

import org.apache.commons.lang3.tuple.ImmutablePair;

import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.robotModels.OutputWriter;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputReadOnly;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.FloatingRootJointRobot;
import us.ihmc.simulationconstructionset.OneDegreeOfFreedomJoint;

public class PerfectPositionControlSimulatedOutputWriter implements OutputWriter
{
   private final String name;
   protected final FloatingRootJointRobot robot;
   protected ImmutablePair<FloatingJoint, FloatingJointBasics> rootJointPair;
   protected final ArrayList<ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics>> revoluteJoints = new ArrayList<ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics>>();
   private final JointDesiredOutputListReadOnly jointDesiredOutputListReadOnly;

   public PerfectPositionControlSimulatedOutputWriter(FloatingRootJointRobot robot, FullRobotModel fullRobotModel,
                                                      JointDesiredOutputListReadOnly jointDesiredOutputList)
   {
      this.name = robot.getName() + "SimulatedSensorReader";
      this.robot = robot;
      this.jointDesiredOutputListReadOnly = jointDesiredOutputList;

      setFullRobotModel(fullRobotModel);
   }

   @Override
   public void initialize()
   {
   }

   @Override
   public void setFullRobotModel(FullRobotModel fullRobotModel)
   {
      revoluteJoints.clear();
      OneDoFJointBasics[] revoluteJointsArray = fullRobotModel.getOneDoFJoints();

      for (OneDoFJointBasics revoluteJoint : revoluteJointsArray)
      {
         String name = revoluteJoint.getName();
         OneDegreeOfFreedomJoint oneDoFJoint = robot.getOneDegreeOfFreedomJoint(name);

         ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = new ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics>(oneDoFJoint, revoluteJoint);
         this.revoluteJoints.add(jointPair);
      }

      rootJointPair = new ImmutablePair<FloatingJoint, FloatingJointBasics>(robot.getRootJoint(), fullRobotModel.getRootJoint());
   }

   public String getName()
   {
      return name;
   }

   public String getDescription()
   {
      return getName();
   }

   @Override
   public void write()
   {
      for (int i = 0; i < revoluteJoints.size(); i++)
      {
         ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i);
         OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
         OneDoFJointBasics revoluteJoint = jointPair.getRight();

         JointDesiredOutputReadOnly jointOutput = jointDesiredOutputListReadOnly.getJointDesiredOutput(revoluteJoint);

         if (jointOutput.hasDesiredPosition())
            pinJoint.setQ(jointOutput.getDesiredPosition());
         if (jointOutput.hasDesiredVelocity())
            pinJoint.setQd(jointOutput.getDesiredVelocity());
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SelectionMatrix3DMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "SelectionMatrix3DMessage" defined in "SelectionMatrix3DMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from SelectionMatrix3DMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit SelectionMatrix3DMessage_.idl instead.
*
*/
public class SelectionMatrix3DMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.SelectionMatrix3DMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::SelectionMatrix3DMessage_";

   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.SelectionMatrix3DMessage 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.SelectionMatrix3DMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.SelectionMatrix3DMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.SelectionMatrix3DMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);


      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.SelectionMatrix3DMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_11(data.getSelectionFrameId());

      cdr.write_type_7(data.getXSelected());

      cdr.write_type_7(data.getYSelected());

      cdr.write_type_7(data.getZSelected());

   }

   public static void read(controller_msgs.msg.dds.SelectionMatrix3DMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setSelectionFrameId(cdr.read_type_11());
      	
      data.setXSelected(cdr.read_type_7());
      	
      data.setYSelected(cdr.read_type_7());
      	
      data.setZSelected(cdr.read_type_7());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.SelectionMatrix3DMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_11("selection_frame_id", data.getSelectionFrameId());
      ser.write_type_7("x_selected", data.getXSelected());
      ser.write_type_7("y_selected", data.getYSelected());
      ser.write_type_7("z_selected", data.getZSelected());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.SelectionMatrix3DMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setSelectionFrameId(ser.read_type_11("selection_frame_id"));
      data.setXSelected(ser.read_type_7("x_selected"));
      data.setYSelected(ser.read_type_7("y_selected"));
      data.setZSelected(ser.read_type_7("z_selected"));
   }

   public static void staticCopy(controller_msgs.msg.dds.SelectionMatrix3DMessage src, controller_msgs.msg.dds.SelectionMatrix3DMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.SelectionMatrix3DMessage createData()
   {
      return new controller_msgs.msg.dds.SelectionMatrix3DMessage();
   }
   @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.SelectionMatrix3DMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.SelectionMatrix3DMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.SelectionMatrix3DMessage src, controller_msgs.msg.dds.SelectionMatrix3DMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public SelectionMatrix3DMessagePubSubType newInstance()
   {
      return new SelectionMatrix3DMessagePubSubType();
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/frames/CommonLeggedReferenceFrames.java<|end_filename|>
package us.ihmc.sensorProcessing.frames;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.robotSide.RobotSegment;
import us.ihmc.robotics.robotSide.SegmentDependentList;

import java.util.EnumMap;

public interface CommonLeggedReferenceFrames<E extends Enum<E> & RobotSegment<E>> extends ReferenceFrames
{
   SegmentDependentList<E, MovingReferenceFrame> getAnkleZUpReferenceFrames();

   SegmentDependentList<E, MovingReferenceFrame> getFootReferenceFrames();

   SegmentDependentList<E, MovingReferenceFrame> getSoleFrames();

   MovingReferenceFrame getAnkleZUpFrame(E robotSegment);

   MovingReferenceFrame getFootFrame(E robotSegment);

   MovingReferenceFrame getLegJointFrame(E robotSegment, LegJointName legJointName);

   EnumMap<LegJointName, MovingReferenceFrame> getLegJointFrames(E robotSegment);

   ReferenceFrame getIMUFrame();

   MovingReferenceFrame getSoleFrame(E robotSegment);

   MovingReferenceFrame getSoleZUpFrame(E robotSegment);

   SegmentDependentList<E, MovingReferenceFrame> getSoleZUpFrames();

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/FrameScalableBoundingBox3d.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.geometry.BoundingBox3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;

public class FrameScalableBoundingBox3d
{
   private final ReferenceFrame frame;
   private BoundingBox3D box;

   private final Point3D dimensions;
   private final Point3D center;

   // TODO make this whole class less garbage-full
   public FrameScalableBoundingBox3d(ReferenceFrame frame, Point3D dimensions, Point3D center)
   {
      this.frame = frame;
      this.dimensions = dimensions;
      this.center = center;

      setScale(1.0f);
   }

   public void setScale(float scale)
   {
      Point3D min = new Point3D(dimensions);
      Point3D max = new Point3D(dimensions);

      min.scaleAdd(-scale, center);
      max.scaleAdd(scale, center);

      this.box = new BoundingBox3D(min, max);
   }

   public boolean contains(double x, double y, double z)
   {
      return contains(new Point3D(x, y, z));
   }

   public boolean contains(Point3D p)
   {
      Point3D local = new Point3D(p);
      frame.getTransformToRoot().inverseTransform(local);

      return box.isInsideInclusive(local);
   }

   public boolean intersects(Point3D start, Point3D end)
   {
      Point3D inFrameStart = new Point3D(start);
      frame.getTransformToRoot().inverseTransform(inFrameStart);
      Point3D inFrameEnd = new Point3D(end);
      frame.getTransformToRoot().inverseTransform(inFrameEnd);

      return box.doesIntersectWithLineSegment3D(start, end);
   }
}


<|start_filename|>ihmc-state-estimation/src/main/java/us/ihmc/stateEstimation/humanoid/kinematicsBasedStateEstimation/IMUBiasProvider.java<|end_filename|>
package us.ihmc.stateEstimation.humanoid.kinematicsBasedStateEstimation;

import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.sensorProcessing.stateEstimation.IMUSensorReadOnly;

public interface IMUBiasProvider
{

   void getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu, Vector3D angularVelocityBiasToPack);

   void getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector3D angularVelocityBiasToPack);

   void getAngularVelocityBiasInWorldFrame(IMUSensorReadOnly imu, Vector3D angularVelocityBiasToPack);

   void getAngularVelocityBiasInWorldFrame(IMUSensorReadOnly imu, FrameVector3D angularVelocityBiasToPack);

   void getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu, Vector3D linearAccelerationBiasToPack);

   void getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector3D linearAccelerationBiasToPack);

   void getLinearAccelerationBiasInWorldFrame(IMUSensorReadOnly imu, Vector3D linearAccelerationBiasToPack);

   void getLinearAccelerationBiasInWorldFrame(IMUSensorReadOnly imu, FrameVector3D linearAccelerationBiasToPack);
}

<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/controllerCore/robotArmWithFixedBase/FixedBaseRobotArmSimulation.java<|end_filename|>
package us.ihmc.exampleSimulations.controllerCore.robotArmWithFixedBase;

import us.ihmc.commonWalkingControlModules.controllerCore.WholeBodyControllerCoreMode;
import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
import us.ihmc.simulationConstructionSetTools.util.inputdevices.MidiSliderBoard;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.SimulationConstructionSetParameters;

public class FixedBaseRobotArmSimulation
{
   public static void main(String[] args)
   {
      double controlDT = 5.0e-5;
      YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry();

      FixedBaseRobotArm robotArm = new FixedBaseRobotArm(controlDT);
      FixedBaseRobotArmController robotArmController = new FixedBaseRobotArmController(robotArm, controlDT, yoGraphicsListRegistry);
      robotArmController.registerControllerCoreModeChangedListener((mode) -> robotArm.setDynamic(mode == WholeBodyControllerCoreMode.INVERSE_DYNAMICS));
      robotArm.setController(robotArmController);

      SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters();
      parameters.setDataBufferSize((int) Math.pow(2, 16)); // => 65536
      SimulationConstructionSet scs = new SimulationConstructionSet(robotArm, parameters);
      setupSliderBoard(scs);
      scs.setFastSimulate(true, 15);
      scs.addYoGraphicsListRegistry(yoGraphicsListRegistry, true);
      scs.setDT(controlDT, 10);
      scs.startOnAThread();
   }

   private static void setupSliderBoard(SimulationConstructionSet scs)
   {
      MidiSliderBoard midiSliderBoard = new MidiSliderBoard(scs);
      int sliderIndex = 1;
      midiSliderBoard.setSlider(sliderIndex++, "handTargetX", scs, -1.0, 1.0);
      midiSliderBoard.setSlider(sliderIndex++, "handTargetY", scs, -1.0, 1.0);
      midiSliderBoard.setSlider(sliderIndex++, "handTargetZ", scs,  0.0, 2.0);
      midiSliderBoard.setSlider(sliderIndex++, "handTargetYaw", scs, -Math.PI, Math.PI);
      midiSliderBoard.setSlider(sliderIndex++, "handTargetPitch", scs, -Math.PI, Math.PI);
      midiSliderBoard.setSlider(sliderIndex++, "handTargetRoll", scs, -Math.PI, Math.PI);

      int buttonIndex = 1;
      midiSliderBoard.setButton(buttonIndex++, "goToTarget", scs);
      midiSliderBoard.setButton(buttonIndex++, "setRandomConfiguration", scs);

      buttonIndex = 9; // Second row
      midiSliderBoard.setButton(buttonIndex++, "controlLinearX", scs);
      midiSliderBoard.setButton(buttonIndex++, "controlLinearY", scs);
      midiSliderBoard.setButton(buttonIndex++, "controlLinearZ", scs);
      midiSliderBoard.setButton(buttonIndex++, "controlAngularX", scs);
      midiSliderBoard.setButton(buttonIndex++, "controlAngularY", scs);
      midiSliderBoard.setButton(buttonIndex++, "controlAngularZ", scs);
   }
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/util/MultipleFolderSelectionDialog.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger.util;

import java.io.File;

import javax.swing.JFileChooser;

public class MultipleFolderSelectionDialog
{
   public static File[] getDirectories()
   {
      final JFileChooser folderDialog = new JFileChooser("Choose logging directories");
      folderDialog.setMultiSelectionEnabled(true);
      folderDialog.setFileSelectionMode(JFileChooser.DIRECTORIES_ONLY);
      
      int selection = folderDialog.showOpenDialog(null);
      if (selection != JFileChooser.APPROVE_OPTION) {
          return null;
      }
      
      File[] folders = folderDialog.getSelectedFiles();
      return folders;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/linearAlgebra/DampedLeastSquaresNullspaceCalculatorTest.java<|end_filename|>
package us.ihmc.robotics.linearAlgebra;

public class DampedLeastSquaresNullspaceCalculatorTest extends DampedNullspaceCalculatorTest
{
   @Override
   public DampedNullspaceCalculator getDampedNullspaceProjectorCalculator()
   {
      return new DampedLeastSquaresNullspaceCalculator(10, 0.0);
   }

   @Override
   public NullspaceCalculator getNullspaceProjectorCalculator()
   {
      return getDampedNullspaceProjectorCalculator();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.stream.Stream;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.Momentum;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.interfaces.SpatialInertiaBasics;

public class MomentumCalculator
{
   private final Twist tempTwist = new Twist();
   private final Momentum tempMomentum = new Momentum();
   private final Vector3D zero = new Vector3D();
   private final RigidBodyBasics[] rigidBodiesInOrders;

   public MomentumCalculator(RigidBodyBasics... rigidBodies)
   {
      rigidBodiesInOrders = Stream.of(rigidBodies).filter(body -> body.getInertia() != null).toArray(RigidBodyBasics[]::new);
   }

   public MomentumCalculator(RigidBodyBasics rootBody)
   {
      this(rootBody.subtreeArray());
   }

   public void computeAndPack(Momentum momentum)
   {
      momentum.getAngularPart().set(zero);
      momentum.getLinearPart().set(zero);

      for (RigidBodyBasics rigidBody : rigidBodiesInOrders)
      {
         SpatialInertiaBasics inertia = rigidBody.getInertia();
         rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist);
         tempMomentum.setReferenceFrame(inertia.getReferenceFrame());
         tempMomentum.compute(inertia, tempTwist);
         tempMomentum.changeFrame(momentum.getReferenceFrame());
         momentum.add(tempMomentum);
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/IntrinsicParametersMessage.java<|end_filename|>
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 used to provides additional properties for cameras.
       */
public class IntrinsicParametersMessage extends Packet<IntrinsicParametersMessage> implements Settable<IntrinsicParametersMessage>, EpsilonComparable<IntrinsicParametersMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int width_;
   public int height_;
   public double fx_;
   public double fy_;
   public double skew_;
   public double cx_;
   public double cy_;
   public us.ihmc.idl.IDLSequence.Double  radial_;
   public double t1_;
   public double t2_;

   public IntrinsicParametersMessage()
   {
      radial_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

   }

   public IntrinsicParametersMessage(IntrinsicParametersMessage other)
   {
      this();
      set(other);
   }

   public void set(IntrinsicParametersMessage other)
   {
      sequence_id_ = other.sequence_id_;

      width_ = other.width_;

      height_ = other.height_;

      fx_ = other.fx_;

      fy_ = other.fy_;

      skew_ = other.skew_;

      cx_ = other.cx_;

      cy_ = other.cy_;

      radial_.set(other.radial_);
      t1_ = other.t1_;

      t2_ = other.t2_;

   }

   /**
            * 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 void setWidth(int width)
   {
      width_ = width;
   }
   public int getWidth()
   {
      return width_;
   }

   public void setHeight(int height)
   {
      height_ = height;
   }
   public int getHeight()
   {
      return height_;
   }

   public void setFx(double fx)
   {
      fx_ = fx;
   }
   public double getFx()
   {
      return fx_;
   }

   public void setFy(double fy)
   {
      fy_ = fy;
   }
   public double getFy()
   {
      return fy_;
   }

   public void setSkew(double skew)
   {
      skew_ = skew;
   }
   public double getSkew()
   {
      return skew_;
   }

   public void setCx(double cx)
   {
      cx_ = cx;
   }
   public double getCx()
   {
      return cx_;
   }

   public void setCy(double cy)
   {
      cy_ = cy;
   }
   public double getCy()
   {
      return cy_;
   }


   public us.ihmc.idl.IDLSequence.Double  getRadial()
   {
      return radial_;
   }

   public void setT1(double t1)
   {
      t1_ = t1;
   }
   public double getT1()
   {
      return t1_;
   }

   public void setT2(double t2)
   {
      t2_ = t2;
   }
   public double getT2()
   {
      return t2_;
   }


   public static Supplier<IntrinsicParametersMessagePubSubType> getPubSubType()
   {
      return IntrinsicParametersMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return IntrinsicParametersMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(IntrinsicParametersMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.width_, other.width_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.height_, other.height_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.fx_, other.fx_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.fy_, other.fy_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.skew_, other.skew_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.cx_, other.cx_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.cy_, other.cy_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.radial_, other.radial_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.t1_, other.t1_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.t2_, other.t2_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof IntrinsicParametersMessage)) return false;

      IntrinsicParametersMessage otherMyClass = (IntrinsicParametersMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.width_ != otherMyClass.width_) return false;

      if(this.height_ != otherMyClass.height_) return false;

      if(this.fx_ != otherMyClass.fx_) return false;

      if(this.fy_ != otherMyClass.fy_) return false;

      if(this.skew_ != otherMyClass.skew_) return false;

      if(this.cx_ != otherMyClass.cx_) return false;

      if(this.cy_ != otherMyClass.cy_) return false;

      if (!this.radial_.equals(otherMyClass.radial_)) return false;
      if(this.t1_ != otherMyClass.t1_) return false;

      if(this.t2_ != otherMyClass.t2_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("IntrinsicParametersMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("width=");
      builder.append(this.width_);      builder.append(", ");
      builder.append("height=");
      builder.append(this.height_);      builder.append(", ");
      builder.append("fx=");
      builder.append(this.fx_);      builder.append(", ");
      builder.append("fy=");
      builder.append(this.fy_);      builder.append(", ");
      builder.append("skew=");
      builder.append(this.skew_);      builder.append(", ");
      builder.append("cx=");
      builder.append(this.cx_);      builder.append(", ");
      builder.append("cy=");
      builder.append(this.cy_);      builder.append(", ");
      builder.append("radial=");
      builder.append(this.radial_);      builder.append(", ");
      builder.append("t1=");
      builder.append(this.t1_);      builder.append(", ");
      builder.append("t2=");
      builder.append(this.t2_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/NeckTrajectoryMessage.java<|end_filename|>
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 commands the controller to move the neck in jointspace to the desired joint angles while going through the specified trajectory points.
       * A third order polynomial function is used to interpolate between trajectory points.
       */
public class NeckTrajectoryMessage extends Packet<NeckTrajectoryMessage> implements Settable<NeckTrajectoryMessage>, EpsilonComparable<NeckTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The trajectories for each joint in order from the one closest to the chest to the one the closest to the head.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage jointspace_trajectory_;

   public NeckTrajectoryMessage()
   {
      jointspace_trajectory_ = new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
   }

   public NeckTrajectoryMessage(NeckTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(NeckTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.staticCopy(other.jointspace_trajectory_, jointspace_trajectory_);
   }

   /**
            * 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_;
   }


   /**
            * The trajectories for each joint in order from the one closest to the chest to the one the closest to the head.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage getJointspaceTrajectory()
   {
      return jointspace_trajectory_;
   }


   public static Supplier<NeckTrajectoryMessagePubSubType> getPubSubType()
   {
      return NeckTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return NeckTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(NeckTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.jointspace_trajectory_.epsilonEquals(other.jointspace_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof NeckTrajectoryMessage)) return false;

      NeckTrajectoryMessage otherMyClass = (NeckTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.jointspace_trajectory_.equals(otherMyClass.jointspace_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("NeckTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("jointspace_trajectory=");
      builder.append(this.jointspace_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LocalizationPacket.java<|end_filename|>
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;

/**
       * Message part of the localization module
       */
public class LocalizationPacket extends Packet<LocalizationPacket> implements Settable<LocalizationPacket>, EpsilonComparable<LocalizationPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean reset_;
   public boolean toggle_;

   public LocalizationPacket()
   {
   }

   public LocalizationPacket(LocalizationPacket other)
   {
      this();
      set(other);
   }

   public void set(LocalizationPacket other)
   {
      sequence_id_ = other.sequence_id_;

      reset_ = other.reset_;

      toggle_ = other.toggle_;

   }

   /**
            * 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 void setReset(boolean reset)
   {
      reset_ = reset;
   }
   public boolean getReset()
   {
      return reset_;
   }

   public void setToggle(boolean toggle)
   {
      toggle_ = toggle;
   }
   public boolean getToggle()
   {
      return toggle_;
   }


   public static Supplier<LocalizationPacketPubSubType> getPubSubType()
   {
      return LocalizationPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return LocalizationPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(LocalizationPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.reset_, other.reset_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.toggle_, other.toggle_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof LocalizationPacket)) return false;

      LocalizationPacket otherMyClass = (LocalizationPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.reset_ != otherMyClass.reset_) return false;

      if(this.toggle_ != otherMyClass.toggle_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("LocalizationPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("reset=");
      builder.append(this.reset_);      builder.append(", ");
      builder.append("toggle=");
      builder.append(this.toggle_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/FootstepParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

/**
 * Created by agrabertilton on 2/20/15.
 */
public interface FootstepParameters
{
   public abstract double getFootForwardOffset();

   public abstract double getFootBackwardOffset();

   public abstract double getFootWidth();

   public abstract double getToeWidth();

   public abstract double getFootLength();

   public abstract double getActualFootWidth();

   public abstract double getActualFootLength();

   public default double getFootstepArea()
   {
      return (getToeWidth() + getFootWidth()) * getFootLength() / 2.0;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/JointSettingConfiguration.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates;

import java.util.List;

import us.ihmc.commonWalkingControlModules.configurations.GroupParameter;
import us.ihmc.commonWalkingControlModules.configurations.HighLevelControllerParameters;
import us.ihmc.commonWalkingControlModules.controllerCore.parameters.JointAccelerationIntegrationParametersReadOnly;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.sensorProcessing.outputData.JointDesiredBehaviorReadOnly;

public interface JointSettingConfiguration
{
   public List<GroupParameter<JointAccelerationIntegrationParametersReadOnly>> getJointAccelerationIntegrationParameters();

   public List<GroupParameter<JointAccelerationIntegrationParametersReadOnly>> getJointAccelerationIntegrationParametersUnderLoad();

   public List<GroupParameter<JointDesiredBehaviorReadOnly>> getDesiredJointBehaviors();

   public List<GroupParameter<JointDesiredBehaviorReadOnly>> getDesiredJointBehaviorsUnderLoad();

   static JointSettingConfiguration extract(HighLevelControllerParameters parameters, HighLevelControllerName stateEnum)
   {
      return new JointSettingConfiguration()
      {
         @Override
         public List<GroupParameter<JointAccelerationIntegrationParametersReadOnly>> getJointAccelerationIntegrationParametersUnderLoad()
         {
            return parameters.getJointAccelerationIntegrationParametersUnderLoad(stateEnum);
         }

         @Override
         public List<GroupParameter<JointAccelerationIntegrationParametersReadOnly>> getJointAccelerationIntegrationParameters()
         {
            return parameters.getJointAccelerationIntegrationParameters(stateEnum);
         }

         @Override
         public List<GroupParameter<JointDesiredBehaviorReadOnly>> getDesiredJointBehaviorsUnderLoad()
         {
            return parameters.getDesiredJointBehaviorsUnderLoad(stateEnum);
         }

         @Override
         public List<GroupParameter<JointDesiredBehaviorReadOnly>> getDesiredJointBehaviors()
         {
            return parameters.getDesiredJointBehaviors(stateEnum);
         }
      };
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/stateTransitionConditions/StopFlamingoCondition.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.stateTransitionConditions;

import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
import us.ihmc.robotics.stateMachine.core.StateTransitionCondition;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.states.FlamingoStanceState;

public class StopFlamingoCondition implements StateTransitionCondition
{
   private final FlamingoStanceState singleSupportState;
   private final WalkingMessageHandler walkingMessageHandler;

   public StopFlamingoCondition(FlamingoStanceState singleSupportState, WalkingMessageHandler walkingMessageHandler)
   {
      this.singleSupportState = singleSupportState;
      this.walkingMessageHandler = walkingMessageHandler;
   }

   @Override
   public boolean testCondition(double timeInState)
   {
      if (!singleSupportState.isDone(timeInState))
         return false;

      return !walkingMessageHandler.hasFootTrajectoryForFlamingoStance(singleSupportState.getSwingSide());
   }
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/OrientationNumericalDifferentiator.java<|end_filename|>
package us.ihmc.robotics.math.filters;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.axisAngle.AxisAngle;
import us.ihmc.euclid.matrix.Matrix3D;
import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly;
import us.ihmc.euclid.referenceFrame.FrameQuaternion;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.rotationConversion.AxisAngleConversion;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.robotics.geometry.RotationTools;

public class OrientationNumericalDifferentiator
{
   public static FrameVector3D differentiate(double dt, ReferenceFrame frame, FrameQuaternion before, FrameQuaternion current, FrameQuaternion after)
   {
      ReferenceFrame beforeInitialFrame = before.getReferenceFrame();
      ReferenceFrame currentInitialFrame = current.getReferenceFrame();
      ReferenceFrame afterInitialFrame = after.getReferenceFrame();

      before.changeFrame(frame);
      current.changeFrame(frame);
      after.changeFrame(frame);

      // R^o_(bt-1) = before
      RotationMatrix oRbtm1 = new RotationMatrix(before);

      // R^(bt-1)_o = R^o_(bt-1)^-1
      RotationMatrix btm1Ro = new RotationMatrix();
      btm1Ro.setAndInvert(oRbtm1);

      // R^o_(bt+1) = after
      RotationMatrix oRbt1 = new RotationMatrix(after);

      // R^(bt-1)_(bt+1) = R^(bt-1)_o * R^o_(bt+1)
      RotationMatrix btRbt1 = new RotationMatrix();
      btRbt1.set(btm1Ro);
      btRbt1.multiply(oRbt1);

      AxisAngle w = new AxisAngle();
      AxisAngleConversion.convertMatrixToAxisAngle(btRbt1, w);

      // v_bt = [w/||w||, ||w||/(2*dt)]
      Vector3D vbt = new Vector3D(w.getX(), w.getY(), w.getZ());
      vbt.normalize();
      vbt.scale(w.getAngle() / (2 * dt));

      // R^o_bt = current
      RotationMatrix oRbt = new RotationMatrix(current);

      // v_wt = R^o_bt * v_bt
      Vector3D vwt = mul3(oRbt, vbt);

      before.changeFrame(beforeInitialFrame);
      current.changeFrame(currentInitialFrame);
      after.changeFrame(afterInitialFrame);

      return new FrameVector3D(frame, vwt);
   }

   public static List<FrameVector3D> differentiate(double dt, ReferenceFrame frame, List<FrameQuaternion> orientations)
   {
      List<FrameVector3D> vs = new ArrayList<>(orientations.size());
      for (int i = 1; i < orientations.size() - 1; i++)
      {
         FrameVector3D v = differentiate(dt, frame, orientations.get(i - 1), orientations.get(i), orientations.get(i + 1));
         vs.add(v);
      }

      // Add endpoints by padding out elements 1 and n-1
      vs.add(0, new FrameVector3D(vs.get(0)));
      vs.add(new FrameVector3D(vs.get(vs.size() - 1)));

      return vs;
   }

   /**
    * @see <a href="https://ocw.mit.edu/courses/mechanical-engineering/2-017j-design-of-electromechanical-robotic-systems-fall-2009/course-text/MIT2_017JF09_ch09.pdf}">KINEMATICS
    * OF MOVING FRAMES</a>, pg. 71.
    */
   public static Vector3D differentiate2(double dt, ReferenceFrame frame, FrameQuaternion before, FrameQuaternion current, FrameQuaternion after)
   {
      ReferenceFrame beforeInitialFrame = before.getReferenceFrame();
      ReferenceFrame currentInitialFrame = current.getReferenceFrame();
      ReferenceFrame afterInitialFrame = after.getReferenceFrame();

      before.changeFrame(frame);
      current.changeFrame(frame);
      after.changeFrame(frame);

      double yaw = current.getYaw();
      double pitch = current.getPitch();
      double roll = current.getRoll();

      double dyaw = (after.getYaw() - before.getYaw()) / (2.0 * dt);
      double dpitch = (after.getPitch() - before.getPitch()) / (2.0 * dt);
      double droll = (after.getRoll() - before.getRoll()) / (2.0 * dt);

      Matrix3D R = new Matrix3D();
      R.set(1, 0, -Math.sin(pitch),
            0, Math.cos(roll), Math.sin(roll) * Math.cos(pitch),
            0, -Math.sin(roll), Math.cos(roll) * Math.cos(pitch));

      Vector3D vE = new Vector3D(droll, dpitch, dyaw);
      Vector3D vbt = mul3(R, vE);

      RotationMatrix oRbt = new RotationMatrix(current);

      Vector3D vwt = mul3(oRbt, vbt);

      before.changeFrame(beforeInitialFrame);
      current.changeFrame(currentInitialFrame);
      after.changeFrame(afterInitialFrame);

      return vwt;
   }

   private static Vector3D mul3(Matrix3DReadOnly M, Vector3DReadOnly v)
   {
      Matrix3D V = new Matrix3D();
      V.set(v.getX(), v.getX(), v.getX(),
            v.getY(), v.getY(), v.getY(),
            v.getZ(), v.getZ(), v.getZ());

      Matrix3D MV = new Matrix3D();
      MV.set(M);
      MV.multiply(V);

      return new Vector3D(MV.getM00(), MV.getM11(), MV.getM22());
   }

   public static void main(String[] args)
   {
      Vector3D initialVelocity = new Vector3D(1.00, 1.00, 1.00);
      double time = 0.01;
      RotationMatrix initialRotation = new RotationMatrix();
      initialRotation.setYawPitchRoll(1, 1, 1);

      // compute m1 in o
      RotationMatrix m1 = new RotationMatrix();
      m1.set(initialRotation);

      // compute m2 in m1
      RotationMatrix m1inv = new RotationMatrix();
      m1inv.setAndInvert(m1);

      RotationMatrix m21 = new RotationMatrix();
      Vector3D velIn1 = mul3(m1inv, initialVelocity);
      RotationTools.integrateAngularVelocity(velIn1, time, m21);

      // compute m2 in o
      RotationMatrix m2 = new RotationMatrix();
      m2.set(m1);
      m2.multiply(m21);

      RotationMatrix m2inv = new RotationMatrix();
      m2inv.setAndInvert(m2);

      // compute m3 in m2
      RotationMatrix m32 = new RotationMatrix();
      Vector3D velIn2 = mul3(m2inv, initialVelocity);
      RotationTools.integrateAngularVelocity(velIn2, time, m32);

      RotationMatrix m3 = new RotationMatrix();
      m3.set(m2);
      m3.multiply(m32);

      FrameQuaternion f1 = new FrameQuaternion(ReferenceFrame.getWorldFrame(), m1);
      FrameQuaternion f2 = new FrameQuaternion(ReferenceFrame.getWorldFrame(), m2);
      FrameQuaternion f3 = new FrameQuaternion(ReferenceFrame.getWorldFrame(), m3);

      System.out.println(f1);
      System.out.println(f2);
      System.out.println(f3);

      System.out.println(initialVelocity);
      System.out.println(differentiate(0.01, ReferenceFrame.getWorldFrame(), f1, f2, f3));
      System.out.println(differentiate2(0.01, ReferenceFrame.getWorldFrame(), f1, f2, f3));
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/NeckDesiredAccelerationsMessage.java<|end_filename|>
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 gives the user the option to bypass IHMC feedback controllers for the neck joints by sending desired neck joint accelerations.
       * One needs experience in control when activating the bypass as it can result in unexpected behaviors for unreasonable accelerations.
       */
public class NeckDesiredAccelerationsMessage extends Packet<NeckDesiredAccelerationsMessage> implements Settable<NeckDesiredAccelerationsMessage>, EpsilonComparable<NeckDesiredAccelerationsMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The desired joint acceleration information.
            */
   public controller_msgs.msg.dds.DesiredAccelerationsMessage desired_accelerations_;

   public NeckDesiredAccelerationsMessage()
   {
      desired_accelerations_ = new controller_msgs.msg.dds.DesiredAccelerationsMessage();
   }

   public NeckDesiredAccelerationsMessage(NeckDesiredAccelerationsMessage other)
   {
      this();
      set(other);
   }

   public void set(NeckDesiredAccelerationsMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.DesiredAccelerationsMessagePubSubType.staticCopy(other.desired_accelerations_, desired_accelerations_);
   }

   /**
            * 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_;
   }


   /**
            * The desired joint acceleration information.
            */
   public controller_msgs.msg.dds.DesiredAccelerationsMessage getDesiredAccelerations()
   {
      return desired_accelerations_;
   }


   public static Supplier<NeckDesiredAccelerationsMessagePubSubType> getPubSubType()
   {
      return NeckDesiredAccelerationsMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return NeckDesiredAccelerationsMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(NeckDesiredAccelerationsMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.desired_accelerations_.epsilonEquals(other.desired_accelerations_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof NeckDesiredAccelerationsMessage)) return false;

      NeckDesiredAccelerationsMessage otherMyClass = (NeckDesiredAccelerationsMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.desired_accelerations_.equals(otherMyClass.desired_accelerations_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("NeckDesiredAccelerationsMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("desired_accelerations=");
      builder.append(this.desired_accelerations_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/environments/DRCTrialsWalkingEnvironment.java<|end_filename|>
package us.ihmc.avatar.environments;

import java.util.List;

import us.ihmc.simulationConstructionSetTools.util.environments.CommonAvatarEnvironmentInterface;
import us.ihmc.simulationConstructionSetTools.util.environments.DefaultCommonAvatarEnvironment;
import us.ihmc.simulationConstructionSetTools.util.environments.SelectableObjectListener;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class DRCTrialsWalkingEnvironment implements CommonAvatarEnvironmentInterface
{
   private final CombinedTerrainObject3D combinedTerrainObject3D;


   public DRCTrialsWalkingEnvironment()
   {
      combinedTerrainObject3D = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.setUpPath4DRCTrialsTrainingWalkingCourse("Path 4 Walking Course"));
      combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.setUpGround("Ground"));
   }

   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject3D;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return null;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/ConvexPolygon2dCalculator.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly;
import us.ihmc.euclid.geometry.interfaces.Line2DReadOnly;
import us.ihmc.euclid.geometry.interfaces.LineSegment2DBasics;
import us.ihmc.euclid.geometry.tools.EuclidGeometryTools;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics;

/**
 * This calculator class contains methods for computations with a ConvexPolygon2d such as
 * orthogonal projections and intersections.
 */
public class ConvexPolygon2dCalculator
{
   /**
    * Determines if the polygonToTest is inside the convex polygon.
    */
   public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, double epsilon, ConvexPolygon2DReadOnly polygon)
   {
      for (int i = 0; i < polygonToTest.getNumberOfVertices(); i++)
      {
         if (!polygon.isPointInside(polygonToTest.getVertex(i), epsilon))
            return false;
      }

      return true;
   }

   /**
    * Determines if the polygonToTest is inside the convex polygon.
    */
   public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, ConvexPolygon2DReadOnly polygon)
   {
      return isPolygonInside(polygonToTest, 0.0, polygon);
   }

   /**
    * Returns the index in the middle of the range from firstIndex to secondIndex moving counter clockwise.
    * E.g. in a polygon with 6 vertices given indices 0 and 2 (in this order) the method will return the
    * middle of the range [0 5 4 3 2]: 4
    */
   public static int getMiddleIndexCounterClockwise(int firstIndex, int secondIndex, ConvexPolygon2DReadOnly polygon)
   {
      int numberOfVertices = polygon.getNumberOfVertices();
      if (secondIndex >= firstIndex)
         return (secondIndex + (firstIndex + numberOfVertices - secondIndex + 1) / 2) % numberOfVertices;
      else
         return (secondIndex + firstIndex + 1) / 2;
   }

   /**
    * Packs a vector that is orthogonal to the given edge, facing towards the outside of the polygon
    */
   public static void getEdgeNormal(int edgeIndex, Vector2DBasics normalToPack, ConvexPolygon2DReadOnly polygon)
   {
      Point2DReadOnly edgeStart = polygon.getVertex(edgeIndex);
      Point2DReadOnly edgeEnd = polygon.getNextVertex(edgeIndex);

      double edgeVectorX = edgeEnd.getX() - edgeStart.getX();
      double edgeVectorY = edgeEnd.getY() - edgeStart.getY();

      normalToPack.set(-edgeVectorY, edgeVectorX);
      normalToPack.normalize();
   }

   /**
    * This finds the edges of the polygon that intersect the given line. Will pack the edges into edgeToPack1 and
    * edgeToPack2. Returns number of intersections found. An edge parallel to the line can not intersect the edge.
    * If the line goes through a vertex but is not parallel to an edge adjacent to that vertex this method will
    * only pack the edge before the vertex, not both edges.
    */
   public static int getIntersectingEdges(Line2DReadOnly line, LineSegment2DBasics edgeToPack1, LineSegment2DBasics edgeToPack2, ConvexPolygon2DReadOnly polygon)
   {
      if (polygon.getNumberOfVertices() == 1)
         return 0;

      int foundEdges = 0;
      for (int i = 0; i < polygon.getNumberOfVertices(); i++)
      {
         Point2DReadOnly startVertex = polygon.getVertex(i);
         Point2DReadOnly endVertex = polygon.getNextVertex(i);

         // edge is on the line
         if (line.isPointOnLine(startVertex) && line.isPointOnLine(endVertex))
         {
            if (polygon.getNumberOfVertices() == 2)
               return 0;
            // set the edges to be the previous and the next edge
            edgeToPack1.set(polygon.getPreviousVertex(i), startVertex);
            edgeToPack2.set(endVertex, polygon.getNextVertex(polygon.getNextVertexIndex(i)));
            return 2;
         }

         if (line.isPointOnLine(startVertex))
            continue;

         if (doesLineIntersectEdge(line, i, polygon) || line.isPointOnLine(endVertex))
         {
            if (foundEdges == 0)
               edgeToPack1.set(startVertex, endVertex);
            else
               edgeToPack2.set(startVertex, endVertex);
            foundEdges++;
         }

         if (foundEdges == 2) break; // performance only
      }

      return foundEdges;
   }

   /**
    * Checks if a line intersects the edge with the given index.
    */
   public static boolean doesLineIntersectEdge(Line2DReadOnly line, int edgeIndex, ConvexPolygon2DReadOnly polygon)
   {
      if (polygon.getNumberOfVertices() < 2)
         return false;

      Point2DReadOnly edgeStart = polygon.getVertex(edgeIndex);
      Point2DReadOnly edgeEnd = polygon.getNextVertex(edgeIndex);

      double lineDirectionX = line.getDirectionX();
      double lineDirectionY = line.getDirectionY();
      double edgeDirectionX = edgeEnd.getX() - edgeStart.getX();
      double edgeDirectionY = edgeEnd.getY() - edgeStart.getY();

      if (EuclidGeometryTools.areVector2DsParallel(lineDirectionX, lineDirectionY, edgeDirectionX, edgeDirectionY, EuclidGeometryTools.ONE_TEN_MILLIONTH))
            return false;
      else
         return EuclidGeometryTools.doLine2DAndLineSegment2DIntersect(line.getPoint(), line.getDirection(), edgeStart, edgeEnd);
   }

   // --- Methods that generate garbage ---
   public static LineSegment2D[] getIntersectingEdgesCopy(Line2DReadOnly line, ConvexPolygon2DReadOnly polygon)
   {
      LineSegment2D edge1 = new LineSegment2D();
      LineSegment2D edge2 = new LineSegment2D();

      int edges = getIntersectingEdges(line, edge1, edge2, polygon);
      if (edges == 2)
         return new LineSegment2D[] {edge1, edge2};
      if (edges == 1)
         return new LineSegment2D[] {edge1};
      return null;
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/utilities/StopThreadUpdatable.java<|end_filename|>
package us.ihmc.humanoidBehaviors.utilities;

import java.util.LinkedHashMap;

import us.ihmc.commonWalkingControlModules.controllers.Updatable;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.humanoidBehaviors.behaviors.AbstractBehavior;
import us.ihmc.humanoidBehaviors.behaviors.AbstractBehavior.BehaviorStatus;
import us.ihmc.humanoidRobotics.communication.packets.behaviors.BehaviorControlModeEnum;
import us.ihmc.humanoidRobotics.communication.subscribers.HumanoidRobotDataReceiver;
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;

public abstract class StopThreadUpdatable implements Updatable
{
   protected final static ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();

   protected final HumanoidRobotDataReceiver robotDataReceiver;
   protected final AbstractBehavior behavior;
   protected final ReferenceFrame frameToKeepTrackOf;
   protected final RigidBodyTransform currentTransformToWorld;

   private boolean stopBehaviorThread = false;

   private BehaviorControlModeEnum requestedControlMode = BehaviorControlModeEnum.RESUME;

   protected LinkedHashMap<BehaviorControlModeEnum, BehaviorStatus> behaviorStatus = new LinkedHashMap<BehaviorControlModeEnum, BehaviorStatus>();
   protected LinkedHashMap<BehaviorControlModeEnum, RigidBodyTransform> testFrameTransformToWorld = new LinkedHashMap<BehaviorControlModeEnum, RigidBodyTransform>();
   protected LinkedHashMap<BehaviorControlModeEnum, HumanoidReferenceFrames> referenceFramesForLogging = new LinkedHashMap<BehaviorControlModeEnum, HumanoidReferenceFrames>();

   public StopThreadUpdatable(HumanoidRobotDataReceiver robotDataReceiver, AbstractBehavior behavior, ReferenceFrame frameToKeepTrackOf)
   {
      this.robotDataReceiver = robotDataReceiver;
      this.behavior = behavior;
      this.frameToKeepTrackOf = frameToKeepTrackOf;
      this.currentTransformToWorld = new RigidBodyTransform();

      testFrameTransformToWorld.put(BehaviorControlModeEnum.PAUSE, new RigidBodyTransform());
      testFrameTransformToWorld.put(BehaviorControlModeEnum.STOP, new RigidBodyTransform());
      testFrameTransformToWorld.put(BehaviorControlModeEnum.RESUME, new RigidBodyTransform());
   }

   public BehaviorControlModeEnum getRequestedBehaviorControlMode()
   {
      return requestedControlMode;
   }

   public void setRequestedBehaviorControlMode(BehaviorControlModeEnum newRequestedControlMode)
   {
      if (!this.requestedControlMode.equals(newRequestedControlMode))
      {
         this.requestedControlMode = newRequestedControlMode;
         behaviorStatus.put(newRequestedControlMode, behavior.getBehaviorStatus());
         testFrameTransformToWorld.get(newRequestedControlMode).set(getCurrentTestFrameTransformToWorld());         
         referenceFramesForLogging.put(newRequestedControlMode, robotDataReceiver.getUpdatedReferenceFramesCopy());
      }
   }

   public HumanoidReferenceFrames getReferenceFramesAtTransition(BehaviorControlModeEnum controlMode)
   {
      return referenceFramesForLogging.get(controlMode);
   }
   
   public RigidBodyTransform getCurrentTestFrameTransformToWorld()
   {
      robotDataReceiver.updateRobotModel();
      frameToKeepTrackOf.getTransformToDesiredFrame(currentTransformToWorld, worldFrame);
      return currentTransformToWorld;
   }

   public RigidBodyTransform getTestFrameTransformToWorldAtTransition(BehaviorControlModeEnum controlMode)
   {
      return testFrameTransformToWorld.get(controlMode);
   }

   public FramePose3D getTestFramePoseCopyAtTransition(BehaviorControlModeEnum controlMode)
   {
      FramePose3D ret = new FramePose3D(ReferenceFrame.getWorldFrame(), getTestFrameTransformToWorldAtTransition(controlMode));
      return ret;
   }

   public FramePose3D getCurrentTestFramePoseCopy()
   {
      FramePose3D ret = new FramePose3D();
      getCurrentTestFramePose(ret);
      return ret;
   }

   public void getCurrentTestFramePose(FramePose3D poseToPack)
   {
      poseToPack.setIncludingFrame(worldFrame, getCurrentTestFrameTransformToWorld());
   }

   public FramePose2D getTestFramePose2dAtTransition(BehaviorControlModeEnum controlMode)
   {
      FramePose2D ret = new FramePose2D();
      ret.setIncludingFrame(ReferenceFrame.getWorldFrame(), getTestFrameTransformToWorldAtTransition(controlMode), false);
      return ret;
   }

   public FramePose2D getCurrentTestFramePose2dCopy()
   {
      return getTestFramePose2dCopy(getCurrentTestFrameTransformToWorld());
   }

   public FramePose2D getTestFramePose2dCopy(RigidBodyTransform testFrameTransformToWorld)
   {
      FramePose2D ret = new FramePose2D(ReferenceFrame.getWorldFrame());
      ret.setIncludingFrame(ReferenceFrame.getWorldFrame(), testFrameTransformToWorld, false);
      return ret;
   }

   public BehaviorStatus getBehaviorStatusAtTransition(BehaviorControlModeEnum controlMode)
   {
      return behaviorStatus.get(controlMode);
   }

   protected void setShouldBehaviorRunnerBeStopped(boolean stop)
   {
      stopBehaviorThread = stop;
   }

   public boolean shouldBehaviorRunnerBeStopped()
   {
      return stopBehaviorThread;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/graphicsBuilders/UIOcTreeNodeMeshView.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.graphicsBuilders;

import javafx.scene.paint.Material;
import javafx.scene.shape.Mesh;
import javafx.scene.shape.MeshView;
import us.ihmc.jOctoMap.key.OcTreeKey;

public class UIOcTreeNodeMeshView extends MeshView
{
   private final OcTreeKey nodeKey;

   public UIOcTreeNodeMeshView(OcTreeKey nodeKey)
   {
      this.nodeKey = nodeKey;
   }

   public UIOcTreeNodeMeshView(OcTreeKey nodeKey, Mesh mesh, Material material)
   {
      super(mesh);
      this.nodeKey = nodeKey;
      setMaterial(material);
   }

   @Override
   public int hashCode()
   {
      return nodeKey.hashCode();
   }

   @Override
   public boolean equals(Object obj)
   {
      if (obj instanceof UIOcTreeNodeMeshView)
         return equals((UIOcTreeNodeMeshView) obj);
      else
         return false;
   }

   public boolean equals(UIOcTreeNodeMeshView other)
   {
      return nodeKey.equals(other.nodeKey);
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/ToeSlippingDetectorParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

public class ToeSlippingDetectorParameters
{
   /**
    * Returns the forceMagnitudeThreshold. As long as the foot force magnitude remains above this
    * threshold, the toe will be considered as not slipping.
    */
   public double getForceMagnitudeThreshold()
   {
      return 25.0;
   }

   /**
    * Returns the velocityThreshold. This is one of the conditions to trigger the slipping detection: the
    * toe linear velocity magnitude has to be greater than this threshold.
    */
   public double getVelocityThreshold()
   {
      return 0.4;
   }

   /**
    * Returns the slippageDistanceThreshold. This is one of the condition to trigger the slipping
    * detection: the amount of slipping has to be greater than this threshold.
    */
   public double getSlippageDistanceThreshold()
   {
      return 0.04;
   }

   /**
    * Returns the filterBreakFrequency. This the break frequency to use for the internal low-pass filters.
    */
   public double getFilterBreakFrequency()
   {
      return 10.0;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/handControl/packetsAndConsumers/HandSensorData.java<|end_filename|>
package us.ihmc.avatar.handControl.packetsAndConsumers;

import us.ihmc.robotics.robotSide.RobotSide;

public interface HandSensorData
{
	public double[] getFingerJointAngles(RobotSide robotSide);
	public boolean isCalibrated();
	public boolean isConnected();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/StateTransitionCondition.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

/**
 * <p>Title: SimulationConstructionSet</p>
 *
 * @author not attributable
 * @version 1.0
 */
@Deprecated
public interface StateTransitionCondition
{
   public abstract boolean checkCondition();
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/externalcontroller/ExternalControllerInterface.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.externalcontroller;

interface ExternalControllerInterface
{
   public void update(double[] dataToBeUpdated);

   public void doControl();

   public double[] getTorques();

}


<|start_filename|>ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/RemoteFootstepPlannerUI.java<|end_filename|>
package us.ihmc.footstepPlanning.ui;

import javafx.application.Application;
import javafx.application.Platform;
import javafx.stage.Stage;
import us.ihmc.footstepPlanning.communication.FootstepPlannerMessagerAPI;
import us.ihmc.javaFXToolkit.messager.SharedMemoryJavaFXMessager;
import us.ihmc.pubsub.DomainFactory;

/**
 * This class provides a visualizer for the remote footstep planner found in the footstep planner toolbox.
 * It allows users to view the resulting plans calculated by the toolbox. It also allows the user to tune
 * the planner parameters, and request a new plan from the planning toolbox.
 */
public class RemoteFootstepPlannerUI extends Application
{

   private SharedMemoryJavaFXMessager messager;
   private RemoteUIMessageConverter messageConverter;

   private FootstepPlannerUI ui;

   @Override
   public void start(Stage primaryStage) throws Exception
   {
      messager = new SharedMemoryJavaFXMessager(FootstepPlannerMessagerAPI.API);
      messageConverter = RemoteUIMessageConverter.createConverter(messager, "", DomainFactory.PubSubImplementation.INTRAPROCESS);

      messager.startMessager();

      ui = FootstepPlannerUI.createMessagerUI(primaryStage, messager);
      ui.show();
   }

   @Override
   public void stop() throws Exception
   {
      super.stop();

      messager.closeMessager();
      messageConverter.destroy();
      ui.stop();

      Platform.exit();
   }

   public static void main(String[] args)
   {
      launch(args);
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerCore/command/ConstraintType.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controllerCore.command;

public enum ConstraintType
{
   /**
    * A equality condition that is set up in the QP as part of the objective rather than as a
    * constraint:<br>
    * min<sub>x</sub> || Ax - b ||, <br>
    * where 'b' is the desired task objective.
    */
   OBJECTIVE,
   /**
    * A equality condition that is set up in the QP as a hard constraint:<br>
    * Ax = b, <br>
    * where 'b' is the desired task objective.
    */
   EQUALITY,
   /**
    * A inequality condition that is set up in the QP as a hard lesser-or-equal inequality constraint:<br>
    * Ax <= b, <br>
    * where 'b' is the desired task objective.
    */
   LEQ_INEQUALITY,
   /**
    * A inequality condition that is set up in the QP as a hard greater-or-equal inequality constraint:<br>
    * Ax >= b, <br>
    * where 'b' is the desired task objective.
    */
   GEQ_INEQUALITY;

   @Override
   public String toString()
   {
      switch (this)
      {
      case OBJECTIVE:
         return "Objective.";
      case EQUALITY:
         return "Equality Motion Constraint.";
      case LEQ_INEQUALITY:
         return "Lesser-or-Equal Inequality Motion Constraint.";
      case GEQ_INEQUALITY:
         return "Greater-or-Equal Inequality Motion Constraint.";
      default:
         return "Unknown case";
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SnapFootstepPacket.java<|end_filename|>
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;

/**
       * Old message, needs to be tested and cleaned up.
       */
public class SnapFootstepPacket extends Packet<SnapFootstepPacket> implements Settable<SnapFootstepPacket>, EpsilonComparable<SnapFootstepPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.FootstepDataMessage>  footstep_data_;
   public us.ihmc.idl.IDLSequence.Integer  footstep_order_;
   public us.ihmc.idl.IDLSequence.Byte  flag_;

   public SnapFootstepPacket()
   {
      footstep_data_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.FootstepDataMessage> (100, new controller_msgs.msg.dds.FootstepDataMessagePubSubType());
      footstep_order_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_2");

      flag_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9");


   }

   public SnapFootstepPacket(SnapFootstepPacket other)
   {
      this();
      set(other);
   }

   public void set(SnapFootstepPacket other)
   {
      sequence_id_ = other.sequence_id_;

      footstep_data_.set(other.footstep_data_);
      footstep_order_.set(other.footstep_order_);
      flag_.set(other.flag_);
   }

   /**
            * 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 us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.FootstepDataMessage>  getFootstepData()
   {
      return footstep_data_;
   }


   public us.ihmc.idl.IDLSequence.Integer  getFootstepOrder()
   {
      return footstep_order_;
   }


   public us.ihmc.idl.IDLSequence.Byte  getFlag()
   {
      return flag_;
   }


   public static Supplier<SnapFootstepPacketPubSubType> getPubSubType()
   {
      return SnapFootstepPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SnapFootstepPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SnapFootstepPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (this.footstep_data_.size() != other.footstep_data_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.footstep_data_.size(); i++)
         {  if (!this.footstep_data_.get(i).epsilonEquals(other.footstep_data_.get(i), epsilon)) return false; }
      }

      if (!us.ihmc.idl.IDLTools.epsilonEqualsIntegerSequence(this.footstep_order_, other.footstep_order_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsByteSequence(this.flag_, other.flag_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SnapFootstepPacket)) return false;

      SnapFootstepPacket otherMyClass = (SnapFootstepPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.footstep_data_.equals(otherMyClass.footstep_data_)) return false;
      if (!this.footstep_order_.equals(otherMyClass.footstep_order_)) return false;
      if (!this.flag_.equals(otherMyClass.flag_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SnapFootstepPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("footstep_data=");
      builder.append(this.footstep_data_);      builder.append(", ");
      builder.append("footstep_order=");
      builder.append(this.footstep_order_);      builder.append(", ");
      builder.append("flag=");
      builder.append(this.flag_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/properties/IntersectionEstimationParametersProperty.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.properties;

import javafx.beans.property.Property;
import us.ihmc.robotEnvironmentAwareness.planarRegion.IntersectionEstimationParameters;

public class IntersectionEstimationParametersProperty extends ParametersProperty<IntersectionEstimationParameters>
{
   private final DoubleField maxDistanceToRegion = new DoubleField(IntersectionEstimationParameters::getMaxDistanceToRegion, (p, v) -> p.setMaxDistanceToRegion(v));
   private final IntegerField minRegionSize = new IntegerField(IntersectionEstimationParameters::getMinRegionSize, (p, v) -> p.setMinRegionSize(v));
   private final DoubleField minIntersectionLength = new DoubleField(IntersectionEstimationParameters::getMinIntersectionLength, (p, v) -> p.setMinIntersectionLength(v));
   private final DoubleField minRegionAngleDifference = new DoubleField(IntersectionEstimationParameters::getMinRegionAngleDifference, (p, v) -> p.setMinRegionAngleDifference(v));
   private final BooleanField addIntersectionsToRegions = new BooleanField(IntersectionEstimationParameters::isAddIntersectionsToRegions, (p, v) -> p.setAddIntersectionsToRegions(v));

   public IntersectionEstimationParametersProperty(Object bean, String name)
   {
      super(bean, name, new IntersectionEstimationParameters());
   }

   public void bindBidirectionalMaxDistanceToRegion(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxDistanceToRegion);
   }

   public void bindBidirectionalMinRegionSize(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minRegionSize);
   }

   public void bindBidirectionalMinIntersectionLength(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minIntersectionLength);
   }

   public void bindBidirectionalMinRegionAngleDifference(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minRegionAngleDifference);
   }

   public void bindBidirectionalAddIntersectionsToRegions(Property<Boolean> property)
   {
      bindFieldBidirectionalToBooleanProperty(property, addIntersectionsToRegions);
   }

   @Override
   protected IntersectionEstimationParameters getValueCopy(IntersectionEstimationParameters valueToCopy)
   {
      return new IntersectionEstimationParameters(valueToCopy);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandDesiredConfigurationMessage.java<|end_filename|>
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 old will be refreshed in a future release.
       * Message for commanding the hands to perform various predefined grasps.
       */
public class HandDesiredConfigurationMessage extends Packet<HandDesiredConfigurationMessage> implements Settable<HandDesiredConfigurationMessage>, EpsilonComparable<HandDesiredConfigurationMessage>
{
   public static final byte ROBOT_SIDE_LEFT = (byte) 0;
   public static final byte ROBOT_SIDE_RIGHT = (byte) 1;
   public static final byte HAND_CONFIGURATION_STOP = (byte) 0;
   public static final byte HAND_CONFIGURATION_OPEN = (byte) 1;
   public static final byte HAND_CONFIGURATION_CLOSE = (byte) 2;
   public static final byte HAND_CONFIGURATION_CRUSH = (byte) 3;
   public static final byte HAND_CONFIGURATION_HOOK = (byte) 4;
   public static final byte HAND_CONFIGURATION_BASIC_GRIP = (byte) 5;
   public static final byte HAND_CONFIGURATION_PINCH_GRIP = (byte) 6;
   public static final byte HAND_CONFIGURATION_WIDE_GRIP = (byte) 7;
   public static final byte HAND_CONFIGURATION_SCISSOR_GRIP = (byte) 8;
   public static final byte HAND_CONFIGURATION_RESET = (byte) 9;
   public static final byte HAND_CONFIGURATION_OPEN_FINGERS = (byte) 10;
   public static final byte HAND_CONFIGURATION_OPEN_THUMB = (byte) 11;
   public static final byte HAND_CONFIGURATION_CLOSE_FINGERS = (byte) 12;
   public static final byte HAND_CONFIGURATION_CLOSE_THUMB = (byte) 13;
   public static final byte HAND_CONFIGURATION_OPEN_INDEX = (byte) 14;
   public static final byte HAND_CONFIGURATION_OPEN_MIDDLE = (byte) 15;
   public static final byte HAND_CONFIGURATION_HALF_CLOSE = (byte) 16;
   public static final byte HAND_CONFIGURATION_CONNECT = (byte) 17;
   public static final byte HAND_CONFIGURATION_CRUSH_INDEX = (byte) 18;
   public static final byte HAND_CONFIGURATION_CRUSH_MIDDLE = (byte) 19;
   public static final byte HAND_CONFIGURATION_CRUSH_THUMB = (byte) 20;
   public static final byte HAND_CONFIGURATION_INVERT_POWER = (byte) 21;
   public static final byte HAND_CONFIGURATION_T_SPREAD = (byte) 22;
   public static final byte HAND_CONFIGURATION_BEND_BACKWARD = (byte) 23;
   public static final byte HAND_CONFIGURATION_CALIBRATE = (byte) 24;
   public static final byte HAND_CONFIGURATION_FINGER_MANIPULATION = (byte) 25;
   public static final byte HAND_CONFIGURATION_PRE_CREEPY_GRASP = (byte) 26;
   public static final byte HAND_CONFIGURATION_PARTIAL_CREEPY_GRASP = (byte) 27;
   public static final byte HAND_CONFIGURATION_CREEPY_GRASPING = (byte) 28;
   public static final byte HAND_CONFIGURATION_CREEPY_GRASPING_HARD = (byte) 29;
   public static final byte HAND_CONFIGURATION_SLOW_CLOSE = (byte) 30;
   /**
            * 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 trajectory
            */
   public byte robot_side_ = (byte) 255;
   /**
            * Specifies the grasp to perform
            */
   public byte desired_hand_configuration_ = (byte) 255;

   public HandDesiredConfigurationMessage()
   {
   }

   public HandDesiredConfigurationMessage(HandDesiredConfigurationMessage other)
   {
      this();
      set(other);
   }

   public void set(HandDesiredConfigurationMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      desired_hand_configuration_ = other.desired_hand_configuration_;

   }

   /**
            * 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_;
   }

   /**
            * Specifies the side of the robot that will execute the trajectory
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * Specifies the side of the robot that will execute the trajectory
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }

   /**
            * Specifies the grasp to perform
            */
   public void setDesiredHandConfiguration(byte desired_hand_configuration)
   {
      desired_hand_configuration_ = desired_hand_configuration;
   }
   /**
            * Specifies the grasp to perform
            */
   public byte getDesiredHandConfiguration()
   {
      return desired_hand_configuration_;
   }


   public static Supplier<HandDesiredConfigurationMessagePubSubType> getPubSubType()
   {
      return HandDesiredConfigurationMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HandDesiredConfigurationMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HandDesiredConfigurationMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.desired_hand_configuration_, other.desired_hand_configuration_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HandDesiredConfigurationMessage)) return false;

      HandDesiredConfigurationMessage otherMyClass = (HandDesiredConfigurationMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if(this.desired_hand_configuration_ != otherMyClass.desired_hand_configuration_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HandDesiredConfigurationMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("desired_hand_configuration=");
      builder.append(this.desired_hand_configuration_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/BCF2000ChannelMapper.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public class BCF2000ChannelMapper implements MidiChannelMapper
{

   @Override
   public int getKnobChannel(int channel)
   {
      return channel - 80;
   }

   @Override
   public int getSliderChannel(int channel)
   {
      return channel;
   }

   @Override
   public int getButtonChannel(int channel)
   {
      int offset;
      if ((channel >= 17) && (channel <= 20))
         offset = -8;
      else
         offset = -16;

      return channel + offset;
   }

   @Override
   public int getKnobButtonChannel(int channel)
   {
      return channel - 48;
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/EuclideanTrajectoryPointMessage.java<|end_filename|>
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 build trajectory messages in taskspace.
       * It holds the only the translational information for one trajectory point (position & linear velocity).
       * Feel free to look at SO3TrajectoryPointMessage (rotational) and SE3TrajectoryPointMessage (rotational AND translational).
       */
public class EuclideanTrajectoryPointMessage extends Packet<EuclideanTrajectoryPointMessage> implements Settable<EuclideanTrajectoryPointMessage>, EpsilonComparable<EuclideanTrajectoryPointMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public double time_;
   /**
            * Define the desired 3D position to be reached at this trajectory point. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple3D.Point3D position_;
   /**
            * Define the desired 3D linear velocity to be reached at this trajectory point. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple3D.Vector3D linear_velocity_;

   public EuclideanTrajectoryPointMessage()
   {
      position_ = new us.ihmc.euclid.tuple3D.Point3D();
      linear_velocity_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public EuclideanTrajectoryPointMessage(EuclideanTrajectoryPointMessage other)
   {
      this();
      set(other);
   }

   public void set(EuclideanTrajectoryPointMessage other)
   {
      sequence_id_ = other.sequence_id_;

      time_ = other.time_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.position_, position_);
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.linear_velocity_, linear_velocity_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }

   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public void setTime(double time)
   {
      time_ = time;
   }
   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public double getTime()
   {
      return time_;
   }


   /**
            * Define the desired 3D position to be reached at this trajectory point. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple3D.Point3D getPosition()
   {
      return position_;
   }


   /**
            * Define the desired 3D linear velocity to be reached at this trajectory point. It is expressed in world frame.
            */
   public us.ihmc.euclid.tuple3D.Vector3D getLinearVelocity()
   {
      return linear_velocity_;
   }


   public static Supplier<EuclideanTrajectoryPointMessagePubSubType> getPubSubType()
   {
      return EuclideanTrajectoryPointMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return EuclideanTrajectoryPointMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(EuclideanTrajectoryPointMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false;

      if (!this.position_.epsilonEquals(other.position_, epsilon)) return false;
      if (!this.linear_velocity_.epsilonEquals(other.linear_velocity_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof EuclideanTrajectoryPointMessage)) return false;

      EuclideanTrajectoryPointMessage otherMyClass = (EuclideanTrajectoryPointMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.time_ != otherMyClass.time_) return false;

      if (!this.position_.equals(otherMyClass.position_)) return false;
      if (!this.linear_velocity_.equals(otherMyClass.linear_velocity_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("EuclideanTrajectoryPointMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("time=");
      builder.append(this.time_);      builder.append(", ");
      builder.append("position=");
      builder.append(this.position_);      builder.append(", ");
      builder.append("linear_velocity=");
      builder.append(this.linear_velocity_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/JointTorqueOffsetProcessor.java<|end_filename|>
package us.ihmc.wholeBodyController;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;

public interface JointTorqueOffsetProcessor
{
   public abstract void subtractTorqueOffset(OneDoFJointBasics oneDoFJoint, double torqueOffset);
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasContactPointParameters.java<|end_filename|>
package us.ihmc.atlas.parameters;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import us.ihmc.atlas.AtlasJointMap;
import us.ihmc.atlas.AtlasRobotVersion;
import us.ihmc.euclid.geometry.LineSegment2D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.robotics.partNames.LeggedJointNameMap;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SegmentDependentList;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.robotiq.model.RobotiqHandModel.RobotiqHandJointNameMinimal;
import us.ihmc.simulationconstructionset.util.LinearGroundContactModel;
import us.ihmc.wholeBodyController.DRCHandType;
import us.ihmc.wholeBodyController.FootContactPoints;
import us.ihmc.wholeBodyController.RobotContactPointParameters;

public class AtlasContactPointParameters extends RobotContactPointParameters<RobotSide>
{
   public static final boolean USE_SIX_CONTACT_POINTS = false;
   private final int numberOfContactableBodies;

   private boolean handContactPointsHaveBeenCreated = false;
   private final AtlasJointMap jointMap;
   private final AtlasRobotVersion atlasVersion;

   private boolean useSoftGroundContactParameters = false;

   public AtlasContactPointParameters(AtlasJointMap jointMap, AtlasRobotVersion atlasVersion, boolean createFootContactPoints,
         FootContactPoints<RobotSide> footContactPoints, boolean createAdditionalContactPoints)
   {
      super(jointMap, jointMap.getPhysicalProperties().getToeWidthForControl(), jointMap.getPhysicalProperties().getFootWidthForControl(),
            jointMap.getPhysicalProperties().getFootLengthForControl(), jointMap.getPhysicalProperties().getSoleToAnkleFrameTransforms());

      this.jointMap = jointMap;
      this.atlasVersion = atlasVersion;
      if (createFootContactPoints)
      {
         if (footContactPoints != null)
         {
            createFootContactPoints(footContactPoints);
         }
         else if(USE_SIX_CONTACT_POINTS)
         {
            createFootContactPoints(new AtlasSixContactFoot());
         }
         else
         {
            createDefaultFootContactPoints();
         }
      }

      int totalContacts = 2;
      if (createAdditionalContactPoints)
         totalContacts += createAdditionalHandContactPoints();

      numberOfContactableBodies = totalContacts;
   }

   public int createAdditionalHandContactPoints()
   {
      switch (atlasVersion)
      {
      case ATLAS_UNPLUGGED_V5_NO_HANDS:
         createHandKnobContactPoints();
         return 2;

      case ATLAS_UNPLUGGED_V5_DUAL_ROBOTIQ:
         if (DRCHandType.ROBOTIQ.isHandSimulated())
         {
            for (RobotSide robotSide : RobotSide.values)
               createRobotiqHandContactPoints(robotSide, false);
         }
         return 2;

      default:
         return 0;
      }
   }

   private void addAdditionalContactPoint(String bodyName, String contactName, RigidBodyTransform transformToContactFrame)
   {
      additionalContactRigidBodyNames.add(bodyName);
      additionalContactNames.add(contactName);
      additionalContactTransforms.add(transformToContactFrame);
   }

   private void createHandKnobContactPoints()
   {
      if (handContactPointsHaveBeenCreated)
      {
         throw new RuntimeException("Contact points for the hands have already been created");
      }
      else
      {
         handContactPointsHaveBeenCreated = true;
      }

      SideDependentList<String> nameOfJointBeforeHands = jointMap.getNameOfJointBeforeHands();

      for (RobotSide robotSide : RobotSide.values)
      {
         String bodyName = jointMap.getHandName(robotSide);
         Point3D pointLocationInParentJoint = new Point3D(0.0, robotSide.negateIfRightSide(0.13), 0.0);

         RigidBodyTransform transformToContactFrame = new RigidBodyTransform(new Quaternion(), pointLocationInParentJoint);
         if (robotSide == RobotSide.LEFT)
            transformToContactFrame.appendRollRotation(Math.PI);

         addSimulationContactPoint(nameOfJointBeforeHands.get(robotSide), pointLocationInParentJoint);

         addAdditionalContactPoint(bodyName, bodyName + "Contact", transformToContactFrame);
      }
   }

   private void createRobotiqHandContactPoints(RobotSide robotSide, boolean areHandsFlipped)
   {
      String nameOfJointBeforeHand = jointMap.getNameOfJointBeforeHands().get(robotSide);

      String finger_1_joint_1 = RobotiqHandJointNameMinimal.FINGER_1_JOINT_1.getJointName(robotSide);
      String finger_1_joint_2 = RobotiqHandJointNameMinimal.FINGER_1_JOINT_2.getJointName(robotSide);
      String finger_1_joint_3 = RobotiqHandJointNameMinimal.FINGER_1_JOINT_3.getJointName(robotSide);

      String finger_2_joint_1 = RobotiqHandJointNameMinimal.FINGER_2_JOINT_1.getJointName(robotSide);
      String finger_2_joint_2 = RobotiqHandJointNameMinimal.FINGER_2_JOINT_2.getJointName(robotSide);
      String finger_2_joint_3 = RobotiqHandJointNameMinimal.FINGER_2_JOINT_3.getJointName(robotSide);

      String thumb_joint_1 = RobotiqHandJointNameMinimal.FINGER_MIDDLE_JOINT_1.getJointName(robotSide);
      String thumb_joint_2 = RobotiqHandJointNameMinimal.FINGER_MIDDLE_JOINT_2.getJointName(robotSide);
      String thumb_joint_3 = RobotiqHandJointNameMinimal.FINGER_MIDDLE_JOINT_3.getJointName(robotSide);

      createRobotiqHandPalmContactPoints(robotSide, nameOfJointBeforeHand, areHandsFlipped);

      double plusOrMinusY = robotSide.negateIfRightSide(1.0);
      double plusOrMinusZ = 1.0;

      if (areHandsFlipped)
         plusOrMinusZ = robotSide.negateIfRightSide(1.0);

      Vector3D thumbJoint1ContactPoint = new Vector3D(0.0, plusOrMinusY * 0.033, plusOrMinusZ * 0.0105);
      Vector3D thumbJoint2ContactPoint1 = new Vector3D(0.0, plusOrMinusY * 0.005, plusOrMinusZ * -0.005);
      Vector3D thumbJoint2ContactPoint2 = new Vector3D(0.0, plusOrMinusY * 0.027, plusOrMinusZ * 0.007);
      Vector3D thumbJoint3ContactPoint = new Vector3D(0.0, plusOrMinusY * 0.025, plusOrMinusZ * -0.006);

      Vector3D fingersJoint1ContactPoint = new Vector3D(0.0, plusOrMinusY * 0.032, plusOrMinusZ * -0.011);
      Vector3D fingersJoint2ContactPoint1 = new Vector3D(0.0, thumbJoint2ContactPoint1.getY(), -thumbJoint2ContactPoint1.getZ());
      Vector3D fingersJoint2ContactPoint2 = new Vector3D(0.0, thumbJoint2ContactPoint2.getY(), -thumbJoint2ContactPoint2.getZ());
      Vector3D fingersJoint3ContactPoint = new Vector3D(0.0, thumbJoint3ContactPoint.getY(), -thumbJoint3ContactPoint.getZ());

      addSimulationContactPoint(finger_1_joint_1, fingersJoint1ContactPoint);
      addSimulationContactPoint(finger_1_joint_2, fingersJoint2ContactPoint1);
      addSimulationContactPoint(finger_1_joint_2, fingersJoint2ContactPoint2);
      addSimulationContactPoint(finger_1_joint_3, fingersJoint3ContactPoint);
      addSimulationContactPoint(finger_2_joint_1, fingersJoint1ContactPoint);
      addSimulationContactPoint(finger_2_joint_2, fingersJoint2ContactPoint1);
      addSimulationContactPoint(finger_2_joint_2, fingersJoint2ContactPoint2);
      addSimulationContactPoint(finger_2_joint_3, fingersJoint3ContactPoint);
      addSimulationContactPoint(thumb_joint_1, thumbJoint1ContactPoint);
      addSimulationContactPoint(thumb_joint_2, thumbJoint2ContactPoint1);
      addSimulationContactPoint(thumb_joint_2, thumbJoint2ContactPoint2);
      addSimulationContactPoint(thumb_joint_3, thumbJoint3ContactPoint);
   }

   private void createRobotiqHandPalmContactPoints(RobotSide robotSide, String nameOfJointBeforeHand, boolean areHandsFlipped)
   {
      double offsetFromWristToPalmPlane = 0.22; // 0.24
      Point3D palmCenter = new Point3D(-0.002, robotSide.negateIfRightSide(offsetFromWristToPalmPlane), 0.0); // [-0.002, 0.22, 0.0] (-0.002, 0.24, 0.015)
      double palmWidth = 0.07;
      double palmHeight = 0.075;

      if (areHandsFlipped)
         palmHeight = robotSide.negateIfLeftSide(palmHeight);

      // Row of five contact points along center of palm
      Vector3D palmContactPoint1 = new Vector3D(palmCenter.getX() - palmWidth / 2.0, palmCenter.getY(), palmCenter.getZ());
      Vector3D palmContactPoint1b = new Vector3D(palmCenter.getX() - palmWidth / 4.0, palmCenter.getY(), palmCenter.getZ());
      Vector3D palmContactPointCenter = new Vector3D(palmCenter.getX(), palmCenter.getY(), palmCenter.getZ());
      Vector3D palmContactPoint2b = new Vector3D(palmCenter.getX() + palmWidth / 4.0, palmCenter.getY(), palmCenter.getZ());
      Vector3D palmContactPoint3 = new Vector3D(palmCenter.getX() + palmWidth / 2.0, palmCenter.getY(), palmCenter.getZ());

      // Pad that sits between the two outer fingers
      Vector3D palmContactPoint4 = new Vector3D(palmCenter.getX(), palmCenter.getY(), palmCenter.getZ() - palmHeight / 2.0);
      Vector3D palmContactPoint4b = new Vector3D(palmCenter.getX(), palmCenter.getY(), palmCenter.getZ() - palmHeight / 4.0);

      // Two pads that sandwich the center thumb
      Vector3D palmContactPoint5 = new Vector3D(palmContactPoint3.getX(), palmCenter.getY(), palmCenter.getZ() + palmHeight / 2.0);
      Vector3D palmContactPoint5b = new Vector3D(palmContactPoint3.getX(), palmCenter.getY(), palmCenter.getZ() + palmHeight / 4.0);
      Vector3D palmContactPoint6 = new Vector3D(palmContactPoint1.getX(), palmCenter.getY(), palmContactPoint5.getZ());
      Vector3D palmContactPoint6b = new Vector3D(palmContactPoint1.getX(), palmCenter.getY(), palmContactPoint5b.getZ());

      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPointCenter);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint1);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint3);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint4);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint5);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint6);

      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint1b);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint2b);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint4b);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint5b);
      addSimulationContactPoint(nameOfJointBeforeHand, palmContactPoint6b);
   }

   @Override
   public void setupGroundContactModelParameters(LinearGroundContactModel linearGroundContactModel)
   {
      double scale = Math.pow(jointMap.getModelScale(), jointMap.getMassScalePower());

      if (useSoftGroundContactParameters)
      {
         linearGroundContactModel.setZStiffness(scale * 4000.0);
         linearGroundContactModel.setZDamping(scale * 750.0);
         linearGroundContactModel.setXYStiffness(scale * 50000.0);
         linearGroundContactModel.setXYDamping(scale * 1000.0);
      }
      else
      {
         linearGroundContactModel.setZStiffness(scale * 2000.0);
         linearGroundContactModel.setZDamping(scale * 1500.0);
         linearGroundContactModel.setXYStiffness(scale * 50000.0);
         linearGroundContactModel.setXYDamping(scale * 2000.0);
      }
   }

   public int getNumberOfContactableBodies()
   {
      return numberOfContactableBodies;
   }
   
   private class AtlasSixContactFoot implements FootContactPoints<RobotSide>
   {
      @Override
      public Map<String, List<Tuple3DBasics>> getSimulationContactPoints(double footLength, double footWidth, double toeWidth, LeggedJointNameMap<RobotSide> jointMap,
            SegmentDependentList<RobotSide, RigidBodyTransform> soleToAnkleFrameTransforms)
      {
         HashMap<String, List<Tuple3DBasics>> ret = new HashMap<>();

         for (RobotSide robotSide : RobotSide.values)
         {
            ArrayList<Tuple3DBasics> footContactPoints = new ArrayList<>();
            String parentJointName = jointMap.getJointBeforeFootName(robotSide);

            RigidBodyTransform transformToParentJointFrame = soleToAnkleFrameTransforms.get(robotSide);
            Point3D hindLeftContactPoint = new Point3D(-footLength / 2.0, footWidth / 2.0, 0.0);
            transformToParentJointFrame.transform(hindLeftContactPoint);
            footContactPoints.add(hindLeftContactPoint);

            Point3D hindRightContactPoint = new Point3D(-footLength / 2.0, -footWidth / 2.0, 0.0);
            transformToParentJointFrame.transform(hindRightContactPoint);
            footContactPoints.add(hindRightContactPoint);

            Point3D frontLeftContactPoint = new Point3D(footLength / 2.0, toeWidth / 2.0, 0.0);
            transformToParentJointFrame.transform(frontLeftContactPoint);
            footContactPoints.add(frontLeftContactPoint);

            Point3D frontRightContactPoint = new Point3D(footLength / 2.0, -toeWidth / 2.0, 0.0);
            transformToParentJointFrame.transform(frontRightContactPoint);
            footContactPoints.add(frontRightContactPoint);
            
            for (RobotSide footSide : RobotSide.values)
            {
               Point3D extraContactPoint = new Point3D(0.0, footSide.negateIfRightSide(footWidth / 2.0), 0.0);
               transformToParentJointFrame.transform(extraContactPoint);
               footContactPoints.add(extraContactPoint);
            }

            ret.put(parentJointName, footContactPoints);
         }
         return ret;
      }

      @Override
      public boolean useSoftContactPointParameters()
      {
         return false;
      }

      @Override
      public SideDependentList<List<Tuple2DBasics>> getControllerContactPoints(double footLength, double footWidth, double toeWidth)
      {
         SideDependentList<List<Tuple2DBasics>> ret = new SideDependentList<List<Tuple2DBasics>>();

         for (RobotSide robotSide : RobotSide.values)
         {
            ArrayList<Tuple2DBasics> footContactPoints = new ArrayList<>();

            double scale = 0.95;
            Point2D hindLeftContactPoint = new Point2D(-footLength / 2.0 * scale, footWidth / 2.0 * scale);
            footContactPoints.add(hindLeftContactPoint);

            Point2D hindRightContactPoint = new Point2D(-footLength / 2.0 * scale, -footWidth / 2.0 * scale);
            footContactPoints.add(hindRightContactPoint);

            Point2D frontLeftContactPoint = new Point2D(footLength / 2.0 * scale, toeWidth / 2.0 * scale);
            footContactPoints.add(frontLeftContactPoint);

            Point2D frontRightContactPoint = new Point2D(footLength / 2.0 * scale, -toeWidth / 2.0 * scale);
            footContactPoints.add(frontRightContactPoint);

            for (RobotSide footSide : RobotSide.values)
            {
               Point2D extraContactPoint = new Point2D(0.0, footSide.negateIfRightSide(footWidth / 2.0));
               extraContactPoint.scale(scale);
               footContactPoints.add(extraContactPoint);
            }

            ret.put(robotSide, footContactPoints);
         }
         return ret;
      }

      @Override
      public SideDependentList<Tuple2DBasics> getToeOffContactPoints(double footLength, double footWidth, double toeWidth)
      {
         SideDependentList<Tuple2DBasics> ret = new SideDependentList<Tuple2DBasics>();

         for (RobotSide robotSide : RobotSide.values)
         {
            Point2D frontLeftContactPoint = new Point2D(footLength / 2.0, toeWidth);

            Point2D frontRightContactPoint = new Point2D(footLength / 2.0, -toeWidth);

            Point2D toeContactPoint = new Point2D();
            toeContactPoint.interpolate(frontLeftContactPoint, frontRightContactPoint, 0.5);
            ret.put(robotSide, toeContactPoint);
         }
         return ret;
      }

      @Override
      public SideDependentList<LineSegment2D> getToeOffContactLines(double footLength, double footWidth, double toeWidth)
      {
         SideDependentList<LineSegment2D> ret = new SideDependentList<>();

         for (RobotSide robotSide : RobotSide.values)
         {
            Point2D frontLeftContactPoint = new Point2D(footLength / 2.0, toeWidth);
            Point2D frontRightContactPoint = new Point2D(footLength / 2.0, -toeWidth);
            ret.put(robotSide, new LineSegment2D(frontLeftContactPoint, frontRightContactPoint));
         }

         return ret;
      }
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/ghostMouse/GhostMouseKeyReleasePlaybackEvent.java<|end_filename|>
package us.ihmc.tools.inputDevices.ghostMouse;

public class GhostMouseKeyReleasePlaybackEvent implements GhostMousePlaybackEvent
{
   private final int keyCode;

   public GhostMouseKeyReleasePlaybackEvent(int keyCode)
   {
      this.keyCode = keyCode;
   }

   public GhostMouseKeyReleasePlaybackEvent(String keyReleaseString)
   {
      this(GhostMouseStringToChar.convertStringToKeycode(keyReleaseString));
   }
   
   @Override
   public void playback(java.awt.Robot awtRobot, double playbackSpeed)
   {
      awtRobot.keyRelease(keyCode);
   }

   @Override
   public String toString()
   {
      String keyPressString = GhostMouseStringToChar.convertKeycodeToString(keyCode);
      
      return "{" + keyPressString + " up}";
   }
   
}


<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/visibilityGraphs/tools/LinearRegression3D.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.tools;

import java.util.List;

import us.ihmc.euclid.tuple3D.Point3D;

/*
 * TODO This class should be cleaned up really good, tested, and renamed LinearRegression3D, so it
 * can eventually move to a lower level library.
 * Check out IncrementalCovariance3D as reference.
 */
public class LinearRegression3D
{
   private List<Point3D> points;
   private double beta1;
   private double beta0;

   public LinearRegression3D(List<Point3D> points)
   {
      this.points = points;
   }

   public void calculateRegression()
   {
      int MAXN = 1000;
      int n = 0;
      double[] x = new double[MAXN];
      double[] y = new double[MAXN];

      // first pass: read in data, compute xbar and ybar
      double sumx = 0.0, sumy = 0.0, sumx2 = 0.0;

      for (int i = 0; i < points.size(); i++)
      {
         x[n] = points.get(n).getX();
         y[n] = points.get(n).getY();
         sumx += x[n];
         sumx2 += x[n] * x[n];
         sumy += y[n];
         n++;
      }

      double xbar = sumx / n;
      double ybar = sumy / n;

      // second pass: compute summary statistics
      double xxbar = 0.0, yybar = 0.0, xybar = 0.0;
      for (int i = 0; i < n; i++)
      {
         xxbar += (x[i] - xbar) * (x[i] - xbar);
         yybar += (y[i] - ybar) * (y[i] - ybar);
         xybar += (x[i] - xbar) * (y[i] - ybar);
      }
      beta1 = xybar / xxbar;
      beta0 = ybar - beta1 * xbar;

      // print results
      //      System.out.println("y   = " + beta1 + " * x + " + beta0);

      // analyze results
      int df = n - 2;
      double rss = 0.0; // residual sum of squares
      double ssr = 0.0; // regression sum of squares
      for (int i = 0; i < n; i++)
      {
         double fit = beta1 * x[i] + beta0;
         rss += (fit - y[i]) * (fit - y[i]);
         ssr += (fit - ybar) * (fit - ybar);
      }
      double R2 = ssr / yybar;
      double svar = rss / df;
      double svar1 = svar / xxbar;
      double svar0 = svar / n + xbar * xbar * svar1;

      //      System.out.println("R^2                 = " + R2);
      //      System.out.println("std error of beta_1 = " + Math.sqrt(svar1));
      //      System.out.println("std error of beta_0 = " + Math.sqrt(svar0));
      //      svar0 = svar * sumx2 / (n * xxbar);
      //      System.out.println("std error of beta_0 = " + Math.sqrt(svar0));
      //
      //      System.out.println("SSTO = " + yybar);
      //      System.out.println("SSE  = " + rss);
      //      System.out.println("SSR  = " + ssr);
   }

   public double getYFromX(double x)
   {
      return beta1 * x + beta0;
   }

   public Point3D[] getTheTwoPointsFurthestApart()
   {
      double maxDist = 0.0;
      Point3D pointA = new Point3D();
      Point3D pointB = new Point3D();

      for (Point3D observer : points)
      {
         for (Point3D target : points)
         {
            double distance = observer.distance(target);

            if (distance > maxDist)
            {
               maxDist = distance;
               pointA = observer;
               pointB = target;
            }
         }
      }
      return new Point3D[] {pointA, pointB};
   }
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/sensors/CenterOfMassDataHolder.java<|end_filename|>
package us.ihmc.robotics.sensors;

import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
import us.ihmc.robotics.screwTheory.GenericCRC32;

public class CenterOfMassDataHolder implements CenterOfMassDataHolderReadOnly
{
   private final FrameVector3D centerOfMassVelocity = new FrameVector3D();
   
   public void setCenterOfMassVelocity(FrameVector3DReadOnly centerOfMassVelocity)
   {
      this.centerOfMassVelocity.setIncludingFrame(centerOfMassVelocity); 
   }

   public void set(CenterOfMassDataHolder estimatorCenterOfMassDataHolder)
   {
      this.centerOfMassVelocity.setIncludingFrame(estimatorCenterOfMassDataHolder.centerOfMassVelocity);
   }

   @Override
   public void getCenterOfMassVelocity(FrameVector3D centerOfMassVelocityToPack)
   {
      centerOfMassVelocityToPack.setIncludingFrame(centerOfMassVelocity);
   }

   public void calculateChecksum(GenericCRC32 checksum)
   {
      checksum.update(centerOfMassVelocity);
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WrenchTrajectoryMessagePubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "WrenchTrajectoryMessage" defined in "WrenchTrajectoryMessage_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from WrenchTrajectoryMessage_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit WrenchTrajectoryMessage_.idl instead.
*
*/
public class WrenchTrajectoryMessagePubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.WrenchTrajectoryMessage>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::WrenchTrajectoryMessage_";

   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.WrenchTrajectoryMessage 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.WrenchTrajectoryMessage data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 50; ++i0)
      {
          current_alignment += controller_msgs.msg.dds.WrenchTrajectoryPointMessagePubSubType.getMaxCdrSerializedSize(current_alignment);}
      current_alignment += controller_msgs.msg.dds.FrameInformationPubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += geometry_msgs.msg.dds.PosePubSubType.getMaxCdrSerializedSize(current_alignment);

      current_alignment += controller_msgs.msg.dds.QueueableMessagePubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WrenchTrajectoryMessage data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.WrenchTrajectoryMessage data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      for(int i0 = 0; i0 < data.getWrenchTrajectoryPoints().size(); ++i0)
      {
          current_alignment += controller_msgs.msg.dds.WrenchTrajectoryPointMessagePubSubType.getCdrSerializedSize(data.getWrenchTrajectoryPoints().get(i0), current_alignment);}

      current_alignment += controller_msgs.msg.dds.FrameInformationPubSubType.getCdrSerializedSize(data.getFrameInformation(), current_alignment);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      current_alignment += geometry_msgs.msg.dds.PosePubSubType.getCdrSerializedSize(data.getControlFramePose(), current_alignment);

      current_alignment += controller_msgs.msg.dds.QueueableMessagePubSubType.getCdrSerializedSize(data.getQueueingProperties(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.WrenchTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getWrenchTrajectoryPoints().size() <= 50)
      cdr.write_type_e(data.getWrenchTrajectoryPoints());else
          throw new RuntimeException("wrench_trajectory_points field exceeds the maximum length");

      controller_msgs.msg.dds.FrameInformationPubSubType.write(data.getFrameInformation(), cdr);
      cdr.write_type_7(data.getUseCustomControlFrame());

      geometry_msgs.msg.dds.PosePubSubType.write(data.getControlFramePose(), cdr);
      controller_msgs.msg.dds.QueueableMessagePubSubType.write(data.getQueueingProperties(), cdr);
   }

   public static void read(controller_msgs.msg.dds.WrenchTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_e(data.getWrenchTrajectoryPoints());	
      controller_msgs.msg.dds.FrameInformationPubSubType.read(data.getFrameInformation(), cdr);	
      data.setUseCustomControlFrame(cdr.read_type_7());
      	
      geometry_msgs.msg.dds.PosePubSubType.read(data.getControlFramePose(), cdr);	
      controller_msgs.msg.dds.QueueableMessagePubSubType.read(data.getQueueingProperties(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.WrenchTrajectoryMessage data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_e("wrench_trajectory_points", data.getWrenchTrajectoryPoints());
      ser.write_type_a("frame_information", new controller_msgs.msg.dds.FrameInformationPubSubType(), data.getFrameInformation());

      ser.write_type_7("use_custom_control_frame", data.getUseCustomControlFrame());
      ser.write_type_a("control_frame_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getControlFramePose());

      ser.write_type_a("queueing_properties", new controller_msgs.msg.dds.QueueableMessagePubSubType(), data.getQueueingProperties());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.WrenchTrajectoryMessage data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_e("wrench_trajectory_points", data.getWrenchTrajectoryPoints());
      ser.read_type_a("frame_information", new controller_msgs.msg.dds.FrameInformationPubSubType(), data.getFrameInformation());

      data.setUseCustomControlFrame(ser.read_type_7("use_custom_control_frame"));
      ser.read_type_a("control_frame_pose", new geometry_msgs.msg.dds.PosePubSubType(), data.getControlFramePose());

      ser.read_type_a("queueing_properties", new controller_msgs.msg.dds.QueueableMessagePubSubType(), data.getQueueingProperties());

   }

   public static void staticCopy(controller_msgs.msg.dds.WrenchTrajectoryMessage src, controller_msgs.msg.dds.WrenchTrajectoryMessage dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.WrenchTrajectoryMessage createData()
   {
      return new controller_msgs.msg.dds.WrenchTrajectoryMessage();
   }
   @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.WrenchTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.WrenchTrajectoryMessage data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.WrenchTrajectoryMessage src, controller_msgs.msg.dds.WrenchTrajectoryMessage dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public WrenchTrajectoryMessagePubSubType newInstance()
   {
      return new WrenchTrajectoryMessagePubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/TotalMassCalculator.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.List;

import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.interfaces.SpatialInertiaBasics;

public class TotalMassCalculator
{
   public static double computeSubTreeMass(RigidBodyBasics rootBody)
   {
      SpatialInertiaBasics inertia = rootBody.getInertia();
      double ret = inertia == null ? 0.0 : inertia.getMass();

      for (JointBasics childJoint : rootBody.getChildrenJoints())
      {
         ret += computeSubTreeMass(childJoint.getSuccessor());
      }

      return ret;
   }

   public static double computeMass(RigidBodyBasics[] rigidBodies)
   {
      double ret = 0.0;
      for (int i = 0; i < rigidBodies.length; i++)
      {
         ret += rigidBodies[i].getInertia().getMass();
      }
      return ret;
   }

   public static double computeMass(Iterable<RigidBodyBasics> rigidBodies)
   {
      double ret = 0.0;
      for (RigidBodyBasics rigidBody : rigidBodies)
      {
         ret += rigidBody.getInertia().getMass();
      }
      return ret;
   }

   public static double computeMass(List<RigidBodyBasics> rigidBodies)
   {
      double ret = 0.0;
      for (int i = 0; i < rigidBodies.size(); i++)
      {
         ret += rigidBodies.get(i).getInertia().getMass();
      }
      return ret;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/FrameInformation.java<|end_filename|>
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;

public class FrameInformation extends Packet<FrameInformation> implements Settable<FrameInformation>, EpsilonComparable<FrameInformation>
{
   /**
          * This message is part of the IHMC whole-body controller API.
          * This message carries the frame related information needed for some messages such as taskspace trajectories.
          * Valid codes and their associated frames include:"
          */
   public static final long WORLD_FRAME = 83766130;
   public static final long MIDFEET_ZUP_FRAME = -100;
   public static final long PELVIS_ZUP_FRAME = -101;
   public static final long PELVIS_FRAME = -102;
   public static final long CHEST_FRAME = -103;
   public static final long CENTER_OF_MASS_FRAME = -104;
   public static final long LEFT_SOLE_FRAME = -105;
   public static final long RIGHT_SOLE_FRAME = -106;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The ID of the reference frame that a trajectory is executed in. Default value is WORLD_FRAME
            */
   public long trajectory_reference_frame_id_ = 83766130;
   /**
            * The ID of the reference frame that trajectory data in a packet is expressed in. The frame of the
            * trajectory data will be switched to the trajectory frame immediately when the message is received
            * by the controller. If set to the value 1 it will be
            * assumed that this is the same frame as the trajectory frame.
            * It is recommended that this should be the same frame as the trajectory_reference_frame_id to
            * avoid unexpected behavior. When different, the controller will change the data to be expressed
            * in the trajectory frame at reception of the message.
            * The data frame is only useful if the user is unable to change the frame the data is expressed in
            * to the trajectory frame. However, unexpected behavior might occur if the data frame is moving
            * with respect to the trajectory frame during execution. To highlight this consider the following
            * example:
            * A hand trajectory needs to be executed while the robot walks to a location in world. The hand
            * trajectory might be known in world frame but for safety the trajectory execution frame is set
            * to a frame attached to the robot. If the data is packed in world and the data frame is set to world
            * this will cause the resulting trajectory to be wrong since the transformation to trajectory frame
            * happens at the start of execution rather than every controller tick.
            */
   public long data_reference_frame_id_ = 1;

   public FrameInformation()
   {
   }

   public FrameInformation(FrameInformation other)
   {
      this();
      set(other);
   }

   public void set(FrameInformation other)
   {
      sequence_id_ = other.sequence_id_;

      trajectory_reference_frame_id_ = other.trajectory_reference_frame_id_;

      data_reference_frame_id_ = other.data_reference_frame_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_;
   }

   /**
            * The ID of the reference frame that a trajectory is executed in. Default value is WORLD_FRAME
            */
   public void setTrajectoryReferenceFrameId(long trajectory_reference_frame_id)
   {
      trajectory_reference_frame_id_ = trajectory_reference_frame_id;
   }
   /**
            * The ID of the reference frame that a trajectory is executed in. Default value is WORLD_FRAME
            */
   public long getTrajectoryReferenceFrameId()
   {
      return trajectory_reference_frame_id_;
   }

   /**
            * The ID of the reference frame that trajectory data in a packet is expressed in. The frame of the
            * trajectory data will be switched to the trajectory frame immediately when the message is received
            * by the controller. If set to the value 1 it will be
            * assumed that this is the same frame as the trajectory frame.
            * It is recommended that this should be the same frame as the trajectory_reference_frame_id to
            * avoid unexpected behavior. When different, the controller will change the data to be expressed
            * in the trajectory frame at reception of the message.
            * The data frame is only useful if the user is unable to change the frame the data is expressed in
            * to the trajectory frame. However, unexpected behavior might occur if the data frame is moving
            * with respect to the trajectory frame during execution. To highlight this consider the following
            * example:
            * A hand trajectory needs to be executed while the robot walks to a location in world. The hand
            * trajectory might be known in world frame but for safety the trajectory execution frame is set
            * to a frame attached to the robot. If the data is packed in world and the data frame is set to world
            * this will cause the resulting trajectory to be wrong since the transformation to trajectory frame
            * happens at the start of execution rather than every controller tick.
            */
   public void setDataReferenceFrameId(long data_reference_frame_id)
   {
      data_reference_frame_id_ = data_reference_frame_id;
   }
   /**
            * The ID of the reference frame that trajectory data in a packet is expressed in. The frame of the
            * trajectory data will be switched to the trajectory frame immediately when the message is received
            * by the controller. If set to the value 1 it will be
            * assumed that this is the same frame as the trajectory frame.
            * It is recommended that this should be the same frame as the trajectory_reference_frame_id to
            * avoid unexpected behavior. When different, the controller will change the data to be expressed
            * in the trajectory frame at reception of the message.
            * The data frame is only useful if the user is unable to change the frame the data is expressed in
            * to the trajectory frame. However, unexpected behavior might occur if the data frame is moving
            * with respect to the trajectory frame during execution. To highlight this consider the following
            * example:
            * A hand trajectory needs to be executed while the robot walks to a location in world. The hand
            * trajectory might be known in world frame but for safety the trajectory execution frame is set
            * to a frame attached to the robot. If the data is packed in world and the data frame is set to world
            * this will cause the resulting trajectory to be wrong since the transformation to trajectory frame
            * happens at the start of execution rather than every controller tick.
            */
   public long getDataReferenceFrameId()
   {
      return data_reference_frame_id_;
   }


   public static Supplier<FrameInformationPubSubType> getPubSubType()
   {
      return FrameInformationPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return FrameInformationPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(FrameInformation other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.trajectory_reference_frame_id_, other.trajectory_reference_frame_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.data_reference_frame_id_, other.data_reference_frame_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 FrameInformation)) return false;

      FrameInformation otherMyClass = (FrameInformation) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.trajectory_reference_frame_id_ != otherMyClass.trajectory_reference_frame_id_) return false;

      if(this.data_reference_frame_id_ != otherMyClass.data_reference_frame_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("FrameInformation {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("trajectory_reference_frame_id=");
      builder.append(this.trajectory_reference_frame_id_);      builder.append(", ");
      builder.append("data_reference_frame_id=");
      builder.append(this.data_reference_frame_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/DesiredAccelerationsMessage.java<|end_filename|>
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.
       * General message that carries desired joint accelerations.
       * It is used by ArmDesiredAccelerationsMessage, SpineDesiredAccelerationsMessage, NeckDesiredAccelerationsMessage.
       */
public class DesiredAccelerationsMessage extends Packet<DesiredAccelerationsMessage> implements Settable<DesiredAccelerationsMessage>, EpsilonComparable<DesiredAccelerationsMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Specifies the desired joint accelerations.
            */
   public us.ihmc.idl.IDLSequence.Double  desired_joint_accelerations_;
   /**
            * Properties for queueing commands.
            */
   public controller_msgs.msg.dds.QueueableMessage queueing_properties_;

   public DesiredAccelerationsMessage()
   {
      desired_joint_accelerations_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      queueing_properties_ = new controller_msgs.msg.dds.QueueableMessage();
   }

   public DesiredAccelerationsMessage(DesiredAccelerationsMessage other)
   {
      this();
      set(other);
   }

   public void set(DesiredAccelerationsMessage other)
   {
      sequence_id_ = other.sequence_id_;

      desired_joint_accelerations_.set(other.desired_joint_accelerations_);
      controller_msgs.msg.dds.QueueableMessagePubSubType.staticCopy(other.queueing_properties_, queueing_properties_);
   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }


   /**
            * Specifies the desired joint accelerations.
            */
   public us.ihmc.idl.IDLSequence.Double  getDesiredJointAccelerations()
   {
      return desired_joint_accelerations_;
   }


   /**
            * Properties for queueing commands.
            */
   public controller_msgs.msg.dds.QueueableMessage getQueueingProperties()
   {
      return queueing_properties_;
   }


   public static Supplier<DesiredAccelerationsMessagePubSubType> getPubSubType()
   {
      return DesiredAccelerationsMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return DesiredAccelerationsMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(DesiredAccelerationsMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.desired_joint_accelerations_, other.desired_joint_accelerations_, epsilon)) return false;

      if (!this.queueing_properties_.epsilonEquals(other.queueing_properties_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof DesiredAccelerationsMessage)) return false;

      DesiredAccelerationsMessage otherMyClass = (DesiredAccelerationsMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.desired_joint_accelerations_.equals(otherMyClass.desired_joint_accelerations_)) return false;
      if (!this.queueing_properties_.equals(otherMyClass.queueing_properties_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("DesiredAccelerationsMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("desired_joint_accelerations=");
      builder.append(this.desired_joint_accelerations_);      builder.append(", ");
      builder.append("queueing_properties=");
      builder.append(this.queueing_properties_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/MessageCollection.java<|end_filename|>
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 mainly with the IHMC whole-body controller.
       * When the execution of a collection of messages is to be synchronized, these messages should be attributed
       * a unique sequence ID. Then by sending beforehand a MessageCollection holding onto the sequence IDs of all these messages,
       * the controller will wait to receive all the messages before processing them.
       */
public class MessageCollection extends Packet<MessageCollection> implements Settable<MessageCollection>, EpsilonComparable<MessageCollection>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The sequence IDs of all the messages that are expected to be received.
            */
   public us.ihmc.idl.IDLSequence.Long  sequences_;

   public MessageCollection()
   {
      sequences_ = new us.ihmc.idl.IDLSequence.Long (100, "type_4");

   }

   public MessageCollection(MessageCollection other)
   {
      this();
      set(other);
   }

   public void set(MessageCollection other)
   {
      sequence_id_ = other.sequence_id_;

      sequences_.set(other.sequences_);
   }

   /**
            * 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_;
   }


   /**
            * The sequence IDs of all the messages that are expected to be received.
            */
   public us.ihmc.idl.IDLSequence.Long  getSequences()
   {
      return sequences_;
   }


   public static Supplier<MessageCollectionPubSubType> getPubSubType()
   {
      return MessageCollectionPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return MessageCollectionPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(MessageCollection other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsLongSequence(this.sequences_, other.sequences_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof MessageCollection)) return false;

      MessageCollection otherMyClass = (MessageCollection) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.sequences_.equals(otherMyClass.sequences_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("MessageCollection {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("sequences=");
      builder.append(this.sequences_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/fingers/ValkyrieHandJointName.java<|end_filename|>
package us.ihmc.valkyrie.fingers;

import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HandJointName;
import us.ihmc.mecano.multiBodySystem.RevoluteJoint;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.robotics.partNames.FingerName;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.simulationconstructionset.FloatingRootJointRobot;
import us.ihmc.simulationconstructionset.PinJoint;

public enum ValkyrieHandJointName implements HandJointName
{
   ThumbRoll,
   ThumbPitch1,
   ThumbPitch2,
   ThumbPitch3,
   IndexFingerPitch1,
   IndexFingerPitch2,
   IndexFingerPitch3,
   MiddleFingerPitch1,
   MiddleFingerPitch2,
   MiddleFingerPitch3,
   PinkyPitch1,
   PinkyPitch2,
   PinkyPitch3;

   public static final ValkyrieHandJointName[] values = values();

   @Override
   public String getJointName(RobotSide robotSide)
   {
      return getCamelCaseJointName(robotSide);
   }

   public String getCamelCaseJointName(RobotSide side)
   {
      return side.getCamelCaseName() + name();
   }

   public String getPascalCaseJointName(RobotSide side)
   {
      return side.getPascalCaseName() + name();
   }

   @Override
   public FingerName getFinger(RobotSide robotSide)
   {
      return getFingerName();
   }

   public FingerName getFingerName()
   {
      switch (this)
      {
      case ThumbRoll:
      case ThumbPitch1:
      case ThumbPitch2:
      case ThumbPitch3:
         return FingerName.THUMB;
      case IndexFingerPitch1:
      case IndexFingerPitch2:
      case IndexFingerPitch3:
         return FingerName.INDEX;
      case MiddleFingerPitch1:
      case MiddleFingerPitch2:
      case MiddleFingerPitch3:
         return FingerName.MIDDLE;
      case PinkyPitch1:
      case PinkyPitch2:
      case PinkyPitch3:
         return FingerName.PINKY;
      default:
         throw new RuntimeException("Unexpected " + getClass().getSimpleName() + " value: " + this);
      }
   }

   public static int getNumberOfFingerJoints(FingerName fingerName)
   {
      int numberOfFingerJoints = 0;
      for (ValkyrieHandJointName joint : values)
      {
         if (joint.getFingerName() == fingerName)
            numberOfFingerJoints++;
      }
      return numberOfFingerJoints;
   }

   @Override
   public int getIndex(RobotSide robotSide)
   {
      return ordinal();
   }

   @Override
   public ValkyrieHandJointName[] getValues()
   {
      return values;
   }

   public RevoluteJoint getRelatedRevoluteJoint(RobotSide robotSide, FullRobotModel fullRobotModel)
   {
      return (RevoluteJoint) fullRobotModel.getOneDoFJointByName(getJointName(robotSide));
   }

   public PinJoint getRelatedPinJoint(RobotSide robotSide, FloatingRootJointRobot sdfRobot)
   {
      return (PinJoint) sdfRobot.getOneDegreeOfFreedomJoint(getJointName(robotSide));
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/HatchEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.List;

import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationConstructionSetTools.robotController.ContactController;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;


public class HatchEnvironment implements CommonAvatarEnvironmentInterface
{
   private final CombinedTerrainObject3D combinedTerrainObject;
      
   private final float HATCH_OPENING_WIDTH = .86f;
   private final float HATCH_UPENING_HEIGHT = 1.7f;
   private final float HATCH_UPENING_HEIGHT_OFF_GROUND = 0.152f;
   private final float FORWARD_OFFSET = 1f;
   private final float HATCH_THICKNESS = 0.152f;


   
   public HatchEnvironment()
   {
      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
      combinedTerrainObject.addTerrainObject(setUpGround("Ground"));
   }
   
   private CombinedTerrainObject3D setUpGround(String name)
   {
      CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);

      combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.Gray());
      combinedTerrainObject.addBox(FORWARD_OFFSET,HATCH_OPENING_WIDTH/2,  FORWARD_OFFSET+HATCH_THICKNESS,(HATCH_OPENING_WIDTH/2)+0.5f, 0, HATCH_UPENING_HEIGHT+HATCH_UPENING_HEIGHT_OFF_GROUND, YoAppearance.DarkGray());
      combinedTerrainObject.addBox(FORWARD_OFFSET,-HATCH_OPENING_WIDTH/2,  FORWARD_OFFSET+HATCH_THICKNESS,-(HATCH_OPENING_WIDTH/2)-0.5f, 0, HATCH_UPENING_HEIGHT+HATCH_UPENING_HEIGHT_OFF_GROUND, YoAppearance.DarkGray());

      combinedTerrainObject.addBox(FORWARD_OFFSET,(HATCH_OPENING_WIDTH/2)+0.5f,  FORWARD_OFFSET+HATCH_THICKNESS,-(HATCH_OPENING_WIDTH/2)-0.5f, 0, HATCH_UPENING_HEIGHT_OFF_GROUND, YoAppearance.DarkGray());

      combinedTerrainObject.addBox(FORWARD_OFFSET,(HATCH_OPENING_WIDTH/2)+0.5f,  FORWARD_OFFSET+HATCH_THICKNESS,-(HATCH_OPENING_WIDTH/2)-0.5f, HATCH_UPENING_HEIGHT+ HATCH_UPENING_HEIGHT_OFF_GROUND, HATCH_UPENING_HEIGHT+ HATCH_UPENING_HEIGHT_OFF_GROUND+HATCH_THICKNESS, YoAppearance.DarkGray());

      
     // combinedTerrainObject.addBox(2.0, -0.05, 3.0, 0.05, 2.0, YoAppearance.Beige());
     // combinedTerrainObject.addBox(3.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.getX(), -0.05, 4.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.getX(), 0.05, 2.0, YoAppearance.Beige());
      
      return combinedTerrainObject;
   }
   
   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return null;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
      ContactController contactController = new ContactController();
      contactController.setContactParameters(100000.0, 100.0, 0.5, 0.3);
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
   
   

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/robotController/SliderBoardControllerInitializer.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.robotController;

import java.util.ArrayList;

public class SliderBoardControllerInitializer
{
   private static ArrayList<SliderBoardRobotController> robotControllers = new ArrayList<SliderBoardRobotController>();

   public static void register(SliderBoardRobotController sliderBoardRobotController)
   {
      robotControllers.add(sliderBoardRobotController);
   }

   public static void initialize()
   {
      for (SliderBoardRobotController sliderBoardRobotController : robotControllers)
      {
         sliderBoardRobotController.initializeSliders();
      }
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/operatorInterfaceDebugging/AtlasUIPacketLoader.java<|end_filename|>
package us.ihmc.atlas.operatorInterfaceDebugging;

import java.io.BufferedReader;
import java.io.DataInputStream;
import java.io.IOException;
import java.nio.file.Files;
import java.util.Scanner;

import us.ihmc.commons.Conversions;
import us.ihmc.commons.PrintTools;
import us.ihmc.commons.nio.FileTools;
import us.ihmc.communication.net.KryoStreamDeSerializer;
import us.ihmc.communication.net.ConnectionStateListener;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.Packet;
import us.ihmc.communication.util.NetworkPorts;
import us.ihmc.humanoidRobotics.kryo.IHMCCommunicationKryoNetClassList;
import us.ihmc.commons.thread.ThreadTools;

public class AtlasUIPacketLoader
{
   private static final boolean PRESS_ENTER = false;
   private static final boolean LOOP_PLAYBACK = false;
   private static final double PLAYBACK_SPEED = 0.5;

   public AtlasUIPacketLoader() throws IOException
   {

      IHMCCommunicationKryoNetClassList netClassList = new IHMCCommunicationKryoNetClassList();

      final KryoStreamDeSerializer kryoStreamDeSerializer = new KryoStreamDeSerializer(Conversions.megabytesToBytes(500));
      kryoStreamDeSerializer.registerClasses(netClassList);

      final PacketCommunicator packetCommunicator = PacketCommunicator.createTCPPacketCommunicatorServer(NetworkPorts.NETWORK_PROCESSOR_TO_UI_TCP_PORT, Conversions.megabytesToBytes(500), Conversions.megabytesToBytes(500), netClassList);
      packetCommunicator.connect();
      packetCommunicator.attachStateListener(new ConnectionStateListener()
      {
         private DataInputStream fileDataInputStream;
         private BufferedReader timingReader;

         public void openFileDataStream() throws IOException
         {
            fileDataInputStream = FileTools.newFileDataInputStream(AtlasUIPacketRecorder.getPacketRecordingFilePath(), Conversions.megabytesToBytes(500));
            timingReader = Files.newBufferedReader(AtlasUIPacketRecorder.getPacketTimingPath());
         }

         @Override
         public void disconnected()
         {
         }

         @Override
         public void connected()
         {
            if (PRESS_ENTER)
               PrintTools.info("Connected. Press Enter to send...");
            else
               PrintTools.info("Connected");

            try
            {
               if (PRESS_ENTER)
               {
                  Scanner scanner = new Scanner(System.in);
                  scanner.nextLine();
                  scanner.close();
               }

               openFileDataStream();

               Packet<?> packet = null;

               do
               {
                  try
                  {
                     packet = (Packet<?>) kryoStreamDeSerializer.read(fileDataInputStream);
                     PrintTools.info("Sending: " + packet);

                     packetCommunicator.send(packet);

                     double timeToWait = 0.01;
                     if (timingReader.ready())
                        timeToWait = Double.valueOf(timingReader.readLine()) / PLAYBACK_SPEED;

                     ThreadTools.sleepSeconds(timeToWait);
                  }
                  catch (IOException e)
                  {
                     PrintTools.error(e.getMessage());
                  }

                  if (LOOP_PLAYBACK)
                  {
                     if (fileDataInputStream.available() < 1)
                     {
                        fileDataInputStream.close();

                        ThreadTools.sleepSeconds(0.5);

                        openFileDataStream();
                     }
                  }
               }
               while (packet != null && fileDataInputStream.available() > 1);

               fileDataInputStream.close();

               packetCommunicator.disconnect();
            }
            catch (IOException e)
            {
               e.printStackTrace();
            }
         }
      });
   }

   public static void main(String[] args) throws IOException
   {
      new AtlasUIPacketLoader();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HeatMapPacket.java<|end_filename|>
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;

public class HeatMapPacket extends Packet<HeatMapPacket> implements Settable<HeatMapPacket>, EpsilonComparable<HeatMapPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.Float  data_;
   public int width_;
   public int height_;
   public java.lang.StringBuilder name_;

   public HeatMapPacket()
   {
      data_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5");

      name_ = new java.lang.StringBuilder(255);
   }

   public HeatMapPacket(HeatMapPacket other)
   {
      this();
      set(other);
   }

   public void set(HeatMapPacket other)
   {
      sequence_id_ = other.sequence_id_;

      data_.set(other.data_);
      width_ = other.width_;

      height_ = other.height_;

      name_.setLength(0);
      name_.append(other.name_);

   }

   /**
            * 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 us.ihmc.idl.IDLSequence.Float  getData()
   {
      return data_;
   }

   public void setWidth(int width)
   {
      width_ = width;
   }
   public int getWidth()
   {
      return width_;
   }

   public void setHeight(int height)
   {
      height_ = height;
   }
   public int getHeight()
   {
      return height_;
   }

   public void setName(java.lang.String name)
   {
      name_.setLength(0);
      name_.append(name);
   }

   public java.lang.String getNameAsString()
   {
      return getName().toString();
   }
   public java.lang.StringBuilder getName()
   {
      return name_;
   }


   public static Supplier<HeatMapPacketPubSubType> getPubSubType()
   {
      return HeatMapPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HeatMapPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HeatMapPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.data_, other.data_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.width_, other.width_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.height_, other.height_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.name_, other.name_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HeatMapPacket)) return false;

      HeatMapPacket otherMyClass = (HeatMapPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.data_.equals(otherMyClass.data_)) return false;
      if(this.width_ != otherMyClass.width_) return false;

      if(this.height_ != otherMyClass.height_) return false;

      if (!us.ihmc.idl.IDLTools.equals(this.name_, otherMyClass.name_)) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HeatMapPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("data=");
      builder.append(this.data_);      builder.append(", ");
      builder.append("width=");
      builder.append(this.width_);      builder.append(", ");
      builder.append("height=");
      builder.append(this.height_);      builder.append(", ");
      builder.append("name=");
      builder.append(this.name_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/mouse3DJoystick/Mouse3DJoystick.java<|end_filename|>
package us.ihmc.tools.inputDevices.mouse3DJoystick;

import java.util.HashMap;
import java.util.Map;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;

import net.java.games.input.Component;
import net.java.games.input.Controller;
import us.ihmc.commons.PrintTools;
import us.ihmc.commons.time.Stopwatch;
import us.ihmc.graphicsDescription.input.mouse.Mouse3DInterface;
import us.ihmc.graphicsDescription.input.mouse.Mouse3DListener;
import us.ihmc.graphicsDescription.input.mouse.Mouse3DListenerHolder;
import us.ihmc.tools.inputDevices.JInputTools;
import us.ihmc.tools.inputDevices.JInputTools.ControllerType;
import us.ihmc.commons.thread.ThreadTools;

public class Mouse3DJoystick implements Mouse3DInterface
{
   private final long POLL_PERIOD_MILLISECONDS = 10;
   private final double POLL_PERIOD_SECONDS = POLL_PERIOD_MILLISECONDS / 1e3;
   
   public final double DEAD_ZONE = 0.001;
   
   public final Map<String, Double> controllerScales = new HashMap<>();
   
   public final double MAX_VALUE = 1.0;
   public final double MIN_VALUE = -1.0;
   
   private final double X_SIGN = 1.0;
   private final double Y_SIGN = -1.0;
   private final double Z_SIGN = -1.0;
   private final double RX_SIGN = 1.0;
   private final double RY_SIGN = 1.0;
   private final double RZ_SIGN = -1.0;
   
   private Mouse3DListenerHolder mouse3DListenerHolder = new Mouse3DListenerHolder();
   private Controller controller;
   private double scale;
   private ScheduledExecutorService scheduledExecutorService;
   private Stopwatch timer = new Stopwatch();
   
   public Mouse3DJoystick()
   {
      controllerScales.put("Space Navigator", 1.0 / 350.0);
      controllerScales.put("SpaceMouse Wireless Receiver", 1.0);
      controllerScales.put("SpaceMouse Pro", 1.0);
      controllerScales.put("SpaceM", 1.0);

      Map<ControllerType, Controller> controllersByType = JInputTools.getControllersByType();
      
      if (controllersByType.containsKey(ControllerType.JOYSTICK_3D))
      {
         controller = controllersByType.get(ControllerType.JOYSTICK_3D);
         
         PrintTools.info(this, "Using Mouse3DJoystick: " + controller.getName());
         
         scale = controllerScales.get(controller.getName());
         
         scheduledExecutorService = Executors.newSingleThreadScheduledExecutor(ThreadTools.getNamedThreadFactory(getClass().getSimpleName()));
         scheduledExecutorService.scheduleAtFixedRate(new Mouse3DJoystickPollService(), 0, POLL_PERIOD_MILLISECONDS, TimeUnit.MILLISECONDS);
      }
   }
   
   private class Mouse3DJoystickPollService implements Runnable
   {
      private boolean timerStarted = false;
      
      @Override
      public void run()
      {
         if (!timerStarted)
         {
            timer.start();
            timerStarted = true;
            return;
         }
         
         controller.poll();
         double lap = timer.lap();
         double timeScale = lap / POLL_PERIOD_SECONDS;

         double dx = X_SIGN * controller.getComponent(Component.Identifier.Axis.X).getPollData() * scale * timeScale;
         double dy = Y_SIGN * controller.getComponent(Component.Identifier.Axis.Y).getPollData() * scale * timeScale;
         double dz = Z_SIGN * controller.getComponent(Component.Identifier.Axis.Z).getPollData() * scale * timeScale;
         double drx = RX_SIGN * controller.getComponent(Component.Identifier.Axis.RX).getPollData() * scale * timeScale;
         double dry = RY_SIGN * controller.getComponent(Component.Identifier.Axis.RY).getPollData() * scale * timeScale;
         double drz = RZ_SIGN * controller.getComponent(Component.Identifier.Axis.RZ).getPollData() * scale * timeScale;
         
         if (Math.abs(dx) < DEAD_ZONE) dx = 0.0;
         if (Math.abs(dy) < DEAD_ZONE) dy = 0.0;
         if (Math.abs(dz) < DEAD_ZONE) dz = 0.0;
         if (Math.abs(drx) < DEAD_ZONE) drx = 0.0;
         if (Math.abs(dry) < DEAD_ZONE) dry = 0.0;
         if (Math.abs(drz) < DEAD_ZONE) drz = 0.0;
         
         if (!(dx == 0.0 && dy == 0.0 && dz == 0.0 && drx == 0.0 && dry == 0.0 && drz == 0.0))
            mouse3DListenerHolder.mouseDragged(dx, dy, dz, drx, dry, drz);
      }
   }
   
   public void addMouse3DListener(Mouse3DListener mouse3DListener)
   {
      mouse3DListenerHolder.addMouse3DListener(mouse3DListener);
   }
   
   public void stopPolling()
   {
      if (scheduledExecutorService != null)
         scheduledExecutorService.shutdown();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HumanoidBehaviorTypePacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class HumanoidBehaviorTypePacket extends Packet<HumanoidBehaviorTypePacket> implements Settable<HumanoidBehaviorTypePacket>, EpsilonComparable<HumanoidBehaviorTypePacket>
{
   public static final byte STOP = (byte) 0;
   public static final byte TEST = (byte) 1;
   public static final byte WALK_TO_LOCATION = (byte) 2;
   public static final byte WALK_TO_GOAL = (byte) 3;
   public static final byte DIAGNOSTIC = (byte) 4;
   public static final byte PICK_UP_BALL = (byte) 5;
   public static final byte RESET_ROBOT = (byte) 6;
   public static final byte TURN_VALVE = (byte) 7;
   public static final byte WALK_THROUGH_DOOR = (byte) 8;
   public static final byte EXAMPLE_BEHAVIOR = (byte) 9;
   public static final byte BALL_DETECTION = (byte) 10;
   public static final byte TEST_PIPELINE = (byte) 11;
   public static final byte TEST_STATEMACHINE = (byte) 12;
   public static final byte FOLLOW_FIDUCIAL_50 = (byte) 13;
   public static final byte LOCATE_FIDUCIAL = (byte) 14;
   public static final byte WALK_OVER_TERRAIN = (byte) 15;
   public static final byte FOLLOW_VALVE = (byte) 16;
   public static final byte LOCATE_VALVE = (byte) 17;
   public static final byte WALK_OVER_TERRAIN_TO_VALVE = (byte) 18;
   public static final byte DEBUG_PARTIAL_FOOTHOLDS = (byte) 19;
   public static final byte WALK_TO_GOAL_ANYTIME_PLANNER = (byte) 20;
   public static final byte TEST_ICP_OPTIMIZATION = (byte) 21;
   public static final byte TEST_GC_GENERATION = (byte) 22;
   public static final byte TEST_SMOOTH_ICP_PLANNER = (byte) 23;
   public static final byte PUSH_AND_WALK = (byte) 24;
   public static final byte COLLABORATIVE_TASK = (byte) 25;
   public static final byte FIRE_FIGHTING = (byte) 26;
   public static final byte CUTTING_WALL = (byte) 27;
   public static final byte REPEATEDLY_WALK_FOOTSTEP_LIST = (byte) 28;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte humanoid_behavior_type_ = (byte) 255;

   public HumanoidBehaviorTypePacket()
   {
   }

   public HumanoidBehaviorTypePacket(HumanoidBehaviorTypePacket other)
   {
      this();
      set(other);
   }

   public void set(HumanoidBehaviorTypePacket other)
   {
      sequence_id_ = other.sequence_id_;

      humanoid_behavior_type_ = other.humanoid_behavior_type_;

   }

   /**
            * 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 void setHumanoidBehaviorType(byte humanoid_behavior_type)
   {
      humanoid_behavior_type_ = humanoid_behavior_type;
   }
   public byte getHumanoidBehaviorType()
   {
      return humanoid_behavior_type_;
   }


   public static Supplier<HumanoidBehaviorTypePacketPubSubType> getPubSubType()
   {
      return HumanoidBehaviorTypePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HumanoidBehaviorTypePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HumanoidBehaviorTypePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.humanoid_behavior_type_, other.humanoid_behavior_type_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HumanoidBehaviorTypePacket)) return false;

      HumanoidBehaviorTypePacket otherMyClass = (HumanoidBehaviorTypePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.humanoid_behavior_type_ != otherMyClass.humanoid_behavior_type_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HumanoidBehaviorTypePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("humanoid_behavior_type=");
      builder.append(this.humanoid_behavior_type_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/dynamicPlanning/slipJumping/SLIPState.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.dynamicPlanning.slipJumping;

public enum SLIPState
{
   STANCE, FLIGHT;

   public final static int stateVectorSize = 12;
   public final static int controlVectorSize = 9;
   public final static int constantVectorSize = 2;

   public final static int x = 0;
   public final static int y = 1;
   public final static int z = 2;
   public final static int thetaX = 3;
   public final static int thetaY = 4;
   public final static int thetaZ = 5;
   public final static int xDot = 6;
   public final static int yDot = 7;
   public final static int zDot = 8;
   public final static int thetaXDot = 9;
   public final static int thetaYDot = 10;
   public final static int thetaZDot = 11;

   public final static int fx = 0;
   public final static int fy = 1;
   public final static int fz = 2;
   public final static int tauX = 3;
   public final static int tauY = 4;
   public final static int tauZ = 5;
   public final static int xF = 6;
   public final static int yF = 7;
   public final static int k = 8;

   public final static int zF = 0;
   public final static int nominalLength = 1;
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedBodyTrajectoryMessage.java<|end_filename|>
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 commands the controller to move in taskspace the body to the desired pose (position & orientation) while going through the specified trajectory points.
       * A third order polynomial function is used to interpolate positions and a Hermite based curve (third order) is used to interpolate the orientations.
       * To execute a single straight line trajectory to reach a desired body pose, set only one trajectory point with zero velocity and its time to be equal to the desired trajectory time.
       * Note that the body position is limited keep the robot's balance (center of mass has to remain inside the support polygon).
       */
public class QuadrupedBodyTrajectoryMessage extends Packet<QuadrupedBodyTrajectoryMessage> implements Settable<QuadrupedBodyTrajectoryMessage>, EpsilonComparable<QuadrupedBodyTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean is_expressed_in_absolute_time_ = true;
   /**
            * The position/orientation trajectory information.
            */
   public controller_msgs.msg.dds.SE3TrajectoryMessage se3_trajectory_;

   public QuadrupedBodyTrajectoryMessage()
   {
      se3_trajectory_ = new controller_msgs.msg.dds.SE3TrajectoryMessage();
   }

   public QuadrupedBodyTrajectoryMessage(QuadrupedBodyTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedBodyTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      is_expressed_in_absolute_time_ = other.is_expressed_in_absolute_time_;

      controller_msgs.msg.dds.SE3TrajectoryMessagePubSubType.staticCopy(other.se3_trajectory_, se3_trajectory_);
   }

   /**
            * 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_;
   }

   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public void setIsExpressedInAbsoluteTime(boolean is_expressed_in_absolute_time)
   {
      is_expressed_in_absolute_time_ = is_expressed_in_absolute_time;
   }
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean getIsExpressedInAbsoluteTime()
   {
      return is_expressed_in_absolute_time_;
   }


   /**
            * The position/orientation trajectory information.
            */
   public controller_msgs.msg.dds.SE3TrajectoryMessage getSe3Trajectory()
   {
      return se3_trajectory_;
   }


   public static Supplier<QuadrupedBodyTrajectoryMessagePubSubType> getPubSubType()
   {
      return QuadrupedBodyTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedBodyTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedBodyTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_expressed_in_absolute_time_, other.is_expressed_in_absolute_time_, epsilon)) return false;

      if (!this.se3_trajectory_.epsilonEquals(other.se3_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedBodyTrajectoryMessage)) return false;

      QuadrupedBodyTrajectoryMessage otherMyClass = (QuadrupedBodyTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.is_expressed_in_absolute_time_ != otherMyClass.is_expressed_in_absolute_time_) return false;

      if (!this.se3_trajectory_.equals(otherMyClass.se3_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedBodyTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("is_expressed_in_absolute_time=");
      builder.append(this.is_expressed_in_absolute_time_);      builder.append(", ");
      builder.append("se3_trajectory=");
      builder.append(this.se3_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/FrameGeometry2dPlotter.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Polygon;
import java.awt.event.MouseEvent;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.LinkedHashMap;

import javax.swing.JPanel;
import javax.swing.event.MouseInputListener;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
import us.ihmc.euclid.referenceFrame.FrameLine2D;
import us.ihmc.euclid.referenceFrame.FrameLineSegment2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;

public class FrameGeometry2dPlotter extends JPanel implements MouseInputListener
{
   private static final long serialVersionUID = 2505175418803166784L;

   private ArrayList<FramePoint2D> testPoints = new ArrayList<FramePoint2D>();

   private FrameConvexPolygon2D polygonToCheckInside = null;

   private LinkedHashMap<Color, FrameLineGroup> frameLineGroups = new LinkedHashMap<Color, FrameLineGroup>();
   private LinkedHashMap<Color, FramePointGroup> framePointGroups = new LinkedHashMap<Color, FramePointGroup>();
   private LinkedHashMap<Color, FrameConvexPolygonGroup> frameConvexPolygonGroups = new LinkedHashMap<Color, FrameConvexPolygonGroup>();
   private LinkedHashMap<Color, FrameLineSegmentGroup> frameLineSegmentGroups = new LinkedHashMap<Color, FrameLineSegmentGroup>();

   private double xCenter, yCenter, scale;
   private int pointPixels = 2;

   private class FrameLineGroup
   {
      private ArrayList<FrameLine2D> frameLines = new ArrayList<FrameLine2D>();
      private final Color color;

      public FrameLineGroup(Color color)
      {
         this.color = color;
      }

      public void addFrameLine(FrameLine2D frameLine2d)
      {
         frameLines.add(frameLine2d);
      }

      public void addFrameLines(ArrayList<FrameLine2D> frameLines2d)
      {
         frameLines.addAll(frameLines2d);
      }

      public void addFrameLines(FrameLine2D[] frameLines2d)
      {
         for (FrameLine2D frameLine : frameLines2d)
         {
            frameLines.add(frameLine);
         }
      }
   }

   private class FramePointGroup
   {
      private ArrayList<FramePoint2DReadOnly> framePoints = new ArrayList<>();
      private final Color color;

      public FramePointGroup(Color color)
      {
         this.color = color;
      }

      public void addFramePoint(FramePoint2DReadOnly framePoint2d)
      {
         framePoints.add(framePoint2d);
      }

      public void addFramePoints(ArrayList<? extends FramePoint2DReadOnly> framePoints2d)
      {
         framePoints.addAll(framePoints2d);
      }

      @SuppressWarnings("unused")
      public void addFramePoints(FramePoint2DReadOnly[] framePoints2d)
      {
         for (FramePoint2DReadOnly framePoint : framePoints2d)
         {
            framePoints.add(framePoint);
         }
      }

   }

   private class FrameConvexPolygonGroup
   {
      private ArrayList<FrameConvexPolygon2D> polygons = new ArrayList<FrameConvexPolygon2D>();
      private final Color color;

      public FrameConvexPolygonGroup(Color color)
      {
         this.color = color;
      }

      public void addFrameConvexPolygon2d(FrameConvexPolygon2D frameConvexPolygon2d)
      {
         polygons.add(frameConvexPolygon2d);
      }

      public void addFrameConvexPolygon2ds(ArrayList<FrameConvexPolygon2D> frameConvexPolygon2ds)
      {
         polygons.addAll(frameConvexPolygon2ds);
      }

      @SuppressWarnings("unused")
      public void addFrameConvexPolygon2ds(FrameConvexPolygon2D[] frameConvexPolygon2ds)
      {
         for (FrameConvexPolygon2D frameConvexPolygon2d : frameConvexPolygon2ds)
         {
            polygons.add(frameConvexPolygon2d);
         }
      }
   }

   private class FrameLineSegmentGroup
   {
      private ArrayList<FrameLineSegment2D> frameLineSegments = new ArrayList<FrameLineSegment2D>();
      private final Color color;

      public FrameLineSegmentGroup(Color color)
      {
         this.color = color;
      }

      public void addFrameLineSegment(FrameLineSegment2D frameLineSegment2d)
      {
         frameLineSegments.add(frameLineSegment2d);
      }

      public void addFrameLineSegments(ArrayList<FrameLineSegment2D> frameLineSegments2d)
      {
         frameLineSegments.addAll(frameLineSegments2d);
      }
   }

   public FrameGeometry2dPlotter(double xCenter, double yCenter, double scale)
   {
      this.scale = scale;
      this.xCenter = xCenter;
      this.yCenter = yCenter;

      this.addMouseListener(this);
      this.addMouseMotionListener(this);
   }

   public void setDrawPointsLarge()
   {
      pointPixels = 8;
   }

   public void setDrawPointsMedium()
   {
      pointPixels = 4;
   }

   public void setPointPixels(int pointPixels)
   {
      this.pointPixels = pointPixels;
   }

   public synchronized void addFrameLine2d(FrameLine2D frameLine2d)
   {
      addFrameLine2d(frameLine2d, Color.black);
   }

   public synchronized void addFrameLines2d(ArrayList<FrameLine2D> frameLines2d)
   {
      addFrameLines2d(frameLines2d, Color.black);
   }

   public synchronized void addFrameLine2d(FrameLine2D frameLine2d, Color color)
   {
      FrameLineGroup frameLineGroup = frameLineGroups.get(color);
      if (frameLineGroup == null)
      {
         frameLineGroup = new FrameLineGroup(color);
         frameLineGroups.put(color, frameLineGroup);
      }

      frameLineGroup.addFrameLine(frameLine2d);
   }

   public synchronized void addFramePoints2d(ArrayList<FramePoint2D> framePoints, Color color)
   {
      FramePointGroup framePointGroup = framePointGroups.get(color);
      if (framePointGroup == null)
      {
         framePointGroup = new FramePointGroup(color);
         framePointGroups.put(color, framePointGroup);
      }

      framePointGroup.addFramePoints(framePoints);
   }

   public synchronized void addFramePoint2d(FramePoint2DReadOnly framePoint, Color color)
   {
      FramePointGroup framePointGroup = framePointGroups.get(color);
      if (framePointGroup == null)
      {
         framePointGroup = new FramePointGroup(color);
         framePointGroups.put(color, framePointGroup);
      }

      framePointGroup.addFramePoint(framePoint);
   }

   public synchronized void addFrameLines2d(ArrayList<FrameLine2D> frameLines2d, Color color)
   {
      FrameLineGroup frameLineGroup = frameLineGroups.get(color);
      if (frameLineGroup == null)
      {
         frameLineGroup = new FrameLineGroup(color);
         frameLineGroups.put(color, frameLineGroup);
      }

      frameLineGroup.addFrameLines(frameLines2d);

   }

   public synchronized void addFrameLines2d(FrameLine2D[] frameLines2d, Color color)
   {
      FrameLineGroup frameLineGroup = frameLineGroups.get(color);
      if (frameLineGroup == null)
      {
         frameLineGroup = new FrameLineGroup(color);
         frameLineGroups.put(color, frameLineGroup);
      }

      frameLineGroup.addFrameLines(frameLines2d);
   }

   public synchronized void addFrameLineSegment2d(FrameLineSegment2D frameLinesSegment2d, Color color)
   {
      FrameLineSegmentGroup frameLineSegmentGroup = frameLineSegmentGroups.get(color);

      if (frameLineSegmentGroup == null)
      {
         frameLineSegmentGroup = new FrameLineSegmentGroup(color);
         frameLineSegmentGroups.put(color, frameLineSegmentGroup);
      }

      frameLineSegmentGroup.addFrameLineSegment(frameLinesSegment2d);

   }

   public synchronized void addFrameLineSegments2d(ArrayList<FrameLineSegment2D> frameLinesSegments2d, Color color)
   {
      FrameLineSegmentGroup frameLineSegmentGroup = frameLineSegmentGroups.get(color);

      if (frameLineSegmentGroup == null)
      {
         frameLineSegmentGroup = new FrameLineSegmentGroup(color);
         frameLineSegmentGroups.put(color, frameLineSegmentGroup);
      }

      frameLineSegmentGroup.addFrameLineSegments(frameLinesSegments2d);
   }

   public synchronized void addPolygon(FrameConvexPolygon2D polygon)
   {
      addPolygon(polygon, Color.BLACK);
   }

   public synchronized void addPolygon(FrameConvexPolygon2D polygon, Color color)
   {
      FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
      if (frameConvexPolygonGroup == null)
      {
         frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
         frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
      }

      frameConvexPolygonGroup.addFrameConvexPolygon2d(polygon);
   }

   public synchronized void addFrameConvexPolygons(ArrayList<FrameConvexPolygon2D> frameConvexPolygons, Color color)
   {
      FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
      if (frameConvexPolygonGroup == null)
      {
         frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
         frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
      }

      frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
   }

   public synchronized void addConvexPolygons(ArrayList<ConvexPolygon2D> convexPolygons, Color color)
   {
      FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
      if (frameConvexPolygonGroup == null)
      {
         frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
         frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
      }

      ArrayList<FrameConvexPolygon2D> frameConvexPolygons = new ArrayList<FrameConvexPolygon2D>();
      for (ConvexPolygon2D polygon : convexPolygons)
      {
         frameConvexPolygons.add(new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), polygon));
      }

      frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
   }

   public synchronized void addConvexPolygons(ConvexPolygon2D[] convexPolygons, Color color)
   {
      FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
      if (frameConvexPolygonGroup == null)
      {
         frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
         frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
      }

      ArrayList<FrameConvexPolygon2D> frameConvexPolygons = new ArrayList<FrameConvexPolygon2D>();
      for (ConvexPolygon2D polygon : convexPolygons)
      {
         if (polygon == null)
            continue;
         frameConvexPolygons.add(new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), polygon));
      }

      frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
   }

   public synchronized void addConvexPolygon(ConvexPolygon2D convexPolygon, Color color)
   {
      FrameConvexPolygonGroup frameConvexPolygonGroup = frameConvexPolygonGroups.get(color);
      if (frameConvexPolygonGroup == null)
      {
         frameConvexPolygonGroup = new FrameConvexPolygonGroup(color);
         frameConvexPolygonGroups.put(color, frameConvexPolygonGroup);
      }

      ArrayList<FrameConvexPolygon2D> frameConvexPolygons = new ArrayList<FrameConvexPolygon2D>();

      if (convexPolygon == null)
         return;
      frameConvexPolygons.add(new FrameConvexPolygon2D(ReferenceFrame.getWorldFrame(), convexPolygon));

      frameConvexPolygonGroup.addFrameConvexPolygon2ds(frameConvexPolygons);
   }

   public synchronized void addFramePoint2d(FramePoint2DReadOnly framePoint2d)
   {
      this.addFramePoint2d(framePoint2d, Color.black);
   }

   public synchronized void addTestPoint(FramePoint2D testPoint)
   {
      testPoints.add(testPoint);
   }

   public synchronized void setPolygonToCheckInside(FrameConvexPolygon2D polygon)
   {
      this.polygonToCheckInside = polygon;
   }

   public synchronized void addTestPoints(ArrayList<FramePoint2D> testPoints)
   {
      this.testPoints.addAll(testPoints);
      repaint();
   }

   public synchronized void removeAllObjectsToDraw()
   {
      testPoints.clear();

      polygonToCheckInside = null;

      frameLineGroups.clear();
      framePointGroups.clear();
      frameConvexPolygonGroups.clear();
      frameLineSegmentGroups.clear();

      repaint();
   }

   public synchronized void paintComponent(Graphics graphics)
   {
      super.paintComponent(graphics);

      if (polygonToCheckInside != null)
         drawPolygon(polygonToCheckInside, graphics);

      drawFramePolygonGroups(frameConvexPolygonGroups, graphics);
      drawFrameLineGroups(frameLineGroups, graphics);
      drawFrameLineSegmentGroups(frameLineSegmentGroups, graphics);

      drawTestPoints(testPoints, polygonToCheckInside, graphics);

      //    drawFramePoint2ds(framePoint2ds, graphics);
      drawFramePointGroups(framePointGroups, graphics);
   }

   private void drawFramePolygonGroups(HashMap<Color, FrameConvexPolygonGroup> frameConvexPolygonGroups, Graphics graphics)
   {
      Collection<FrameConvexPolygonGroup> groups = frameConvexPolygonGroups.values();

      for (FrameConvexPolygonGroup frameConvexPolygonGroup : groups)
      {
         graphics.setColor(frameConvexPolygonGroup.color);

         //       drawFrameConvexPolygon(frameConvexPolygonGroup.polygons, graphics);
         drawPolygons(frameConvexPolygonGroup.polygons, graphics);
      }

   }

   private void drawFrameLineGroups(HashMap<Color, FrameLineGroup> frameLineGroups, Graphics graphics)
   {
      Collection<FrameLineGroup> groups = frameLineGroups.values();

      for (FrameLineGroup frameLineGroup : groups)
      {
         graphics.setColor(frameLineGroup.color);
         drawFrameLines(frameLineGroup.frameLines, graphics);
      }
   }

   private void drawFrameLineSegmentGroups(HashMap<Color, FrameLineSegmentGroup> frameLineSegmentGroups, Graphics graphics)
   {
      Collection<FrameLineSegmentGroup> groups = frameLineSegmentGroups.values();

      for (FrameLineSegmentGroup frameLineSegmentGroup : groups)
      {
         graphics.setColor(frameLineSegmentGroup.color);
         drawFrameLineSegments(frameLineSegmentGroup.frameLineSegments, graphics);
      }
   }

   private void drawFramePointGroups(HashMap<Color, FramePointGroup> framePointGroups, Graphics graphics)
   {
      Collection<FramePointGroup> groups = framePointGroups.values();

      for (FramePointGroup framePointGroup : groups)
      {
         graphics.setColor(framePointGroup.color);
         drawFramePoints(framePointGroup.framePoints, graphics);
      }
   }

   private void drawFrameLines(ArrayList<FrameLine2D> frameLines, Graphics graphics)
   {
      for (FrameLine2D frameLine : frameLines)
      {
         this.drawLine(frameLine, graphics);
      }
   }

   private void drawFrameLineSegments(ArrayList<FrameLineSegment2D> frameLineSegments, Graphics graphics)
   {
      for (FrameLineSegment2D frameLineSegment : frameLineSegments)
      {
         this.drawLineSegment(frameLineSegment, graphics);
      }
   }

   private void drawFramePoints(ArrayList<? extends FramePoint2DReadOnly> framePoints, Graphics graphics)
   {
      for (FramePoint2DReadOnly framePoint : framePoints)
      {
         this.drawPoint(framePoint, graphics);
      }
   }

   private void drawTestPoints(ArrayList<FramePoint2D> testPoints, FrameConvexPolygon2D polygon, Graphics graphics)
   {
      for (FramePoint2D testPoint : testPoints)
      {
         boolean isInside = polygon.isPointInside(testPoint);

         if (isInside)
         {
            drawInsidePoint(testPoint, graphics);
         }
         else
         {
            drawOutsidePoint(testPoint, graphics);
         }
      }
   }

   @SuppressWarnings("unused")
   private void drawPoint(FramePoint2D testPoint, Color color, Graphics graphics)
   {
      int xInt = doubleToInt(testPoint.getX(), xCenter, scale, getWidth());
      int yInt = doubleToInt(testPoint.getY(), yCenter, -scale, getHeight());

      graphics.setColor(color);
      graphics.fillOval(xInt - 2, yInt - 2, 4, 4);
   }

   private void drawInsidePoint(FramePoint2D testPoint, Graphics graphics)
   {
      int xInt = doubleToInt(testPoint.getX(), xCenter, scale, getWidth());
      int yInt = doubleToInt(testPoint.getY(), yCenter, -scale, getHeight());

      graphics.setColor(Color.green);
      graphics.fillOval(xInt - 2, yInt - 2, 4, 4);
   }

   private void drawOutsidePoint(FramePoint2D testPoint, Graphics graphics)
   {
      int xInt = doubleToInt(testPoint.getX(), xCenter, scale, getWidth());
      int yInt = doubleToInt(testPoint.getY(), yCenter, -scale, getHeight());

      graphics.setColor(Color.red);
      graphics.fillOval(xInt - 2, yInt - 2, 4, 4);
   }

   private void drawPolygons(ArrayList<FrameConvexPolygon2D> polygons, Graphics graphics)
   {
      for (FrameConvexPolygon2D polygon : polygons)
      {
         drawPolygon(polygon, graphics);
      }
   }

   private void drawPolygon(FrameConvexPolygon2D frameConvexPolygon2d, Graphics graphics)
   {
      Polygon polygon = new Polygon();

      for (int i = 0; i < frameConvexPolygon2d.getNumberOfVertices(); i++)
      {
         Point2DReadOnly vertex = frameConvexPolygon2d.getVertex(i);
         int xInt = doubleToInt(vertex.getX(), xCenter, scale, getWidth());
         int yInt = doubleToInt(vertex.getY(), yCenter, -scale, getHeight());

         polygon.addPoint(xInt, yInt);
      }

      graphics.drawPolygon(polygon);
   }

   private void drawLine(FrameLine2D frameLine2d, Graphics graphics)
   {
      Point2D point = new Point2D(frameLine2d.getPoint());
      Vector2D vector = new Vector2D(frameLine2d.getDirection());

      double largeNumber = scale * 100; // 1.0e3;

      double X2 = point.getX() + largeNumber * vector.getX();
      double Y2 = point.getY() + largeNumber * vector.getY();

      drawLine(point.getX(), point.getY(), X2, Y2, graphics);
   }

   private void drawLineSegment(FrameLineSegment2D frameLineSegment2d, Graphics graphics)
   {
      Point2DBasics firstEndpoint = frameLineSegment2d.getFirstEndpoint();
      Point2DBasics secondEndpoint = frameLineSegment2d.getSecondEndpoint();

      drawLine(firstEndpoint.getX(), firstEndpoint.getY(), secondEndpoint.getX(), secondEndpoint.getY(), graphics);
   }

   private void drawPoint(FramePoint2DReadOnly framePoint2d, Graphics graphics)
   {
      drawPoint(framePoint2d.getX(), framePoint2d.getY(), graphics);
   }

   private void drawLine(double X1, double Y1, double X2, double Y2, Graphics graphics)
   {
      int x1 = doubleToInt(X1, xCenter, scale, getWidth());
      int x2 = doubleToInt(X2, xCenter, scale, getWidth());
      int y1 = doubleToInt(Y1, yCenter, -scale, getHeight());
      int y2 = doubleToInt(Y2, yCenter, -scale, getHeight());

      graphics.drawLine(x1, y1, x2, y2);
   }

   private void drawPoint(double X1, double Y1, Graphics graphics)
   {
      int x1 = doubleToInt(X1, xCenter, scale, getWidth());
      int y1 = doubleToInt(Y1, yCenter, -scale, getHeight());

      graphics.fillOval(x1 - pointPixels / 2, y1 - pointPixels / 2, pointPixels, pointPixels);
   }

   private int doubleToInt(double value, double center, double scale, int extent)
   {
      return (int) ((value - center) * scale + extent / 2);
   }

   private double intToDouble(int value, double center, double scale, int extent)
   {
      return (value - extent / 2) / scale + center;
   }

   private double lastMousePressX, lastMousePressY;

   public void mouseClicked(MouseEvent e)
   {
      int clickCount = e.getClickCount();

      if (clickCount > 1)
      {
         this.xCenter = lastMousePressX;
         this.yCenter = lastMousePressY;

         this.repaint();
      }

   }

   private int buttonPressed;
   private int dragStartX;
   private int dragStartY;

   public void mousePressed(MouseEvent e)
   {
      buttonPressed = e.getButton();

      int xInt = e.getX();
      int yInt = e.getY();

      double x = intToDouble(xInt, xCenter, scale, getWidth());
      double y = intToDouble(yInt, yCenter, -scale, getHeight());

      lastMousePressX = x;
      lastMousePressY = y;

      if (buttonPressed == MouseEvent.BUTTON2)
      {
         dragStartY = e.getY();
      }
   }

   public void mouseReleased(MouseEvent e)
   {
   }

   public void mouseEntered(MouseEvent e)
   {
   }

   public void mouseExited(MouseEvent e)
   {
   }

   public void mouseDragged(MouseEvent e)
   {
      if (buttonPressed == MouseEvent.BUTTON2)
      {
         int yDifferenceFromStartOfDrag = -(e.getY() - dragStartY);
         double scalePercent = 1.0 + (((double) yDifferenceFromStartOfDrag) / 200.0);
         if (scalePercent < 0.1)
            scalePercent = 0.1;

         double newScale = scale * scalePercent;
         if (newScale < 0.1)
            newScale = 0.1;

         scale = newScale;
         dragStartY = e.getY();
         repaint();
      }
   }

   public void mouseMoved(MouseEvent e)
   {
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TextToSpeechPacket.java<|end_filename|>
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;

/**
       * Use this message for the robot to speak.
       */
public class TextToSpeechPacket extends Packet<TextToSpeechPacket> implements Settable<TextToSpeechPacket>, EpsilonComparable<TextToSpeechPacket>
{
   public static final String FOOTSTEP_COMPLETED = "Finished Taking Footstep";

   public static final String FINISHED_WALKING = "Finished walking";

   public static final String WALKING_ABORTED = "walking aborted";

   public static final String STARTING_CONTROLLER = "starting controller";

   public static final String WALKING = "walking";

   public static final String MOVING_LEFT_ARM = "moving the left arm";

   public static final String MOVING_RIGHT_ARM = "moving the right arm";

   public static final String NETWORKPROCESSOR_ONLINE = "Reestablished Connection To The Network Processor";

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean speak_packet_;
   public boolean beep_ = true;
   public java.lang.StringBuilder text_to_speak_;

   public TextToSpeechPacket()
   {
      text_to_speak_ = new java.lang.StringBuilder(255);
   }

   public TextToSpeechPacket(TextToSpeechPacket other)
   {
      this();
      set(other);
   }

   public void set(TextToSpeechPacket other)
   {
      sequence_id_ = other.sequence_id_;

      speak_packet_ = other.speak_packet_;

      beep_ = other.beep_;

      text_to_speak_.setLength(0);
      text_to_speak_.append(other.text_to_speak_);

   }

   /**
            * 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 void setSpeakPacket(boolean speak_packet)
   {
      speak_packet_ = speak_packet;
   }
   public boolean getSpeakPacket()
   {
      return speak_packet_;
   }

   public void setBeep(boolean beep)
   {
      beep_ = beep;
   }
   public boolean getBeep()
   {
      return beep_;
   }

   public void setTextToSpeak(java.lang.String text_to_speak)
   {
      text_to_speak_.setLength(0);
      text_to_speak_.append(text_to_speak);
   }

   public java.lang.String getTextToSpeakAsString()
   {
      return getTextToSpeak().toString();
   }
   public java.lang.StringBuilder getTextToSpeak()
   {
      return text_to_speak_;
   }


   public static Supplier<TextToSpeechPacketPubSubType> getPubSubType()
   {
      return TextToSpeechPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return TextToSpeechPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(TextToSpeechPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.speak_packet_, other.speak_packet_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.beep_, other.beep_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.text_to_speak_, other.text_to_speak_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof TextToSpeechPacket)) return false;

      TextToSpeechPacket otherMyClass = (TextToSpeechPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.speak_packet_ != otherMyClass.speak_packet_) return false;

      if(this.beep_ != otherMyClass.beep_) return false;

      if (!us.ihmc.idl.IDLTools.equals(this.text_to_speak_, otherMyClass.text_to_speak_)) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("TextToSpeechPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("speak_packet=");
      builder.append(this.speak_packet_);      builder.append(", ");
      builder.append("beep=");
      builder.append(this.beep_);      builder.append(", ");
      builder.append("text_to_speak=");
      builder.append(this.text_to_speak_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/sliderboardVisualizer/VisualizerIcon.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices.sliderboardVisualizer;

import javax.swing.ImageIcon;
import javax.swing.JLabel;

public class VisualizerIcon
{
   private final JLabel image;
   private final JLabel label;
   private final JLabel max;
   private final JLabel min;
   private final JLabel currentValue;

   public VisualizerIcon(ImageIcon imageIcon)
   {
      image = new JLabel(imageIcon);
      label = new JLabel("");
      max = new JLabel("");
      min = new JLabel("");
      currentValue = new JLabel("");
   }

   public void clear()
   {
      label.setText("");
      max.setText("");
      min.setText("");
      currentValue.setText("");
   }

   public void setImageSize(int x, int y)
   {
      image.setSize(x, y);
   }

   public void setImageLocation(int x, int y)
   {
      image.setLocation(x, y);
   }

   public void setLabelBounds(int x, int y, int width, int height)
   {
      label.setBounds(x, y, width, height);
   }

   public void setLabelText(String text)
   {
      label.setText(text);
   }

   public void setMaxBounds(int x, int y, int width, int height)
   {
      max.setBounds(x, y, width, height);
   }

   public void setMinBounds(int x, int y, int width, int height)
   {
      min.setBounds(x, y, width, height);
   }

   public void setMaxMinValues(double max, double min)
   {
      this.max.setText("" + max);
      this.min.setText("" + min);
   }

   public void clearMaxMinValues()
   {
      max.setText("");
      min.setText("");
   }

   public void setCurrentValueBounds(int x, int y, int width, int height)
   {
      currentValue.setBounds(x, y, width, height);
   }

   public void clearCurrentValue()
   {
      currentValue.setText("");
   }

   public void updateCurrentValue(double currentValue)
   {
      this.currentValue.setText("" + currentValue);
   }

   public void updateCurrentValue(String currentValue)
   {
      this.currentValue.setText(currentValue);
   }

   public JLabel getImage()
   {
      return image;
   }

   public JLabel getLabel()
   {
      return label;
   }

   public JLabel getMax()
   {
      return max;
   }

   public JLabel getMin()
   {
      return min;
   }


   public JLabel getCurrentValue()
   {
      return currentValue;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/VehiclePosePacket.java<|end_filename|>
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;

/**
       * Old message used for the VRC? Needs to be revised, maybe deleted?
       */
public class VehiclePosePacket extends Packet<VehiclePosePacket> implements Settable<VehiclePosePacket>, EpsilonComparable<VehiclePosePacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.tuple3D.Point3D position_;
   public us.ihmc.euclid.tuple4D.Quaternion orientation_;
   public int index_;

   public VehiclePosePacket()
   {
      position_ = new us.ihmc.euclid.tuple3D.Point3D();
      orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
   }

   public VehiclePosePacket(VehiclePosePacket other)
   {
      this();
      set(other);
   }

   public void set(VehiclePosePacket other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.position_, position_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.orientation_, orientation_);
      index_ = other.index_;

   }

   /**
            * 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 us.ihmc.euclid.tuple3D.Point3D getPosition()
   {
      return position_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getOrientation()
   {
      return orientation_;
   }

   public void setIndex(int index)
   {
      index_ = index;
   }
   public int getIndex()
   {
      return index_;
   }


   public static Supplier<VehiclePosePacketPubSubType> getPubSubType()
   {
      return VehiclePosePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return VehiclePosePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(VehiclePosePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.position_.epsilonEquals(other.position_, epsilon)) return false;
      if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false;
      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.index_, other.index_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof VehiclePosePacket)) return false;

      VehiclePosePacket otherMyClass = (VehiclePosePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.position_.equals(otherMyClass.position_)) return false;
      if (!this.orientation_.equals(otherMyClass.orientation_)) return false;
      if(this.index_ != otherMyClass.index_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("VehiclePosePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("position=");
      builder.append(this.position_);      builder.append(", ");
      builder.append("orientation=");
      builder.append(this.orientation_);      builder.append(", ");
      builder.append("index=");
      builder.append(this.index_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/TimeInCurrentStateProvider.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

@Deprecated
public interface TimeInCurrentStateProvider
{
   public abstract double timeInCurrentState();
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/polaris/DRCWorld.java<|end_filename|>
package us.ihmc.avatar.polaris;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.modelFileLoaders.SdfLoader.DRCRobotSDFLoader;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationConstructionSetTools.util.environments.SelectableObjectListener;

public abstract class DRCWorld 
{
   private static final Class<DRCRobotSDFLoader> myClass = DRCRobotSDFLoader.class;
   protected final ArrayList<String> resourceDirectories = new ArrayList<String>();

   public DRCWorld()
   {

      // DRC resources
      resourceDirectories.add("models/GFE/gazebo");
      resourceDirectories.add("models/GFE/");
      resourceDirectories.add("models");
      resourceDirectories.add("models/polaris_ranger_xp900_no_roll_cage/");
      resourceDirectories.add("models/polaris_ranger_xp900_no_roll_cage/meshes/");
      resourceDirectories.add("models/polaris_ranger_xp900_no_roll_cage/materials/");
      resourceDirectories.add("models/polaris_ranger_xp900_no_roll_cage/scripts/");
      resourceDirectories.add("models/polaris_ranger_xp900/");
      resourceDirectories.add("models/polaris_ranger_xp900/meshes/");
      resourceDirectories.add("models/polaris_ranger_xp900/materials/");
      resourceDirectories.add("models/polaris_ranger_xp900/materials/scripts/");
      resourceDirectories.add("models/polaris_ranger_ev/");
      resourceDirectories.add("models/polaris_ranger_ev/meshes/");
      resourceDirectories.add("models/polaris_ranger_ev/materials/");
      resourceDirectories.add("models/polaris_ranger_ev/materials/scripts/");

      // Gazebo resources
      resourceDirectories.add("models/GFE/models");
      resourceDirectories.add("models/GFE/drcsim");
      
      resourceDirectories.add("models/GFE/gazebo_models/environments/");

      resourceDirectories.add("models/GFE/gazebo_models/environments/golf_cart/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_heightmap_3/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate_number_1/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/drc_terrain/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_heightmap_2/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_standpipe/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_firehose_long/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate_number_3/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_valve/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/platform_2/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/cutout_wall/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate_number_4/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate_number_5/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vehicle_gate/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/mud_atlas/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/platform_4/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_heightmap_1/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate_number_2/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/platform_1/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_heightmap_4/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/platform_3/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/powerplant/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_valve_a/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_firehose_long_a/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/grass_plane/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/stepping_block/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_standpipe_a/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/environments/pavement_plane/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/atlas_description/atlas/materials/textures");
      resourceDirectories.add("models/GFE/gazebo_models/multisense_sl_description/multisense_sl_description/materials/textures");
      resourceDirectories.add("models/models/asphalt_plane/materials/textures");
      resourceDirectories.add("models/models/beer/materials/textures");
      resourceDirectories.add("models/models/drc_practice_valve_wall/materials/textures");
      resourceDirectories.add("models/models/house_3/materials/textures");
      resourceDirectories.add("models/models/gas_station/materials/textures");
      resourceDirectories.add("models/models/cordless_drill/materials/textures");
      resourceDirectories.add("models/models/jersey_barrier/materials/textures");
      resourceDirectories.add("models/models/mailbox/materials/textures");
      resourceDirectories.add("models/models/nist_maze_wall_triple_holes_120/materials/textures");
      resourceDirectories.add("models/models/turtlebot/materials/textures");
      resourceDirectories.add("models/models/drc_practice_handle_wheel_valve/materials/textures");
      resourceDirectories.add("models/models/drc_practice_wood_slats/materials/textures");
      resourceDirectories.add("models/models/house_2/materials/textures");
      resourceDirectories.add("models/models/lamp_post/materials/textures");
      resourceDirectories.add("models/models/polaris_ranger_ev/materials/textures");
      resourceDirectories.add("models/models/nist_elevated_floor_120/materials/textures");
      resourceDirectories.add("models/models/coke_can/materials/textures");
      resourceDirectories.add("models/models/drc_practice_2x6/materials/textures");
      resourceDirectories.add("models/models/cinder_block/materials/textures");
      resourceDirectories.add("models/models/kinect/materials/textures");
      resourceDirectories.add("models/models/drc_practice_4x4x40/materials/textures");
      resourceDirectories.add("models/models/stop_sign/materials/textures");
      resourceDirectories.add("models/models/mud_box/materials/textures");
      resourceDirectories.add("models/models/pioneer2dx/materials/textures");
      resourceDirectories.add("models/models/construction_barrel/materials/textures");
      resourceDirectories.add("models/models/cinder_block_2/materials/textures");
      resourceDirectories.add("models/models/create/materials/textures");
      resourceDirectories.add("models/models/starting_pen/materials/textures");
      resourceDirectories.add("models/models/hammer/materials/textures");
      resourceDirectories.add("models/models/nist_simple_ramp_120/materials/textures");
      resourceDirectories.add("models/models/speed_limit_sign/materials/textures");
      resourceDirectories.add("models/models/house_1/materials/textures");
      resourceDirectories.add("models/models/dumpster/materials/textures");
      resourceDirectories.add("models/models/kitchen_dining/materials/textures");
      resourceDirectories.add("models/models/brick_box_3x1x3/materials/textures");
      resourceDirectories.add("models/models/grey_wall/materials/textures");
      resourceDirectories.add("models/models/nist_maze_wall_240/materials/textures");
      resourceDirectories.add("models/models/powerplant/materials/textures");
      resourceDirectories.add("models/models/drc_practice_truss/materials/textures");
      resourceDirectories.add("models/models/willowgarage/materials/textures");
      resourceDirectories.add("models/models/construction_cone/materials/textures");
      resourceDirectories.add("models/models/drc_practice_4x4x20/materials/textures");
      resourceDirectories.add("models/models/drc_practice_wheel_valve_large/materials/textures");
      resourceDirectories.add("models/models/drc_practice_2x4/materials/textures");
      resourceDirectories.add("models/models/fast_food/materials/textures");
      resourceDirectories.add("models/models/drc_practice_ball_valve/materials/textures");
      resourceDirectories.add("models/models/nist_stairs_120/materials/textures");
      resourceDirectories.add("models/models/nist_maze_wall_120/materials/textures");
      resourceDirectories.add("models/GFE/atlas_description/materials/textures");
      resourceDirectories.add("models/drcsim/media/materials/textures");
      resourceDirectories.add("models/GFE/multisense_sl_description/materials/textures");
      resourceDirectories.add("models/gazebo/media/materials/textures");

      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_firehose_long/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/gate/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_valve/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/cutout_wall/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/mud_atlas/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_valve_a/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/vrc_firehose_long_a/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/grass_plane/materials/scripts");
      resourceDirectories.add("models/GFE/gazebo_models/environments/pavement_plane/materials/scripts");
      resourceDirectories.add("models/models/asphalt_plane/materials/scripts");
      resourceDirectories.add("models/models/beer/materials/scripts");
      resourceDirectories.add("models/models/youbot/materials/scripts");
      resourceDirectories.add("models/models/house_3/materials/scripts");
      resourceDirectories.add("models/models/gas_station/materials/scripts");
      resourceDirectories.add("models/models/mailbox/materials/scripts");
      resourceDirectories.add("models/models/house_2/materials/scripts");
      resourceDirectories.add("models/models/polaris_ranger_ev/materials/scripts");
      resourceDirectories.add("models/models/stop_sign/materials/scripts");
      resourceDirectories.add("models/models/mud_box/materials/scripts");
      resourceDirectories.add("models/models/starting_pen/materials/scripts");
      resourceDirectories.add("models/models/house_1/materials/scripts");
      resourceDirectories.add("models/models/dumpster/materials/scripts");
      resourceDirectories.add("models/models/brick_box_3x1x3/materials/scripts");
      resourceDirectories.add("models/models/grey_wall/materials/scripts");
      resourceDirectories.add("models/models/fast_food/materials/scripts");
      resourceDirectories.add("models/drcsim/media/materials/scripts");
      resourceDirectories.add("models/gazebo/media/materials/scripts");
   }

   public List<Robot> getEnvironmentRobots()
   {
      return new ArrayList<Robot>();
   }

   public void createAndSetContactControllerToARobot()
   {
   }

   public void addContactPoints(ExternalForcePoint[] externalForcePoints)
   {
   }

   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/wholeBodyTrajectoryToolboxModule/WholeBodyTrajectoryToolboxCommandConverter.java<|end_filename|>
package us.ihmc.avatar.networkProcessor.wholeBodyTrajectoryToolboxModule;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import controller_msgs.msg.dds.KinematicsToolboxRigidBodyMessage;
import us.ihmc.communication.controllerAPI.CommandConversionInterface;
import us.ihmc.communication.controllerAPI.command.Command;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.humanoidRobotics.communication.kinematicsToolboxAPI.KinematicsToolboxRigidBodyCommand;
import us.ihmc.humanoidRobotics.communication.wholeBodyTrajectoryToolboxAPI.WholeBodyTrajectoryToolboxAPI;
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.tools.MultiBodySystemTools;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotics.screwTheory.ScrewTools;
import us.ihmc.sensorProcessing.frames.ReferenceFrameHashCodeResolver;

/**
 * This class allows the retrieve the rigid-body from its hash code when converting a
 * {@link KinematicsToolboxRigidBodyMessage} into a {@link KinematicsToolboxRigidBodyCommand}.
 * 
 * @author <NAME>
 *
 */
public class WholeBodyTrajectoryToolboxCommandConverter implements CommandConversionInterface
{
   private final Map<Integer, RigidBodyBasics> rigidBodyHashMap = new HashMap<>();
   private final ReferenceFrameHashCodeResolver referenceFrameHashCodeResolver;

   public WholeBodyTrajectoryToolboxCommandConverter(FullHumanoidRobotModel fullRobotModel)
   {
      referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(fullRobotModel, new HumanoidReferenceFrames(fullRobotModel));

      RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator());
      for (RigidBodyBasics rigidBody : rootBody.subtreeIterable())
         rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody);
   }

   public WholeBodyTrajectoryToolboxCommandConverter(RigidBodyBasics rootBody)
   {
      List<ReferenceFrame> referenceFrames = new ArrayList<>();
      for (JointBasics joint : rootBody.childrenSubtreeIterable())
      {
         referenceFrames.add(joint.getFrameAfterJoint());
         referenceFrames.add(joint.getFrameBeforeJoint());
      }

      for (RigidBodyBasics rigidBody : rootBody.subtreeIterable())
      {
         referenceFrames.add(rigidBody.getBodyFixedFrame());
      }

      referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(referenceFrames);

      for (RigidBodyBasics rigidBody : rootBody.subtreeIterable())
         rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody);
   }

   public RigidBodyBasics getRigidBody(int hashCode)
   {
      return rigidBodyHashMap.get(hashCode);
   }

   /**
    * Only converting {@link KinematicsToolboxRigidBodyMessage}.
    */
   @Override
   public <C extends Command<?, M>, M extends Settable<M>> boolean isConvertible(C command, M message)
   {
      if (command instanceof WholeBodyTrajectoryToolboxAPI<?>)
         return true;
      return false;
   }

   /**
    * Retrieves the end-effector and convert the message into its command counterpart.
    */
   //@SuppressWarnings("unchecked")
   @Override
   public <C extends Command<?, M>, M extends Settable<M>> void process(C command, M message)
   {
      WholeBodyTrajectoryToolboxAPI<M> wholeBodyTrajectoryCommand = (WholeBodyTrajectoryToolboxAPI<M>) command;
      wholeBodyTrajectoryCommand.set(message, rigidBodyHashMap, referenceFrameHashCodeResolver);
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerAPI/input/MessagePool.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controllerAPI.input;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.commons.lists.RecyclingArrayList;

/**
 * This class provides a simple way of creating a pool of messages that is garbage free.
 * 
 * @author <NAME>
 */
public class MessagePool
{
   /** The list of pools, one for each message. */
   private final List<RecyclingArrayList<? extends Settable<?>>> messagePoolList = new ArrayList<>();
   /** The map for retrieving a message pool given its class. */
   private final Map<Class<?>, RecyclingArrayList<? extends Settable<?>>> messagePoolMap = new HashMap<>();

   /**
    * Creates a new message pool only for the given message types.
    * 
    * @param supportedMessages the list of the message types to be supported.
    */
   public MessagePool(List<Class<? extends Settable<?>>> supportedMessages)
   {
      for (int i = 0; i < supportedMessages.size(); i++)
      {
         Class<? extends Settable<?>> messageType = supportedMessages.get(i);
         RecyclingArrayList<? extends Settable<?>> messagePool = new RecyclingArrayList<>(messageType);
         messagePoolMap.put(messageType, messagePool);
         messagePoolList.add(messagePool);
      }
   }

   /**
    * Requests an available instance of the given type.
    * 
    * @param messageClass the type to get an instance of.
    * @return the instance.
    */
   public <T extends Settable<T>> T requestMessage(Class<T> messageClass)
   {
      @SuppressWarnings("unchecked")
      RecyclingArrayList<T> messagePool = (RecyclingArrayList<T>) messagePoolMap.get(messageClass);
      return messagePool.add();
   }

   /**
    * Marks all the internal memory as available for use.
    * <p>
    * Once this method has been called, messages previously requested will be recycled.
    * </p>
    */
   public void reset()
   {
      for (int i = 0; i < messagePoolList.size(); i++)
         messagePoolList.get(i).clear();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/KinematicsPlanningToolboxCenterOfMassMessage.java<|end_filename|>
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 inverse kinematics module.
       * It holds all the information needed for detailing the type of constraint to apply to the center of mass.
       */
public class KinematicsPlanningToolboxCenterOfMassMessage extends Packet<KinematicsPlanningToolboxCenterOfMassMessage> implements Settable<KinematicsPlanningToolboxCenterOfMassMessage>, EpsilonComparable<KinematicsPlanningToolboxCenterOfMassMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * This is the list of desired times for each key frames.
            */
   public us.ihmc.idl.IDLSequence.Double  way_point_times_;
   /**
            * Specifies the desired center of mass position.
            * The data is assumed to be expressed in world frame.
            */
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  desired_way_point_positions_in_world_;
   /**
            * The selection matrix is used to determinate which degree of freedom of the center of mass
            * should be controlled.
            * The selection frame coming along with the given selection matrix is used to determine to what
            * reference frame the selected axes are referring to. For instance, if only the hand height in
            * world should be controlled only the linear z component of the selection matrix should be
            * selected and the reference frame should world frame. When no reference frame is provided with
            * the selection matrix, it will be used as it is in the center of mass frame which is aligned with the world axes.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage selection_matrix_;
   /**
            * Specifies the priority of controller the position along each axis independently.
            * If no frame is provided, the weight matrix will be applied in the center of mass frame which is
            * aligned with the world axes.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage weights_;

   public KinematicsPlanningToolboxCenterOfMassMessage()
   {
      way_point_times_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

      desired_way_point_positions_in_world_ = new us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D> (100, new geometry_msgs.msg.dds.PointPubSubType());
      selection_matrix_ = new controller_msgs.msg.dds.SelectionMatrix3DMessage();
      weights_ = new controller_msgs.msg.dds.WeightMatrix3DMessage();

   }

   public KinematicsPlanningToolboxCenterOfMassMessage(KinematicsPlanningToolboxCenterOfMassMessage other)
   {
      this();
      set(other);
   }

   public void set(KinematicsPlanningToolboxCenterOfMassMessage other)
   {
      sequence_id_ = other.sequence_id_;

      way_point_times_.set(other.way_point_times_);
      desired_way_point_positions_in_world_.set(other.desired_way_point_positions_in_world_);
      controller_msgs.msg.dds.SelectionMatrix3DMessagePubSubType.staticCopy(other.selection_matrix_, selection_matrix_);
      controller_msgs.msg.dds.WeightMatrix3DMessagePubSubType.staticCopy(other.weights_, weights_);
   }

   /**
            * 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_;
   }


   /**
            * This is the list of desired times for each key frames.
            */
   public us.ihmc.idl.IDLSequence.Double  getWayPointTimes()
   {
      return way_point_times_;
   }


   /**
            * Specifies the desired center of mass position.
            * The data is assumed to be expressed in world frame.
            */
   public us.ihmc.idl.IDLSequence.Object<us.ihmc.euclid.tuple3D.Point3D>  getDesiredWayPointPositionsInWorld()
   {
      return desired_way_point_positions_in_world_;
   }


   /**
            * The selection matrix is used to determinate which degree of freedom of the center of mass
            * should be controlled.
            * The selection frame coming along with the given selection matrix is used to determine to what
            * reference frame the selected axes are referring to. For instance, if only the hand height in
            * world should be controlled only the linear z component of the selection matrix should be
            * selected and the reference frame should world frame. When no reference frame is provided with
            * the selection matrix, it will be used as it is in the center of mass frame which is aligned with the world axes.
            */
   public controller_msgs.msg.dds.SelectionMatrix3DMessage getSelectionMatrix()
   {
      return selection_matrix_;
   }


   /**
            * Specifies the priority of controller the position along each axis independently.
            * If no frame is provided, the weight matrix will be applied in the center of mass frame which is
            * aligned with the world axes.
            */
   public controller_msgs.msg.dds.WeightMatrix3DMessage getWeights()
   {
      return weights_;
   }


   public static Supplier<KinematicsPlanningToolboxCenterOfMassMessagePubSubType> getPubSubType()
   {
      return KinematicsPlanningToolboxCenterOfMassMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return KinematicsPlanningToolboxCenterOfMassMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(KinematicsPlanningToolboxCenterOfMassMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.way_point_times_, other.way_point_times_, epsilon)) return false;

      if (this.desired_way_point_positions_in_world_.size() != other.desired_way_point_positions_in_world_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.desired_way_point_positions_in_world_.size(); i++)
         {  if (!this.desired_way_point_positions_in_world_.get(i).epsilonEquals(other.desired_way_point_positions_in_world_.get(i), epsilon)) return false; }
      }

      if (!this.selection_matrix_.epsilonEquals(other.selection_matrix_, epsilon)) return false;
      if (!this.weights_.epsilonEquals(other.weights_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof KinematicsPlanningToolboxCenterOfMassMessage)) return false;

      KinematicsPlanningToolboxCenterOfMassMessage otherMyClass = (KinematicsPlanningToolboxCenterOfMassMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.way_point_times_.equals(otherMyClass.way_point_times_)) return false;
      if (!this.desired_way_point_positions_in_world_.equals(otherMyClass.desired_way_point_positions_in_world_)) return false;
      if (!this.selection_matrix_.equals(otherMyClass.selection_matrix_)) return false;
      if (!this.weights_.equals(otherMyClass.weights_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("KinematicsPlanningToolboxCenterOfMassMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("way_point_times=");
      builder.append(this.way_point_times_);      builder.append(", ");
      builder.append("desired_way_point_positions_in_world=");
      builder.append(this.desired_way_point_positions_in_world_);      builder.append(", ");
      builder.append("selection_matrix=");
      builder.append(this.selection_matrix_);      builder.append(", ");
      builder.append("weights=");
      builder.append(this.weights_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/behaviors/WalkToGoalAction.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.behaviors;

public enum WalkToGoalAction
{
   FIND_PATH, EXECUTE, EXECUTE_UNKNOWN, STOP;

   public static final WalkToGoalAction[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static WalkToGoalAction fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/NumericSlider.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import org.apache.commons.math3.util.Precision;

import javafx.geometry.Side;
import javafx.scene.control.ContextMenu;
import javafx.scene.control.MenuItem;
import javafx.scene.control.Slider;

public abstract class NumericSlider<T extends Number> extends Slider
{
   public NumericSlider()
   {
      setShowTickMarks(true);
      setShowTickLabels(true);
      setPrefWidth(200.0);

      // Sets up a context menu that will show the current value while the slider is dragged.
      ContextMenu contextMenu = new ContextMenu();
      MenuItem tooltip = new MenuItem();
      contextMenu.getItems().add(tooltip);
      valueProperty().addListener((observable, oldValue, newValue) -> tooltip.setText(Double.toString(roundToPrecision(getValue()))));
      valueChangingProperty().addListener((observable, oldValue, newValue) -> {
         if (newValue)
         {
            if (!contextMenu.isShowing())
            {
               contextMenu.show(NumericSlider.this, Side.BOTTOM, 0.0, 0.0);
            }
         }
         else
         {
            contextMenu.hide();
         }
      });
   }

   public void updateSlider(T value, T min, T max)
   {
      double doubleValue = toDouble(value);
      double doubleMin = toDouble(min);
      double doubleMax = toDouble(max);

      boolean boundsValid = doubleValue < doubleMin || doubleValue > doubleMax || doubleMin >= doubleMax;
      boolean boundsInfinite = Double.isInfinite(doubleMin) || Double.isInfinite(doubleMax);
      if (boundsValid || boundsInfinite)
      {
         setDisable(true);
         return;
      }

      setDisable(false);
      double range = doubleMax - doubleMin;

      setMin(doubleMin);
      setMax(doubleMax);
      setValue(doubleValue);

      double majorTickUnit = roundToPrecision(range / 2.0);
      majorTickUnit = Math.max(majorTickUnit, getStepSize());
      setMajorTickUnit(majorTickUnit);

      int minorTickCount = (int) (majorTickUnit / getStepSize()) - 1;
      if (minorTickCount > 7)
      {
         setSnapToTicks(false);
         setMinorTickCount(0);
      }
      else
      {
         setSnapToTicks(true);
         setMinorTickCount(minorTickCount);
      }
   }

   public T getNumber()
   {
      return toNumber(roundToPrecision(getValue()));
   }

   public abstract double toDouble(T number);

   public abstract T toNumber(double doubleValue);

   public abstract int getPrecision();

   private double getStepSize()
   {
      return Math.pow(10.0, -getPrecision());
   }

   public double roundToPrecision(double value)
   {
      return Precision.round(value, getPrecision());
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SymmetricNumericalDifferentiator.java<|end_filename|>
package us.ihmc.robotics.math.filters;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DBasics;

public class SymmetricNumericalDifferentiator
{
   public static double[] differentiate(double[] xs, double[] f)
   {
      double[] df = new double[f.length];
      for (int i = 1; i < f.length - 1; i++)
      {
         double lower = f[i - 1];
         double upper = f[i + 1];
         double diff = (upper - lower) / (xs[i] - xs[i - 1] + xs[i + 1] - xs[i]);
         df[i] = diff;
      }

      // Add endpoints by padding out elements 1 and n-1
      df[0] = df[1];
      df[df.length - 1] = df[df.length - 2];

      return df;
   }

   public static double[] differentiate(double dt, double[] f)
   {
      double[] xs = new double[f.length];
      for (int i = 0; i < xs.length; i++)
      {
         xs[i] = i * dt;
      }
      return differentiate(xs, f);
   }

   public static List<FrameVector3D> differentiate(double dt, ReferenceFrame frame, List<? extends FrameTuple3DBasics> xs)
   {
      List<FrameVector3D> vs = new ArrayList<>(xs.size());
      for (int i = 1; i < xs.size() - 1; i++)
      {
         FrameTuple3DBasics before = xs.get(i - 1);
         FrameTuple3DBasics after = xs.get(i + 1);

         FrameVector3D v = difference(dt, frame, before, after);
         vs.add(v);
      }

      // Add endpoints by padding out elements 1 and n-1
      vs.add(0, new FrameVector3D(vs.get(0)));
      vs.add(new FrameVector3D(vs.get(vs.size() - 1)));

      return vs;
   }

   private static FrameVector3D difference(double dt, ReferenceFrame frame, FrameTuple3DBasics before, FrameTuple3DBasics after)
   {
      after.checkReferenceFrameMatch(before);

      FrameVector3D v = new FrameVector3D(after.getReferenceFrame());
      v.sub(after, before);
      v.scale(1 / (2.0 * dt));
      v.changeFrame(frame);

      return v;
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/IntegratorGainsReadOnly.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains;

public interface IntegratorGainsReadOnly
{
   /**
    * Get the integral gain, as in a PID controller.
    * @return the integral gain
    */
   default double getKi()
   {
      return 0.0;
   }

   /**
    * Get the maximum total integral error. This can be used to limit the
    * maximum feedback action caused by the integration.
    * <p>
    * To disable integral error limiting set to {@link Double#POSITIVE_INFINITY}
    * </p>
    * @return the maximum integral error
    */
   default double getMaxIntegralError()
   {
      return Double.POSITIVE_INFINITY;
   }

   /**
    * Get the leak ratio of an integrator. If using a leaking integrator
    * the integal error will decay over time as it is multiplied by the leak
    * rate at each control tick.
    * <p>
    * To disable leaking set to {@code 1.0}
    * </p>
    * @return the integrator leak rate
    */
   default double getIntegralLeakRatio()
   {
      return 1.0;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/TrajectoryPoint1DMessage.java<|end_filename|>
package controller_msgs.msg.dds;

import us.ihmc.communication.packets.Packet;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.euclid.interfaces.EpsilonComparable;
import java.util.function.Supplier;
import us.ihmc.pubsub.TopicDataType;

/**
       * This message is part of the IHMC whole-body controller API.
       * This class is used to build 1D trajectory messages including jointspace trajectory messages.
       * For 3D trajectory points look at:
       * - EuclideanTrajectoryMessage (translational),
       * - SO3TrajectoryPointMessage (rotational),
       * - SE3TrajectoryPointMessage (translational AND rotational).
       */
public class TrajectoryPoint1DMessage extends Packet<TrajectoryPoint1DMessage> implements Settable<TrajectoryPoint1DMessage>, EpsilonComparable<TrajectoryPoint1DMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public double time_;
   /**
            * Define the desired 1D position to be reached at this trajectory point.
            */
   public double position_;
   /**
            * Define the desired 1D velocity to be reached at this trajectory point.
            */
   public double velocity_;

   public TrajectoryPoint1DMessage()
   {
   }

   public TrajectoryPoint1DMessage(TrajectoryPoint1DMessage other)
   {
      this();
      set(other);
   }

   public void set(TrajectoryPoint1DMessage other)
   {
      sequence_id_ = other.sequence_id_;

      time_ = other.time_;

      position_ = other.position_;

      velocity_ = other.velocity_;

   }

   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public void setSequenceId(long sequence_id)
   {
      sequence_id_ = sequence_id;
   }
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long getSequenceId()
   {
      return sequence_id_;
   }

   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public void setTime(double time)
   {
      time_ = time;
   }
   /**
            * Time at which the trajectory point has to be reached. The time is relative to when the trajectory starts.
            */
   public double getTime()
   {
      return time_;
   }

   /**
            * Define the desired 1D position to be reached at this trajectory point.
            */
   public void setPosition(double position)
   {
      position_ = position;
   }
   /**
            * Define the desired 1D position to be reached at this trajectory point.
            */
   public double getPosition()
   {
      return position_;
   }

   /**
            * Define the desired 1D velocity to be reached at this trajectory point.
            */
   public void setVelocity(double velocity)
   {
      velocity_ = velocity;
   }
   /**
            * Define the desired 1D velocity to be reached at this trajectory point.
            */
   public double getVelocity()
   {
      return velocity_;
   }


   public static Supplier<TrajectoryPoint1DMessagePubSubType> getPubSubType()
   {
      return TrajectoryPoint1DMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return TrajectoryPoint1DMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(TrajectoryPoint1DMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.time_, other.time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.position_, other.position_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.velocity_, other.velocity_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof TrajectoryPoint1DMessage)) return false;

      TrajectoryPoint1DMessage otherMyClass = (TrajectoryPoint1DMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.time_ != otherMyClass.time_) return false;

      if(this.position_ != otherMyClass.position_) return false;

      if(this.velocity_ != otherMyClass.velocity_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("TrajectoryPoint1DMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("time=");
      builder.append(this.time_);      builder.append(", ");
      builder.append("position=");
      builder.append(this.position_);      builder.append(", ");
      builder.append("velocity=");
      builder.append(this.velocity_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/OverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder;


public abstract class OverheadPath implements ReferenceFrameHolder
{
   public abstract FramePose2D getPoseAtS(double pathVariableS);

   public abstract FramePose2D getExtrapolatedPoseAtS(double pathVariableS);

   public abstract OverheadPath changeFrameCopy(ReferenceFrame desiredFrame);
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/outputData/LowLevelActuatorMode.java<|end_filename|>
package us.ihmc.sensorProcessing.outputData;

public enum LowLevelActuatorMode
{
   DISABLED,
   EFFORT,
   VELOCITY,
   POSITION;

   public static LowLevelActuatorMode[] values = values();
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/SpatialNodeTree.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.manipulation.planning.exploringSpatial.SpatialNode;

public class SpatialNodeTree
{
   private SpatialNode rootNode;
   private double mostAdvancedTime = 0.0;
   private List<SpatialNode> validNodes = new ArrayList<>();

   private List<SpatialNode> invalidNodes = new ArrayList<>();

   private double timeWeight = 0.5;
   private double positionWeight = 1.0;
   private double orientationWeight = 1.0;

   private double maxTimeInterval = 1 / 2.0;
   private double maxPositionDistance = 0.05;
   private double maxOrientationDistance = Math.toRadians(10.0);
   
//   private double maxTimeInterval = 1 / 3.0;
//   private double maxPositionDistance = 0.08;
//   private double maxOrientationDistance = Math.toRadians(25);

   private SpatialNode currentCandidate = null;
   private SpatialNode currentCandidateParent = null;

   private SpatialNode randomNode = null;

   public SpatialNodeTree()
   {

   }

   public SpatialNodeTree(SpatialNode rootNode)
   {
      this.rootNode = rootNode;
      validNodes.add(rootNode);
   }

   public void addInitialNode(SpatialNode node)
   {
      validNodes.add(node);
   }

   public void setRandomNode(SpatialNode node)
   {
      randomNode = node;
   }

   public SpatialNode getCandidate()
   {
      return currentCandidate;
   }

   public boolean findNearestValidNodeToCandidate(boolean includeTimeComparison)
   {
      double distanceToNearestNode = Double.MAX_VALUE;
      SpatialNode nearestNode = null;

      for (SpatialNode candidateForParent : validNodes)
      {
         if (randomNode.getTime() < candidateForParent.getTime())
            continue;

         double distance;

         if (includeTimeComparison)
            //distance = candidateForParent.computeDistance(timeWeight, positionWeight, orientationWeight, randomNode);
            distance = candidateForParent.computeDistanceWithinMaxDistance(timeWeight, positionWeight, orientationWeight, randomNode, maxTimeInterval,
                                                                           maxPositionDistance, maxOrientationDistance);
         else
            //distance = candidateForParent.computeDistance(0.0, positionWeight, orientationWeight, randomNode);
            distance = candidateForParent.computeDistanceWithinMaxDistance(0.0, positionWeight, orientationWeight, randomNode, maxTimeInterval,
                                                                           maxPositionDistance, maxOrientationDistance);

         if (distance < distanceToNearestNode)
         {
            distanceToNearestNode = distance;
            nearestNode = candidateForParent;
         }
      }

      if (nearestNode == null)
      {
         return false;
      }
      else
      {
         currentCandidateParent = nearestNode;
         return true;
      }
   }

   public void limitCandidateDistanceFromParent(double trajectoryTime)
   {
      //currentCandidate = currentCandidateParent.createNodeWithinMaxDistance(maxTimeInterval, maxPositionDistance, maxOrientationDistance, randomNode);
      currentCandidate = currentCandidateParent.createNodeWithinTimeStep(maxTimeInterval, randomNode);
      if (currentCandidate.getTime() > trajectoryTime)
         currentCandidate.setTime(trajectoryTime);
      currentCandidate.setParent(currentCandidateParent);
   }

   public void attachCandidate()
   {
      if (!currentCandidate.isValid())
         throw new RuntimeException("Should only attach valid nodes to this tree.");

      mostAdvancedTime = Math.max(currentCandidate.getTime(), mostAdvancedTime);
      validNodes.add(new SpatialNode(currentCandidate));
   }

   public void attachCandidate(SpatialNode node)
   {
      mostAdvancedTime = Math.max(node.getTime(), mostAdvancedTime);
      validNodes.add(new SpatialNode(node));
   }

   public void dismissCandidate()
   {
      if (currentCandidate.isValid())
         throw new RuntimeException("Should attach valid nodes to this tree.");

      currentCandidate.clearParent();
      invalidNodes.add(currentCandidate);
   }

   public SpatialNode getLastNodeAdded()
   {
      return validNodes.get(validNodes.size() - 1);
   }

   public double getTimeWeight()
   {
      return timeWeight;
   }

   public double getPositionWeight()
   {
      return positionWeight;
   }

   public double getOrientationWeight()
   {
      return orientationWeight;
   }

   public double getMaxTimeInterval()
   {
      return maxTimeInterval;
   }

   public double getMaxPositionDistance()
   {
      return maxPositionDistance;
   }

   public double getMaxOrientationDistance()
   {
      return maxOrientationDistance;
   }

   public void setTimeWeight(double timeWeight)
   {
      this.timeWeight = timeWeight;
   }

   public void setPositionWeight(double positionWeight)
   {
      this.positionWeight = positionWeight;
   }

   public void setOrientationWeight(double orientationWeight)
   {
      this.orientationWeight = orientationWeight;
   }

   public void setMaxTimeInterval(double maxTimeInterval)
   {
      this.maxTimeInterval = maxTimeInterval;
   }

   public void setMaxPositionDistance(double maxPositionDistance)
   {
      this.maxPositionDistance = maxPositionDistance;
   }

   public void setMaxOrientationDistance(double maxOrientationDistance)
   {
      this.maxOrientationDistance = maxOrientationDistance;
   }

   public List<SpatialNode> getValidNodes()
   {
      return validNodes;
   }

   public double getMostAdvancedTime()
   {
      return mostAdvancedTime;
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/RRTPiecewisePath.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt;

import java.util.ArrayList;
import java.util.Random;

/**
 * <RRTPiecewisePath> class a node path divides into @param sizeOfPiecewise.
 * ArrayList<RRTNode> piecewisePath is initialized when this class is initiated.
 * @method updateShortCutPath() will return validity of the shortcut path after updating shortcutPath from the piecewisePath and linearPath.
 * 
 * @author InhoLee 170224
 *
 */
public class RRTPiecewisePath
{
   private ArrayList<RRTNode> linearPath = new ArrayList<RRTNode>();
   private ArrayList<RRTNode> piecewisePath = new ArrayList<RRTNode>();
   private ArrayList<RRTNode> shortcutPath = new ArrayList<RRTNode>();
   private int[] pieceIndex;
   private int sizeOfPiecewisePath;

   private RRTNode nodeCreator;

   public RRTPiecewisePath(ArrayList<RRTNode> linearPath, int sizeOfPicewise)
   {
      this.linearPath = linearPath;
      this.sizeOfPiecewisePath = sizeOfPicewise;
      pieceIndex = new int[this.sizeOfPiecewisePath];
      nodeCreator = linearPath.get(0).createNode();
      initializePiecewisePath();
   }

   private void initializePiecewisePath()
   {
      double totalLength = 0;
      double pieceLength;
      double remainderLength;

      for (int i = 0; i < linearPath.size() - 1; i++)
      {
         totalLength = totalLength + linearPath.get(i).getDistance(linearPath.get(i + 1));
      }

      pieceLength = totalLength / (sizeOfPiecewisePath - 1);

      RRTNode curPathNode;
      RRTNode nextPathNode;
      RRTNode precedentNode = linearPath.get(0);
      RRTNode succedentNode;

      // initial
      int curPiece = 0;
      curPathNode = linearPath.get(curPiece);
      nextPathNode = linearPath.get(curPiece + 1);
      addPathPiecewise(precedentNode, curPiece);

      for (int i = 1; i < sizeOfPiecewisePath - 1; i++)
      {
         succedentNode = nodeCreator.createNode();

         remainderLength = pieceLength;

         double nomalizedDistance;
         double distanceToGo;

         while (true)
         {
            nomalizedDistance = precedentNode.getDistance(nextPathNode);

            if (nomalizedDistance >= remainderLength)
            {
               distanceToGo = remainderLength;
               break;
            }
            else
            {
               remainderLength = remainderLength - nomalizedDistance;

               curPiece++;

               curPathNode = linearPath.get(curPiece);
               nextPathNode = linearPath.get(curPiece + 1);
               precedentNode = linearPath.get(curPiece);
            }
         }

         setNodeDataInterpolation(precedentNode, nextPathNode, succedentNode, distanceToGo);

         addPathPiecewise(succedentNode, curPiece);
         precedentNode = succedentNode;

      }

      // final
      succedentNode = nodeCreator.createNode();
      for (int i = 0; i < precedentNode.getDimensionOfNodeData(); i++)
      {
         succedentNode.setNodeData(i, linearPath.get(linearPath.size() - 1).getNodeData(i));
      }
      addPathPiecewise(succedentNode, curPiece);
   }

   private void setNodeDataInterpolation(RRTNode nodeOne, RRTNode nodeTwo, RRTNode targetNode, double desLength)
   {
      double normailizedDistance = nodeOne.getDistance(nodeTwo);
      for (int i = 0; i < targetNode.getDimensionOfNodeData(); i++)
      {
         double dataOne = nodeOne.getNodeData(i);
         double dataTwo = nodeTwo.getNodeData(i);
         double interpolatedData = desLength / normailizedDistance * (dataTwo - dataOne) + dataOne;
         targetNode.setNodeData(i, interpolatedData);
      }
   }

   private void addPathPiecewise(RRTNode node, int idOfPiece)
   {
      piecewisePath.add(node);
      pieceIndex[piecewisePath.size() - 1] = idOfPiece;
   }

   private int getRandomNumber(int minIndex, int maxIndex)
   {
      Random randomManager = new Random();
      if (minIndex == maxIndex)
      {
         return minIndex;
      }
      else
      {
         return (randomManager.nextInt(maxIndex - minIndex) + minIndex);
      }
   }

   private int getRandomNumber(int minIndex, int maxIndex, int exceptNumber)
   {
      Random randomManager = new Random();
      int ret;
      while (true)
      {
         ret = randomManager.nextInt(maxIndex - minIndex) + minIndex;
         if (ret != exceptNumber)
         {
            break;
         }
      }
      return ret;
   }

   private int getRandomPiecewiseNodeIndex(int piece)
   {
      int minIndex = 0, maxIndex = 0;
      for (int i = 0; i < piecewisePath.size(); i++)
      {
         if (pieceIndex[i] == piece)
         {
            maxIndex = i;
         }
      }
      for (int i = piecewisePath.size() - 1; i > -1; i--)
      {
         if (pieceIndex[i] == piece)
         {
            minIndex = i;
         }
      }

      if (minIndex == 0 && maxIndex == 0)
      {
         return -1;
      }
      return getRandomNumber(minIndex, maxIndex);
   }

   private int[] getPiecePair()
   {
      int[] retPair = new int[2];

      retPair[0] = getRandomNumber(0, linearPath.size() - 1);
      retPair[1] = getRandomNumber(0, linearPath.size() - 1, retPair[0]);

      if (retPair[0] > retPair[1])
      {
         int pieceTemp = retPair[0];
         retPair[0] = retPair[1];
         retPair[1] = pieceTemp;
      }

      return retPair;
   }

   private boolean isValidPath(ArrayList<RRTNode> nodePath)
   {
      RRTPiecewisePath newPiecewisePath = new RRTPiecewisePath(nodePath, this.sizeOfPiecewisePath);

      for (int i = 0; i < newPiecewisePath.piecewisePath.size(); i++)
      {
         if (newPiecewisePath.piecewisePath.get(i).isValidNode() == false)
         {
            return false;
         }
      }
      return true;
   }

   public boolean updateShortCutPath()
   {
      shortcutPath.clear();

      int[] linearPiecePair;
      int[] piecewisePair = new int[2];

      while (true)
      {
         linearPiecePair = getPiecePair();
         piecewisePair[0] = getRandomPiecewiseNodeIndex(linearPiecePair[0]);
         piecewisePair[1] = getRandomPiecewiseNodeIndex(linearPiecePair[1]);
         if (piecewisePair[0] != -1 && piecewisePair[1] != -1)
         {
            break;
         }
      }

      for (int i = 0; i <= linearPiecePair[0]; i++)
      {
         shortcutPath.add(linearPath.get(i));
      }
      shortcutPath.add(piecewisePath.get(piecewisePair[0]));
      shortcutPath.add(piecewisePath.get(piecewisePair[1]));

      for (int i = linearPiecePair[1] + 1; i < linearPath.size(); i++)
      {
         shortcutPath.add(linearPath.get(i));
      }

      return isValidPath(shortcutPath);
   }

   public ArrayList<RRTNode> getLinearPath()
   {
      return linearPath;
   }

   public ArrayList<RRTNode> getPiecewisePath()
   {
      return piecewisePath;
   }
   
   public ArrayList<RRTNode> getShortCutPath()
   {
      return shortcutPath;
   }

}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosPointCloudPublisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import org.jboss.netty.buffer.ChannelBuffer;
import org.jboss.netty.buffer.LittleEndianHeapChannelBuffer;
import org.ros.message.Time;

import sensor_msgs.PointCloud2;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.graphicsDescription.color.MutableColor;
import us.ihmc.utilities.ros.types.PointType;

public class RosPointCloudPublisher extends RosTopicPublisher<PointCloud2>
{
 

   PointType pointType;

   public RosPointCloudPublisher(PointType pointType, boolean latch)
   {
      super(PointCloud2._TYPE, latch);
      this.pointType = pointType;
   }
   


   public void publish(Point3D[] points, float[] intensities, String frameId)
   {
      PointCloud2 message = getMessage();
      message.getHeader().setFrameId(frameId);
      message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis()));
      message.setHeight(1);
      message.setWidth(points.length);
      message.setPointStep(pointType.getPointStep());
      int dataLength = pointType.getPointStep() * points.length;
      message.setRowStep(dataLength);
      message.setIsBigendian(false);
      message.setIsDense(true);
      message.setFields(pointType.getPointField());
      
      ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength);
      for(int i=0;i<points.length;i++)
      {
         buffer.writeFloat((float)points[i].getX());
         buffer.writeFloat((float)points[i].getY());
         buffer.writeFloat((float)points[i].getZ());
         buffer.writeFloat(intensities[i]);
      }
      message.setData(buffer);
      
      publish(message);
   }



   public void publish(Point3D[] points, MutableColor[] colors, String frameId)
   {
      PointCloud2 message = getMessage();
      message.getHeader().setFrameId(frameId);
      message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis()));
      message.setHeight(1);
      message.setWidth(points.length);
      message.setPointStep(pointType.getPointStep());
      int dataLength = pointType.getPointStep() * points.length;
      message.setRowStep(dataLength);
      message.setIsBigendian(false);
      message.setIsDense(true);
      message.setFields(pointType.getPointField());
      
      ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength);
      for(int i=0;i<points.length;i++)
      {
         buffer.writeFloat((float)points[i].getX());
         buffer.writeFloat((float)points[i].getY());
         buffer.writeFloat((float)points[i].getZ());
         buffer.writeByte((int)colors[i].getZ());
         buffer.writeByte((int)colors[i].getY());
         buffer.writeByte((int)colors[i].getX());
         buffer.writeByte(0); //dummy;
      }
      message.setData(buffer);
      
      publish(message);
      
   }

   public void publish(Point3D[] points, MutableColor color, String frameId)
   {
      PointCloud2 message = getMessage();
      message.getHeader().setFrameId(frameId);
      message.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis()));
      message.setHeight(1);
      message.setWidth(points.length);
      message.setPointStep(pointType.getPointStep());
      int dataLength = pointType.getPointStep() * points.length;
      message.setRowStep(dataLength);
      message.setIsBigendian(false);
      message.setIsDense(true);
      message.setFields(pointType.getPointField());

      ChannelBuffer buffer = new LittleEndianHeapChannelBuffer(dataLength);
      for(int i=0;i<points.length;i++)
      {
         buffer.writeFloat((float)points[i].getX());
         buffer.writeFloat((float)points[i].getY());
         buffer.writeFloat((float)points[i].getZ());
         buffer.writeByte((int)color.getZ());
         buffer.writeByte((int)color.getY());
         buffer.writeByte((int)color.getX());
         buffer.writeByte(0); //dummy;
      }
      message.setData(buffer);

      publish(message);
   }

}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/AtlasStatisticsDisplay.java<|end_filename|>
package us.ihmc.atlas;

import java.lang.reflect.Constructor;
import java.lang.reflect.InvocationTargetException;

import com.martiansoftware.jsap.FlaggedOption;
import com.martiansoftware.jsap.JSAP;
import com.martiansoftware.jsap.JSAPResult;

import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.robotModels.FullHumanoidRobotModel;

public class AtlasStatisticsDisplay
{
   public static void main(String[] args)
   {
      // Flag to set robot model
      JSAP jsap = new JSAP();
      FlaggedOption robotModel = new FlaggedOption("robotModel").setLongFlag("model").setShortFlag('m').setRequired(true).setStringParser(JSAP.STRING_PARSER);
      robotModel.setHelp("Robot models: " + AtlasRobotModelFactory.robotModelsToString());

      DRCRobotModel model;
      try
      {
         jsap.registerParameter(robotModel);

         JSAPResult config = jsap.parse(args);

         if (config.success())
         {
            model = AtlasRobotModelFactory.createDRCRobotModel(config.getString("robotModel"), RobotTarget.SCS, false);
         }
         else
         {
            System.out.println("Enter a robot model.");

            return;
         }
      }
      catch (Exception e)
      {
         System.out.println("Robot model not found");
         e.printStackTrace();

         return;
      }

      try
      {
    	  Class<?> clazz = Class.forName("us.ihmc.humanoidOperatorInterface.networking.StatisticsDisplay");
    	  Constructor<?> constructor = clazz.getDeclaredConstructor(FullHumanoidRobotModel.class);
    	  constructor.newInstance(model.createFullRobotModel());
      }
      catch (ClassNotFoundException e)
      {
        // TODO insert Dennis Nedry troll here
    	  e.printStackTrace();
      }
      catch (NoSuchMethodException | SecurityException | InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException e)
      {
    	  e.printStackTrace();
      }
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/simpleArm/SimpleRobotInputOutputMap.java<|end_filename|>
package us.ihmc.exampleSimulations.simpleArm;

import us.ihmc.exampleSimulations.simpleArm.SimpleArmRobot.ArmJoint;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;

public class SimpleRobotInputOutputMap
{
   private final SimpleArmRobot robot;

   public SimpleRobotInputOutputMap(SimpleArmRobot robot)
   {
      this.robot = robot;
   }

   public void readFromSimulation()
   {
      robot.updateInverseDynamicsStructureFromSimulation();
   }

   public void writeToSimulation()
   {
      robot.updateSimulationFromInverseDynamicsTorques();
   }

   public double getYaw()
   {
      return robot.getJoint(ArmJoint.YAW).getQ();
   }

   public double getPitch1()
   {
      return robot.getJoint(ArmJoint.PITCH_1).getQ();
   }

   public double getPitch2()
   {
      return robot.getJoint(ArmJoint.PITCH_2).getQ();
   }

   public void addYawTorque(double yawTorque)
   {
      OneDoFJointBasics joint = robot.getJoint(ArmJoint.YAW);
      joint.setTau(joint.getTau() + yawTorque);
   }

   public void addPitch1Torque(double pitch1Torque)
   {
      OneDoFJointBasics joint = robot.getJoint(ArmJoint.PITCH_1);
      joint.setTau(joint.getTau() + pitch1Torque);
   }

   public void addPitch2Torque(double pitch2Torque)
   {
      OneDoFJointBasics joint = robot.getJoint(ArmJoint.PITCH_2);
      joint.setTau(joint.getTau() + pitch2Torque);
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tabs/TabSavingTools.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tabs;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.prefs.Preferences;
import java.util.stream.Collectors;

import org.apache.commons.lang3.StringUtils;
import org.apache.commons.lang3.tuple.ImmutablePair;

import javafx.scene.control.Alert;
import javafx.scene.control.Alert.AlertType;
import javafx.scene.control.TabPane;
import javafx.stage.FileChooser;
import javafx.stage.FileChooser.ExtensionFilter;
import javafx.stage.Window;
import javafx.util.Pair;
import us.ihmc.parameterTuner.guiElements.GuiElement;
import us.ihmc.parameterTuner.guiElements.tuners.Tuner;

public class TabSavingTools
{
   public static File saveTab(TuningTab tab, Window window)
   {
      FileChooser fileChooser = new FileChooser();
      fileChooser.getExtensionFilters().add(getExtensionFilter());
      fileChooser.setTitle("Save Tab");
      File defaultFilePath = getDefaultFilePath();
      if (defaultFilePath != null)
      {
         fileChooser.setInitialDirectory(defaultFilePath);
      }
      fileChooser.setInitialFileName(tab.getName() + ".tab");

      File file = fileChooser.showSaveDialog(window);
      if (file != null)
      {
         setDefaultFilePath(file);
         if (saveTab(tab, file))
         {
            return file;
         }
      }
      return null;
   }

   private static boolean saveTab(TuningTab tab, File file)
   {
      try
      {
         FileWriter writer = new FileWriter(file);
         writer.write(tab.getName() + "\n");
         List<Pair<String, Integer>> info = tab.getParameterSavingInfo();
         for (Pair<String, Integer> parameterInfo : info)
         {
            writer.write(parameterInfo.getKey() + " " + parameterInfo.getValue() + "\n");
         }
         writer.close();
         return true;
      }
      catch (IOException e)
      {
         e.printStackTrace();
         return false;
      }
   }

   public static ImmutablePair<TuningTab, File> loadTab(TabPane tabPane, Map<String, Tuner> tunerMap, Window window)
   {
      FileChooser fileChooser = new FileChooser();
      fileChooser.getExtensionFilters().add(getExtensionFilter());
      fileChooser.setTitle("Load Tab");
      File defaultFilePath = getDefaultFilePath();
      if (defaultFilePath != null)
      {
         fileChooser.setInitialDirectory(defaultFilePath);
      }

      File file = fileChooser.showOpenDialog(window);
      if (file != null)
      {
         setDefaultFilePath(file);
         TuningTab newTab = loadTab(tabPane, tunerMap, file, true);
         if (newTab != null)
         {
            return new ImmutablePair<>(newTab, file);
         }
      }
      return null;
   }

   public static List<ImmutablePair<TuningTab, File>> loadDefaultTabs(TabPane tabPane, Map<String, Tuner> tunerMap)
   {
      List<ImmutablePair<TuningTab, File>> loadedFiles = new ArrayList<>();
      getDefaultTuningTabFiles().forEach(file -> {
         TuningTab newTab = loadTab(tabPane, tunerMap, file, false);
         if (newTab != null)
         {
            loadedFiles.add(new ImmutablePair<>(newTab, file));
         }
      });
      return loadedFiles;
   }

   public static TuningTab loadTab(TabPane tabPane, Map<String, Tuner> tunerMap, File file, boolean showWarning)
   {
      try
      {
         FileReader fileReader = new FileReader(file);
         BufferedReader reader = new BufferedReader(fileReader);

         String name = reader.readLine();
         if (name == null)
         {
            reader.close();
            return null;
         }
         TuningTab newTab = new TuningTab(name, tabPane);
         newTab.setTunerMap(tunerMap);

         String line = reader.readLine();
         List<String> namesNotFound = new ArrayList<>();
         while (line != null)
         {
            String[] splitLine = line.split("\\s");
            String parameter = splitLine[0];
            int sliderIndex = splitLine.length > 1 ? Integer.parseInt(splitLine[1]) : -1;

            if (tunerMap.containsKey(parameter))
            {
               newTab.handleNewParameter(parameter, sliderIndex);
            }
            else
            {
               namesNotFound.add(parameter);
            }

            line = reader.readLine();
         }
         reader.close();

         if (!namesNotFound.isEmpty() && showWarning)
         {
            showParametersNotFound(namesNotFound);
         }

         return newTab;
      }
      catch (IOException e)
      {
         e.printStackTrace();
         return null;
      }
   }

   private static void showParametersNotFound(List<String> namesNotFound)
   {
      String names = "";
      for (String name : namesNotFound)
      {
         String[] splitNamespace = StringUtils.split(name, GuiElement.SEPERATOR);
         String simpleName = splitNamespace[splitNamespace.length - 1];
         names += simpleName + "\n";
      }

      Alert alert = new Alert(AlertType.INFORMATION);
      alert.setTitle("Missing Parameters");
      alert.setHeaderText("Unable to locate parameters:");
      alert.setContentText(names);
      alert.showAndWait();
   }

   private static ExtensionFilter getExtensionFilter()
   {
      return new FileChooser.ExtensionFilter("Tab Files (*.tab)", "*.tab");
   }

   private static File getDefaultFilePath()
   {
      Preferences prefs = Preferences.userNodeForPackage(TuningTabManager.class);
      String filePath = prefs.get("tuningTabConfigurationPath", null);

      if (filePath != null && Files.isDirectory(Paths.get(filePath)))
         return new File(filePath);
      else
         return null;
   }

   private static void setDefaultFilePath(File file)
   {
      Preferences prefs = Preferences.userNodeForPackage(TuningTabManager.class);
      if (file != null)
      {
         if (!file.isDirectory())
            file = file.getParentFile();

         prefs.put("tuningTabConfigurationPath", file.getAbsolutePath());
      }
   }

   public static List<File> getDefaultTuningTabFiles()
   {
      Preferences prefs = Preferences.userNodeForPackage(TuningTabManager.class);
      String prefix = "defaultTuningTab";
      int index = 0;

      List<File> files = new ArrayList<>();

      while (prefs.get(prefix + index, null) != null)
      {
         String filePath = prefs.get(prefix + index, null);
         if (Files.isRegularFile(Paths.get(filePath)))
         {
            files.add(new File(filePath));
         }
         index++;
      }

      return files;
   }

   public static void saveDefaultTabFiles(List<File> files)
   {
      clearDefaultTabFiles();

      Preferences prefs = Preferences.userNodeForPackage(TuningTabManager.class);
      String prefix = "defaultTuningTab";

      List<File> filteredFiles = files.stream().filter(file -> (file != null && file.isFile())).collect(Collectors.toList());
      for (int index = 0; index < filteredFiles.size(); index++)
      {
         File file = filteredFiles.get(index);
         prefs.put(prefix + index, file.getAbsolutePath());
      }
   }

   public static void clearDefaultTabFiles()
   {
      Preferences prefs = Preferences.userNodeForPackage(TuningTabManager.class);
      String prefix = "defaultTuningTab";
      int index = 0;

      while (prefs.get(prefix + index, null) != null)
      {
         prefs.remove(prefix + index);
         index++;
      }
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/joystick/EnumDependentJoystickMapping.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.joystick;

import java.util.ArrayList;

import us.ihmc.simulationConstructionSetTools.util.inputdevices.EnumDependentInputMapping;
import us.ihmc.tools.inputDevices.joystick.JoystickEventListener;

public interface EnumDependentJoystickMapping<T extends Enum<T>> extends EnumDependentInputMapping<T>
{
   public ArrayList<JoystickEventListener> getEventListeners();
}


<|start_filename|>ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/linearAlgebra/QRNullspaceCalculatorTest.java<|end_filename|>
package us.ihmc.robotics.linearAlgebra;

/**
 * @author twan
 * Date: 4/11/13
 */
public class QRNullspaceCalculatorTest extends NullspaceCalculatorTest
{
   @Override
   public NullspaceCalculator getNullspaceProjectorCalculator()
   {
      return new QRNullspaceCalculator(10);
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/partNames/ContactPointDefinitionHolder.java<|end_filename|>
package us.ihmc.robotics.partNames;

import java.util.List;

import org.apache.commons.lang3.tuple.ImmutablePair;

import us.ihmc.euclid.tuple3D.Vector3D;

public interface ContactPointDefinitionHolder
{
   public abstract List<ImmutablePair<String, Vector3D>> getJointNameGroundContactPointMap();
   
   default int getGroupIdentifier(ImmutablePair<String, Vector3D> nameAndGroundContactPoint)
   {
      return 0;
   }
}



<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/posePlayback/PosePlaybackPacket.java<|end_filename|>
package us.ihmc.avatar.posePlayback;

import java.util.Map;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;

public interface PosePlaybackPacket
{
   public abstract Map<OneDoFJointBasics, Double> getJointKps();
   public abstract Map<OneDoFJointBasics, Double> getJointKds();
   
   public abstract PlaybackPoseSequence getPlaybackPoseSequence();
   
   public abstract double getInitialGainScaling();
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullHumanoidRobotModelFromDescription.java<|end_filename|>
package us.ihmc.robotModels;

import java.util.ArrayList;
import java.util.EnumMap;
import java.util.List;

import org.apache.commons.lang3.tuple.ImmutablePair;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.tools.MultiBodySystemTools;
import us.ihmc.robotics.partNames.ArmJointName;
import us.ihmc.robotics.partNames.HumanoidJointNameMap;
import us.ihmc.robotics.partNames.JointRole;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.partNames.LimbName;
import us.ihmc.robotics.partNames.NeckJointName;
import us.ihmc.robotics.partNames.SpineJointName;
import us.ihmc.robotics.robotDescription.FloatingJointDescription;
import us.ihmc.robotics.robotDescription.JointDescription;
import us.ihmc.robotics.robotDescription.LinkDescription;
import us.ihmc.robotics.robotDescription.RobotDescription;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.robotics.screwTheory.ScrewTools;

public class FullHumanoidRobotModelFromDescription extends FullRobotModelFromDescription implements FullHumanoidRobotModel
{
   private SideDependentList<EnumMap<ArmJointName, OneDoFJointBasics>> armJointLists;
   private SideDependentList<EnumMap<LegJointName, OneDoFJointBasics>> legJointLists;

   private SideDependentList<ArrayList<OneDoFJointBasics>> armJointIDsList;
   private SideDependentList<ArrayList<OneDoFJointBasics>> legJointIDsList;

   private RigidBodyBasics[] endEffectors = new RigidBodyBasics[4];
   private SideDependentList<RigidBodyBasics> feet;
   private SideDependentList<RigidBodyBasics> hands;

   private RigidBodyBasics chest;

   private boolean initialized = false;

   private final SideDependentList<MovingReferenceFrame> soleFrames = new SideDependentList<>();
   private final SideDependentList<MovingReferenceFrame> handControlFrames = new SideDependentList<>();
   private final ArrayList<OneDoFJointBasics> oneDoFJointsExcludingHands = new ArrayList<>();

   private HumanoidJointNameMap humanoidJointNameMap;

   // copy constructor
   public FullHumanoidRobotModelFromDescription(FullHumanoidRobotModelFromDescription modelToCopy)
   {
      this(modelToCopy.description, modelToCopy.humanoidJointNameMap, modelToCopy.sensorLinksToTrack);
   }

   public FullHumanoidRobotModelFromDescription(RobotDescription description, HumanoidJointNameMap jointNameMap)
   {
      this(description, jointNameMap, new String[0]);
   }

   public FullHumanoidRobotModelFromDescription(RobotDescription description, HumanoidJointNameMap sdfJointNameMap, String[] sensorLinksToTrack)
   {
      super(description, sdfJointNameMap, sensorLinksToTrack);

      humanoidJointNameMap = sdfJointNameMap;

      FloatingJointDescription rootJointDescription = (FloatingJointDescription) description.getRootJoints().get(0);
      if (!rootJointDescription.getName().equals(sdfJointNameMap.getPelvisName()))
      {
         throw new RuntimeException("Pelvis joint is assumed to be the root joint");
      }


      for (RobotSide robotSide : RobotSide.values)
      {
         RigidBodyTransform soleToAnkleTransform = sdfJointNameMap.getSoleToParentFrameTransform(robotSide);
         String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression();
         MovingReferenceFrame soleFrame = MovingReferenceFrame.constructFrameFixedInParent(sidePrefix + "Sole", getEndEffectorFrame(robotSide, LimbName.LEG), soleToAnkleTransform);
         soleFrames.put(robotSide, soleFrame);

         RigidBodyTransform handControlFrameToWristTransform = sdfJointNameMap.getHandControlFrameToWristTransform(robotSide);

         if (handControlFrameToWristTransform != null)
         {
            MovingReferenceFrame attachmentPlateFrame = MovingReferenceFrame.constructFrameFixedInParent(sidePrefix + "HandControlFrame", getEndEffectorFrame(robotSide, LimbName.ARM),
                  handControlFrameToWristTransform);
            handControlFrames.put(robotSide, attachmentPlateFrame);
         }
         else
         {
            handControlFrames.put(robotSide, null);
         }
      }

      int index = 0;
      for (RobotSide robotSide : RobotSide.values)
      {
         endEffectors[index] = feet.get(robotSide);
         endEffectors[index + 2] = hands.get(robotSide);
         index++;
      }

      getAllJointsExcludingHands(oneDoFJointsExcludingHands);
   }

   @Override
   public void setJointAngles(RobotSide side, LimbName limb, double[] q)
   {
      int i = 0;
      if (limb == LimbName.ARM)
      {
         for (OneDoFJointBasics joint : armJointIDsList.get(side))
         {
            joint.setQ(q[i]);
            i++;
         }
      }
      else if (limb == LimbName.LEG)
      {
         for (OneDoFJointBasics jnt : legJointIDsList.get(side))
         {
            jnt.setQ(q[i]);
            i++;
         }
      }
   }

//   public void getJointAngles(RobotSide side, LimbName limb, double[] q)
//   {
//      int i = 0;
//      if (limb == LimbName.ARM)
//      {
//         for (OneDoFJoint jnt : armJointIDsList.get(side))
//         {
//            q[i] = jnt.getQ();
//            i++;
//         }
//      }
//      else if (limb == LimbName.LEG)
//      {
//         for (OneDoFJoint jnt : legJointIDsList.get(side))
//         {
//            q[i] = jnt.getQ();
//            i++;
//         }
//      }
//   }

//   public void copyJointAnglesAcrossSide(RobotSide sourceSide, LimbName limb)
//   {
//      SideDependentList<ArrayList<OneDoFJoint>> joints;
//      if (limb == LimbName.ARM)
//      {
//         joints = armJointIDsList;
//      }
//      else if (limb == LimbName.LEG)
//      {
//         joints = legJointIDsList;
//      }
//      else
//      {
//         return;
//      }
//
//      ArrayList<OneDoFJoint> sourceJoints = joints.get(sourceSide);
//      ArrayList<OneDoFJoint> destJoints = joints.get(sourceSide.getOppositeSide());
//
//      for (int i = 0; i < sourceJoints.size(); i++)
//      {
//         destJoints.get(i).setQ(sourceJoints.get(i).getQ());
//      }
//   }

   /** {@inheritDoc} */
   @Override
   public OneDoFJointBasics[] getControllableOneDoFJoints()
   {
      return oneDoFJointsExcludingHands.toArray(new OneDoFJointBasics[oneDoFJointsExcludingHands.size()]);
   }

   /** {@inheritDoc} */
   @Override
   public void getControllableOneDoFJoints(List<OneDoFJointBasics> oneDoFJointsToPack)
   {
      oneDoFJointsToPack.addAll(oneDoFJointsExcludingHands);
   }

   /** {@inheritDoc} */
   @Override
   public MovingReferenceFrame getFrameAfterLegJoint(RobotSide robotSide, LegJointName legJointName)
   {
      return getLegJoint(robotSide, legJointName).getFrameAfterJoint();
   }

   /** {@inheritDoc} */
   @Override
   public OneDoFJointBasics getLegJoint(RobotSide robotSide, LegJointName legJointName)
   {
      return legJointLists.get(robotSide).get(legJointName);
   }

   /** {@inheritDoc} */
   @Override
   public RigidBodyBasics getPelvis()
   {
      return getRootBody();
   }

   /** {@inheritDoc} */
   @Override
   public RigidBodyBasics getChest()
   {
      return chest;
   }


   /** {@inheritDoc} */
   @Override
   public OneDoFJointBasics getArmJoint(RobotSide robotSide, ArmJointName armJointName)
   {
      return armJointLists.get(robotSide).get(armJointName);
   }

   /** {@inheritDoc} */
   @Override
   public RigidBodyBasics getFoot(RobotSide robotSide)
   {
      return getEndEffector(robotSide, LimbName.LEG);
   }

   /** {@inheritDoc} */
   @Override
   public RigidBodyBasics getHand(RobotSide robotSide)
   {
      return getEndEffector(robotSide, LimbName.ARM);
   }

   /** {@inheritDoc} */
   @Override
   public RigidBodyBasics getEndEffector(RobotSide robotSide, LimbName limbName)
   {
      switch (limbName)
      {
      case ARM:
         return hands.get(robotSide);
      case LEG:
         return feet.get(robotSide);
      default:
         throw new RuntimeException("Unknown end effector");
      }
   }

   /** {@inheritDoc} */
   @Override
   public MovingReferenceFrame getEndEffectorFrame(RobotSide robotSide, LimbName limbName)
   {
      return getEndEffector(robotSide, limbName).getParentJoint().getFrameAfterJoint();
   }

   /** {@inheritDoc} */
   @Override
   public MovingReferenceFrame getSoleFrame(RobotSide robotSide)
   {
      return soleFrames.get(robotSide);
   }

   @Override
   public SideDependentList<MovingReferenceFrame> getSoleFrames()
   {
      return soleFrames;
   }

   /** {@inheritDoc} */
   @Override
   public MovingReferenceFrame getHandControlFrame(RobotSide robotSide)
   {
      return handControlFrames.get(robotSide);
   }

   //   private void copyAllJointsButKeepOneFootFixed( OneDoFJoint[] jointsToCopyFrom, RobotSide sideOfSole )
   //   {
   //      HashMap<String, Double> newJointAngles = new HashMap<String, Double>();
   //      int numJoints = getOneDoFJoints().length;
   //
   //      for (int i=0; i<numJoints; i++)  {
   //         newJointAngles.put( jointsToCopyFrom[i].getName() ,  jointsToCopyFrom[i].getQ() );
   //      }
   //      updateJointsAngleButKeepOneFootFixed( newJointAngles, sideOfSole);
   //   }
   //
   //   private void copyAllJointsButKeepOneFootFixed( ArrayList<OneDoFJoint> jointsToCopyFrom, RobotSide sideOfSole )
   //   {
   //      HashMap<String, Double> newJointAngles = new HashMap<String, Double>();
   //
   //      for (int i=0; i<jointsToCopyFrom.size(); i++)  {
   //         newJointAngles.put( jointsToCopyFrom.get(i).getName() ,  jointsToCopyFrom.get(i).getQ() );
   //      }
   //      updateJointsAngleButKeepOneFootFixed( newJointAngles, sideOfSole);
   //   }
   //
   //   private void updateJointsAngleButKeepOneFootFixed( Map<String, Double> jointsAngles, RobotSide sideOfSole )
   //   {
   //      updateJointsStateButKeepOneFootFixed( jointsAngles,null, sideOfSole);
   //   }
   //
   //   private void updateJointsStateButKeepOneFootFixed( Map<String, Double> jointsAngles, Map<String, Double> jointsVelocities, RobotSide sideOfSole )
   //   {
   //      // next line of code must be executed BEFORE modifying the model
   //      RigidBodyTransform worldToFoot  = getSoleFrame(sideOfSole).getTransformToWorldFrame();
   //
   //      if( jointsAngles != null)
   //      {
   //         for ( Map.Entry<String, Double> entry: jointsAngles.entrySet() )
   //         {
   //             getOneDoFJointByName( entry.getKey() ).setQ( entry.getValue() );
   //         }
   //      }
   //
   //      if( jointsVelocities != null)
   //      {
   //         for ( Map.Entry<String, Double> entry: jointsVelocities.entrySet() )
   //         {
   //             getOneDoFJointByName( entry.getKey() ).setQd( entry.getValue() );
   //         }
   //      }
   //
   //      this.updateFrames();
   //
   //      ReferenceFrame footFrame   = getSoleFrame(sideOfSole);
   //      ReferenceFrame pelvisFrame = getRootJoint().getFrameAfterJoint();
   //
   //      RigidBodyTransform footToPelvis = pelvisFrame.getTransformToDesiredFrame( footFrame );
   //
   //      RigidBodyTransform worldToPelvis = new RigidBodyTransform();
   //      worldToPelvis.multiply( worldToFoot, footToPelvis );
   //
   //      getRootJoint().setPositionAndRotation(worldToPelvis);
   //
   //      this.updateFrames();
   //   }

   @Override
   protected void mapRigidBody(JointDescription joint, OneDoFJointBasics inverseDynamicsJoint, RigidBodyBasics rigidBody)
   {
      initializeLists();

      LinkDescription childLink = joint.getLink();
      HumanoidJointNameMap humanoidJointNameMap = (HumanoidJointNameMap) sdfJointNameMap;

      if (rigidBody.getName().equals(humanoidJointNameMap.getChestName()))
      {
         chest = rigidBody;
      }

      if (rigidBody.getName().equals(humanoidJointNameMap.getHeadName()))
      {
         head = rigidBody;
      }
      ImmutablePair<RobotSide, LimbName> limbSideAndName = humanoidJointNameMap.getLimbName(childLink.getName());
      if (limbSideAndName != null)
      {
         RobotSide limbSide = limbSideAndName.getLeft();
         LimbName limbName = limbSideAndName.getRight();
         switch (limbName)
         {
         case ARM:
            hands.put(limbSide, rigidBody);
            break;
         case LEG:
            feet.put(limbSide, rigidBody);
            break;
         }
      }

      JointRole jointRole = sdfJointNameMap.getJointRole(joint.getName());
      if (jointRole != null)
      {
         switch (jointRole)
         {
         //TODO: Should armJointLists use legJoingName.first or armJointName.first?? looks backwards
         case ARM:
            ImmutablePair<RobotSide, ArmJointName> armJointName = humanoidJointNameMap.getArmJointName(joint.getName());
            armJointLists.get(armJointName.getLeft()).put(armJointName.getRight(), inverseDynamicsJoint);
            armJointIDsList.get(armJointName.getLeft()).add(inverseDynamicsJoint);
            break;
         case LEG:
            ImmutablePair<RobotSide, LegJointName> legJointName = humanoidJointNameMap.getLegJointName(joint.getName());
            legJointLists.get(legJointName.getLeft()).put(legJointName.getRight(), inverseDynamicsJoint);
            legJointIDsList.get(legJointName.getLeft()).add(inverseDynamicsJoint);
            break;
         case NECK:
            NeckJointName neckJointName = humanoidJointNameMap.getNeckJointName(joint.getName());
            neckJoints.put(neckJointName, inverseDynamicsJoint);
            break;
         case SPINE:
            SpineJointName spineJointName = humanoidJointNameMap.getSpineJointName(joint.getName());
            spineJoints.put(spineJointName, inverseDynamicsJoint);
            break;
         }
      }
   }

   private void initializeLists()
   {
      if (!initialized)
      {
         armJointLists = SideDependentList.createListOfEnumMaps(ArmJointName.class);
         legJointLists = SideDependentList.createListOfEnumMaps(LegJointName.class);

         armJointIDsList = SideDependentList.createListOfArrayLists();
         legJointIDsList = SideDependentList.createListOfArrayLists();

         feet = new SideDependentList<RigidBodyBasics>();
         hands = new SideDependentList<RigidBodyBasics>();
         initialized = true;
      }
   }

   private void getAllJointsExcludingHands(ArrayList<OneDoFJointBasics> jointsToPack)
   {
      getOneDoFJoints(jointsToPack);
      for (RobotSide robotSide : RobotSide.values)
      {
         RigidBodyBasics hand = getHand(robotSide);
         if (hand != null)
         {
            OneDoFJointBasics[] fingerJoints = MultiBodySystemTools.filterJoints(MultiBodySystemTools.collectSubtreeJoints(hand), OneDoFJointBasics.class);
            for (OneDoFJointBasics fingerJoint : fingerJoints)
            {
               jointsToPack.remove(fingerJoint);
            }
         }
      }
   }

   @Override
   public RobotSide[] getRobotSegments()
   {
      return RobotSide.values;
   }
}


<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/statistics/PlannerStatistics.java<|end_filename|>
package us.ihmc.pathPlanning.statistics;

public interface PlannerStatistics<T extends PlannerStatistics<T>>
{
   StatisticsType getStatisticsType();

   void set(T other);
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/compressor/LogFileCompressor.java<|end_filename|>
package us.ihmc.robotDataVisualizer.compressor;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.PrintStream;
import java.nio.ByteBuffer;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.atomic.AtomicInteger;

import org.tukaani.xz.LZMA2Options;
import org.tukaani.xz.XZOutputStream;

import com.google.common.hash.Hashing;
import com.google.common.io.Files;

import us.ihmc.robotDataLogger.LogProperties;
import us.ihmc.robotDataLogger.logger.LogPropertiesReader;
import us.ihmc.robotDataLogger.logger.YoVariableLogReader;
import us.ihmc.robotDataLogger.logger.YoVariableLoggerListener;
import us.ihmc.robotDataVisualizer.logger.util.CustomProgressMonitor;
import us.ihmc.robotDataVisualizer.logger.util.ProgressMonitorInterface;

public class LogFileCompressor extends YoVariableLogReader
{
   public static final int NUMBER_OF_THREADS = 8;
   public static final int MAX_BUFFER_SIZE_IN_BYTE = 1000000000;
   public static final int COMPRESSION_LEVEL = 3;

   private final PrintStream out;
   private final File targetDirectory;

   private int bufferedElements;
   private volatile int compressedDataFiles = 0;

   private final ExecutorService threadPool = Executors.newFixedThreadPool(NUMBER_OF_THREADS);
   private final LinkedBlockingQueue<DataBuffer> availableBuffers = new LinkedBlockingQueue<>();
   private final LinkedBlockingQueue<DataBuffer> writableBuffers = new LinkedBlockingQueue<>();

   private final AtomicInteger progress = new AtomicInteger(0);
   private final ProgressMonitorInterface progressMonitor;

   public LogFileCompressor(File logDirectory, File targetDirectory, LogProperties logProperties, ProgressMonitorInterface progressMonitor) throws IOException
   {
      super(logDirectory, logProperties);
      this.targetDirectory = targetDirectory;

      initialize();

      int maxBufferedElements = MAX_BUFFER_SIZE_IN_BYTE / 8;

      bufferedElements = maxBufferedElements / getNumberOfVariables();

      this.progressMonitor = progressMonitor;
      progressMonitor.initialize("Log file compression", null, 0, getNumberOfEntries() / bufferedElements);
      out = progressMonitor.getPrintStream();

      out.println("Reading " + getNumberOfEntries() + " entries.");
      out.println("Found " + getNumberOfVariables() + " variables.");
      out.println("Writing " + bufferedElements + " data points at a time");
      out.println("Total space " + (getNumberOfEntries() * getNumberOfVariables() * 8) / (1024 * 1024) + " MB.");

      if (bufferedElements > getNumberOfEntries())
      {
         out.println("Reading all elements into buffer");
         bufferedElements = getNumberOfEntries();
      }

      createBuffers();

      decompressData();

      copyMetaData(targetDirectory);

      writeCompressionProperties(targetDirectory);

      close();
      progressMonitor.close();
   }

   private void incrementProgressMonitor()
   {
      progressMonitor.setProgress(progress.incrementAndGet());
   }

   private void createBuffers()
   {
      for (int i = 0; i < NUMBER_OF_THREADS; i++)
      {
         try
         {
            availableBuffers.put(new DataBuffer(getNumberOfVariables(), bufferedElements * 8));
         }
         catch (InterruptedException e)
         {
            throw new RuntimeException(e);
         }
      }
   }

   private void writeCompressionProperties(File targetDirectory) throws IOException, FileNotFoundException
   {
      CompressionProperties properties = new CompressionProperties();
      properties.setNumberOfBufferedElements(bufferedElements);
      properties.setCompressedDataFiles(compressedDataFiles);
      setChecksums(properties);

      FileOutputStream propertiesFile = new FileOutputStream(new File(targetDirectory, "robotData.compressed"));
      properties.store(propertiesFile, "Description of compression properties");
      propertiesFile.close();
   }

   private void setChecksums(CompressionProperties properties) throws IOException
   {
      out.println("Saving checksums to file");
      File logdata = new File(logDirectory, logProperties.getVariables().getDataAsString());
      properties.setDataChecksum(Files.hash(logdata, Hashing.sha1()).toString());

      File index = new File(logDirectory, logProperties.getVariables().getDataAsString());
      properties.setTimestampChecksum(Files.hash(index, Hashing.sha1()).toString());
      out.println("Saved checksums");
   }

   private void decompressData() throws IOException
   {

      int fileIndex = 0;
      int currentFileEntry = 0;

      DataBuffer buffer = getFreeDataBuffer(fileIndex);

      for (int i = 0; i < getNumberOfEntries(); i++)
      {
         ByteBuffer dataLine = readData(i);

         for (int e = 0; e < getNumberOfVariables(); e++)
         {
            buffer.data[e][currentFileEntry * 8] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 1] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 2] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 3] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 4] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 5] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 6] = dataLine.get();
            buffer.data[e][currentFileEntry * 8 + 7] = dataLine.get();
         }

         currentFileEntry++;
         if (currentFileEntry >= bufferedElements)
         {
            submitBuffer(buffer, currentFileEntry);
            fileIndex++;
            buffer = getFreeDataBuffer(fileIndex);
            currentFileEntry = 0;
         }
      }
      if(currentFileEntry != 0)
      {
         submitBuffer(buffer, currentFileEntry);
      }

      out.println("All data read");
      threadPool.shutdown();

   }

   private void submitBuffer(DataBuffer buffer, int numberOfEntries)
   {
      try
      {
         buffer.length = numberOfEntries;
         writableBuffers.put(buffer);
         threadPool.execute(new DataWriter());
         compressedDataFiles++;
      }
      catch (InterruptedException e)
      {
         throw new RuntimeException(e);
      }
   }

   private DataBuffer getFreeDataBuffer(int currentFile)
   {
      try
      {
         DataBuffer dataBuffers = availableBuffers.take();

         dataBuffers.fileID = currentFile;
         return dataBuffers;
      }
      catch (InterruptedException e1)
      {
         throw new RuntimeException(e1);
      }
   }

   private class DataBuffer
   {
      private final byte[][] data;
      private int fileID;
      private int length;

      public DataBuffer(int variables, int elements)
      {
         data = new byte[variables][elements];
      }
   }

   private class DataWriter implements Runnable
   {

      @Override
      public void run()
      {
         try
         {
            DataBuffer buffer = writableBuffers.take();
            int currentFile = buffer.fileID;
            byte[][] dataBuffers = buffer.data;

            FileOutputStream logOutputStream = new FileOutputStream(new File(targetDirectory, "robotData." + currentFile + ".xz"));
            XZOutputStream xzOutputStream = new XZOutputStream(logOutputStream, new LZMA2Options(COMPRESSION_LEVEL));
            int writtenLength = 0;
            for (int i = 0; i < getNumberOfVariables(); i++)
            {
               writtenLength += buffer.length * 8;
               xzOutputStream.write(dataBuffers[i], 0, buffer.length * 8);
            }

            out.println("Wrote " + writtenLength / (1024 * 1024) + " MB to robotData." + currentFile + ".xz");
            xzOutputStream.close();
            logOutputStream.close();
            availableBuffers.put(buffer);
            incrementProgressMonitor();
         }
         catch (IOException | InterruptedException e)
         {
            e.printStackTrace();
         }

      }

   }

   public static void main(String[] args) throws IOException
   {
      //      File logDirectory = FileSelectionDialog.loadDirectoryWithFileNamed(YoVariableLoggerListener.propertyFile);
      File logDirectory = new File("/home/jesper/robotLogs/20160520_130501_Atlas_WalkOnStraightLinesFallStanding_Step/");
      File targetDirectory = new File("/home/jesper/scratch/compressed/");
      if (logDirectory != null)
      {
         LogPropertiesReader logProperties = new LogPropertiesReader(new File(logDirectory, YoVariableLoggerListener.propertyFile));

         new LogFileCompressor(logDirectory, targetDirectory, logProperties, new CustomProgressMonitor());
      }
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/HumanoidFloatingRootJointRobot.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.robotics.partNames.HumanoidJointNameMap;
import us.ihmc.robotics.robotDescription.RobotDescription;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;
import us.ihmc.simulationconstructionset.FloatingRootJointRobot;
import us.ihmc.simulationconstructionset.GroundContactPoint;
import us.ihmc.simulationconstructionset.Joint;

public class HumanoidFloatingRootJointRobot extends FloatingRootJointRobot
{
   private final SideDependentList<String> jointsBeforeFeet = new SideDependentList<String>();

   private final SideDependentList<ArrayList<GroundContactPoint>> footGroundContactPoints = new SideDependentList<ArrayList<GroundContactPoint>>();
   private final SideDependentList<ArrayList<GroundContactPoint>> handGroundContactPoints = new SideDependentList<ArrayList<GroundContactPoint>>();

   public HumanoidFloatingRootJointRobot(RobotDescription robotDescription, HumanoidJointNameMap sdfJointNameMap)
   {
      this(robotDescription, sdfJointNameMap, true, true);
   }

   public HumanoidFloatingRootJointRobot(RobotDescription robotDescription, HumanoidJointNameMap sdfJointNameMap, boolean enableDamping, boolean enableJointTorqueAndVelocityLimits)
   {
      super(robotDescription, enableDamping, enableJointTorqueAndVelocityLimits && (sdfJointNameMap == null || sdfJointNameMap.isTorqueVelocityLimitsEnabled()));

      for (RobotSide robotSide : RobotSide.values)
      {
         footGroundContactPoints.put(robotSide, new ArrayList<GroundContactPoint>());
         handGroundContactPoints.put(robotSide, new ArrayList<GroundContactPoint>());
         if(sdfJointNameMap != null)
         {
            jointsBeforeFeet.put(robotSide, sdfJointNameMap.getJointBeforeFootName(robotSide));
         }
      }

      for(Joint joint : getOneDegreeOfFreedomJoints())
      {
         for(RobotSide robotSide : RobotSide.values)
         {
            ArrayList<GroundContactPoint> contactPointsForJoint = getGroundContactPointsOnJoint(joint);

            if(contactPointsForJoint != null)
            {
               String jointName = joint.getName();
               if (jointName.equals(sdfJointNameMap.getJointBeforeFootName(robotSide)))
               {
                  footGroundContactPoints.get(robotSide).addAll(contactPointsForJoint);
               }
               else if (jointName.equals(sdfJointNameMap.getJointBeforeHandName(robotSide)))
               {
                  handGroundContactPoints.get(robotSide).addAll(contactPointsForJoint);
               }
            }
         }
      }
   }

   public List<GroundContactPoint> getFootGroundContactPoints(RobotSide robotSide)
   {
      return footGroundContactPoints.get(robotSide);
   }

   public SideDependentList<String> getJointNamesBeforeFeet()
   {
      return jointsBeforeFeet;
   }

}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/NumericSpinner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import java.util.List;
import java.util.function.UnaryOperator;

import javafx.application.Platform;
import javafx.beans.value.ChangeListener;
import javafx.scene.control.ContextMenu;
import javafx.scene.control.MenuItem;
import javafx.scene.control.Spinner;
import javafx.scene.control.SpinnerValueFactory;
import javafx.scene.control.TextFormatter;
import javafx.scene.control.TextFormatter.Change;
import javafx.util.StringConverter;
import us.ihmc.parameterTuner.ParameterTuningTools;

public abstract class NumericSpinner<T extends Number> extends Spinner<T>
{
   public NumericSpinner(SpinnerValueFactory<T> valueFactory)
   {
      super(valueFactory);

      setEditable(true);
      setPrefWidth(120.0);
      setMinWidth(80.0);

      // Add formatter that will prevent any input that is not a number with a few exceptions (e.g. '-').
      UnaryOperator<TextFormatter.Change> filter = new UnaryOperator<TextFormatter.Change>()
      {
         @Override
         public Change apply(Change change)
         {
            String newText = change.getControlNewText();
            if (newText.isEmpty() || newText.equals("-"))
            {
               // So the user can delete all text or start with a minus.
               return change;
            }
            if (isValidString(newText))
            {
               // If the text parses to a number it id fine.
               return change;
            }
            if (change.isDeleted())
            {
               // In case the textbox contained a special string such as "Infinity" the delete action should clear the editor.
               Platform.runLater(() -> getEditor().setText(""));
            }
            return null;
         }
      };
      getEditor().setTextFormatter(new TextFormatter<>(filter));

      // Add a custom converter that will allow inputing allowed text (e.g. '-') without exception.
      getValueFactory().setConverter(new StringConverter<T>()
      {
         @Override
         public String toString(T number)
         {
            return convertNumberToString(number);
         }

         @Override
         public T fromString(String string)
         {
            try
            {
               return convertStringToNumber(string);
            }
            catch (Exception e)
            {
               // If the text does not parse (e.g. it is empty or just a minus sign) remember the last value.
               return getValue();
            }
         }
      });

      // Update value when enter is pressed or focus is lost.
      ParameterTuningTools.addThreadSafeListeners(getEditor(), () -> {
         T newNumber = getValueFactory().getConverter().fromString(getEditor().getText());
         setValue(newNumber);
      });

      // Add options for strings that are not allowed to type such as "Infinity" for Double.
      List<MenuItem> contextMenuOptions = getContextMenuOptions();
      if (contextMenuOptions != null)
      {
         ContextMenu contextMenu = new ContextMenu();
         for (MenuItem menuItem : contextMenuOptions)
         {
            contextMenu.getItems().add(menuItem);
         }
         getEditor().setContextMenu(contextMenu);
      }
   }

   public void setValue(T newValue)
   {
      getValueFactory().setValue(newValue);
      getEditor().setText(getValueFactory().getConverter().toString(getValue()));
   }

   public String getValueAsText()
   {
      return convertNumberToString(getValue());
   }

   public void addListener(ChangeListener<T> listener)
   {
      valueProperty().addListener(listener);
   }

   public boolean isValidString(String numberString)
   {
      try
      {
         convertStringToNumber(numberString);
         return true;
      }
      catch (Exception e)
      {
         return false;
      }
   }

   public abstract T convertStringToNumber(String numberString);

   public abstract String convertNumberToString(T number);

   public List<MenuItem> getContextMenuOptions()
   {
      return null;
   }

   public NumericSpinner<T> createLinkedDuplicate()
   {
      return new NumericSpinner<T>(getValueFactory())
      {
         @Override
         public List<MenuItem> getContextMenuOptions()
         {
            return NumericSpinner.this.getContextMenuOptions();
         }

         @Override
         public T convertStringToNumber(String numberString)
         {
            return NumericSpinner.this.convertStringToNumber(numberString);
         }

         @Override
         public String convertNumberToString(T number)
         {
            return NumericSpinner.this.convertNumberToString(number);
         }
      };
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/microphone/DrillDetectionAlgorithmSimple.java<|end_filename|>
package us.ihmc.avatar.sensors.microphone;

import us.ihmc.commons.Conversions;
import us.ihmc.simulationconstructionset.gui.BodePlotConstructor;

public class DrillDetectionAlgorithmSimple extends DrillDetectionAlgorithm
{
   private static final double decibelsOnThreshold = 80.0; //dB
   private static final double frequencyBandLowerBound = 7000; //Hz
   private static final double frequencyBandUpperBound = 9000; //Hz

   @Override
   public DrillDetectionResult isDrillOn(byte[] audioBytes, int size)
   {
      int nlengthInSamples = size / 2;
      int[] audioData = new int[nlengthInSamples];

      //Little-Endian code
      for (int i = 0; i < nlengthInSamples; i++)
      {
         int LSB = (int) audioBytes[2 * i];     /* First byte is LSB (low order) */
         int MSB = (int) audioBytes[2 * i + 1]; /* Second byte is MSB (high order) */
         audioData[i] = (MSB << 8) | (LSB & 0xFF);
      }

      double[] input = new double[audioData.length];
      double[] time = new double[audioData.length];
      for (int i = 0; i < audioData.length; i++)
      {
         input[i] = (double) audioData[i];
         time[i] = (double) i / getSampleRate();
      }

      double[][] fftData = BodePlotConstructor.computeFreqMagPhase(time, input);
      double[] frequency = fftData[0];
      double[] magnitude = new double[fftData[1].length];
      for (int i = 0; i < fftData[1].length; i++)
      {
         magnitude[i] = Conversions.amplitudeToDecibels(fftData[1][i]);
      }

      //Peak detection math
      int frequencyBandLowerBoundIndex = 0;
      int frequencyBandUpperBoundIndex = 0;

      for (int index = 0; index < frequency.length; index++)
      {
         if (frequency[index] <= frequencyBandLowerBound)
         {
            frequencyBandLowerBoundIndex = index;
         }

         if (frequency[index] <= frequencyBandUpperBound)
         {
            frequencyBandUpperBoundIndex = index;
         }
      }

      double bandAverageMag = 0;
      int rangeSize = frequencyBandUpperBoundIndex - frequencyBandLowerBoundIndex;

      for (int index = 0; index < rangeSize; index++)
      {
         bandAverageMag += magnitude[frequencyBandLowerBoundIndex + index];
      }
      bandAverageMag /= rangeSize;

      DrillDetectionResult result = new DrillDetectionResult();
      result.isOn = (bandAverageMag >= decibelsOnThreshold);
      result.averageValues = new double[] { bandAverageMag };
      result.bodeData = getBodeData(time, input);

      return result;
   }

   @Override
   public int getNumReturnedBands()
   {
      return 1;
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasElectricMotorAutoEnableFlagPacket.java<|end_filename|>
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;

/**
       * Atlas specific message.
       */
public class AtlasElectricMotorAutoEnableFlagPacket extends Packet<AtlasElectricMotorAutoEnableFlagPacket> implements Settable<AtlasElectricMotorAutoEnableFlagPacket>, EpsilonComparable<AtlasElectricMotorAutoEnableFlagPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean should_auto_enable_;

   public AtlasElectricMotorAutoEnableFlagPacket()
   {
   }

   public AtlasElectricMotorAutoEnableFlagPacket(AtlasElectricMotorAutoEnableFlagPacket other)
   {
      this();
      set(other);
   }

   public void set(AtlasElectricMotorAutoEnableFlagPacket other)
   {
      sequence_id_ = other.sequence_id_;

      should_auto_enable_ = other.should_auto_enable_;

   }

   /**
            * 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 void setShouldAutoEnable(boolean should_auto_enable)
   {
      should_auto_enable_ = should_auto_enable;
   }
   public boolean getShouldAutoEnable()
   {
      return should_auto_enable_;
   }


   public static Supplier<AtlasElectricMotorAutoEnableFlagPacketPubSubType> getPubSubType()
   {
      return AtlasElectricMotorAutoEnableFlagPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AtlasElectricMotorAutoEnableFlagPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AtlasElectricMotorAutoEnableFlagPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.should_auto_enable_, other.should_auto_enable_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AtlasElectricMotorAutoEnableFlagPacket)) return false;

      AtlasElectricMotorAutoEnableFlagPacket otherMyClass = (AtlasElectricMotorAutoEnableFlagPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.should_auto_enable_ != otherMyClass.should_auto_enable_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AtlasElectricMotorAutoEnableFlagPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("should_auto_enable=");
      builder.append(this.should_auto_enable_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/walking/FootstepStatus.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.walking;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;

public enum FootstepStatus
{
   @RosEnumValueDocumentation(documentation = "execution of a footstep has begun. actualFootPositionInWorld and actualFootOrientationInWorld should be ignored in this state")
   STARTED, @RosEnumValueDocumentation(documentation = "a footstep is completed")
   COMPLETED;

   public static final FootstepStatus[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static FootstepStatus fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/partNames/NeckJointName.java<|end_filename|>
package us.ihmc.robotics.partNames;

import org.apache.commons.lang3.StringUtils;

public enum NeckJointName
{
   PROXIMAL_NECK_YAW, PROXIMAL_NECK_PITCH, PROXIMAL_NECK_ROLL, DISTAL_NECK_YAW, DISTAL_NECK_PITCH, DISTAL_NECK_ROLL;

   /**
    * @deprecated Do not iterate over these values!
    */
   public static final NeckJointName[] values = values();
   
   public String getUnderBarName()
   {
      switch (this)
      {
      case PROXIMAL_NECK_YAW:
         return "proximal_neck_yaw";
      case PROXIMAL_NECK_PITCH:
         return "proximal_neck_pitch";
      case PROXIMAL_NECK_ROLL:
         return "proximal_neck_roll";
      case DISTAL_NECK_YAW:
         return "distal_neck_yaw";
      case DISTAL_NECK_PITCH:
         return "distal_neck_pitch";
      case DISTAL_NECK_ROLL:
         return "distal_neck_roll";
      default:
         throw new RuntimeException("Should not get to here");
      }
   }
   
   public String getCamelCaseName()
   {
      switch (this)
      {
      case PROXIMAL_NECK_YAW:
         return "proximalNeckYaw";
      case PROXIMAL_NECK_PITCH:
         return "proximalNeckPitch";
      case PROXIMAL_NECK_ROLL:
         return "proximalNeckRoll";
      case DISTAL_NECK_YAW:
         return "distalNeckYaw";
      case DISTAL_NECK_PITCH:
         return "distalNeckPitch";
      case DISTAL_NECK_ROLL:
         return "distalNeckRoll";
      default:
         throw new RuntimeException("Should not get to here");
      }
   }
   
   /**
    * @deprecated Use getCamelCaseName() instead.
    */
   public String getCamelCaseNameForStartOfExpression()
   {
      return getCamelCaseName();
   }
   
   public String getPascalCaseName()
   {
      return StringUtils.capitalize(getCamelCaseName());
   }

   /**
    * @deprecated Use getPascalCaseName() instead.
    */
   public String getCamelCaseNameForMiddleOfExpression()
   {
      return getPascalCaseName();
   }

   @Override
   public String toString()
   {
      return getPascalCaseName();
   }
}


<|start_filename|>robotiq-hand-drivers/src/main/java/us/ihmc/robotiq/data/RobotiqHandSensorData.java<|end_filename|>
package us.ihmc.robotiq.data;

import static us.ihmc.robotiq.model.RobotiqHandModel.RobotiqHandJointNameMinimal.*;

import us.ihmc.avatar.handControl.packetsAndConsumers.HandSensorData;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotiq.communication.RobotiqReadResponse;
import us.ihmc.robotiq.communication.registers.FaultStatusRegister.gFLT;
import us.ihmc.robotiq.communication.registers.GripperStatusRegister.gACT;
import us.ihmc.robotiq.model.RobotiqHandModel.RobotiqHandJointNameMinimal;

public class RobotiqHandSensorData implements HandSensorData
{
   private final double[] jointAngles = new double[RobotiqHandJointNameMinimal.values.length];
   private final RobotiqReadResponse response = new RobotiqReadResponse();

   private boolean connected;

   public void update(RobotiqReadResponse response, boolean connected)
   {
      this.response.setAll(response);
      this.connected = connected;
   }
   
   public boolean hasError()
   {
      return !response.getFaultStatus().getGflt().equals(gFLT.NO_FAULT);
   }
   
   public gFLT getFaultStatus()
   {
      return response.getFaultStatus().getGflt();
   }
   
   @Override
   public double[] getFingerJointAngles(RobotSide robotSide)
   {
      jointAngles[FINGER_MIDDLE_JOINT_1.getIndex(robotSide)] = (response.getFingerAPosition().getRegisterValue() & 0xFF) * (25.0/72) * Math.PI / 0xFF; //62.5 degrees
      jointAngles[FINGER_MIDDLE_JOINT_2.getIndex(robotSide)] = (response.getFingerAPosition().getRegisterValue() & 0xFF) * (0.5) * Math.PI / 0xFF; //90 degrees
      jointAngles[FINGER_MIDDLE_JOINT_3.getIndex(robotSide)] = 0.0;
      
      jointAngles[PALM_FINGER_1_JOINT.getIndex(robotSide)] = -((double)(response.getScissorPosition().getRegisterValue() & 0xFF) * (8.0/45) / 0xFF - (4.0/45)) * Math.PI; //32 degrees
      jointAngles[FINGER_1_JOINT_1.getIndex(robotSide)]    =   (double)(response.getFingerBPosition().getRegisterValue() & 0xFF) * (25.0/72) * Math.PI / 0xFF; //62.5 degrees
      jointAngles[FINGER_1_JOINT_2.getIndex(robotSide)]    =   (double)(response.getFingerBPosition().getRegisterValue() & 0xFF) * (0.5) * Math.PI / 0xFF; //90 degrees
      jointAngles[FINGER_1_JOINT_3.getIndex(robotSide)]    = 0.0;
      
      jointAngles[PALM_FINGER_2_JOINT.getIndex(robotSide)] = ((double)(response.getScissorPosition().getRegisterValue() & 0xFF) * (8.0/45) / 0xFF - (4.0/45)) * Math.PI; //32 degrees
      jointAngles[FINGER_2_JOINT_1.getIndex(robotSide)]    =  (double)(response.getFingerCPosition().getRegisterValue() & 0xFF) * (25.0/72) * Math.PI / 0xFF; //62.5 degrees
      jointAngles[FINGER_2_JOINT_2.getIndex(robotSide)]    =  (double)(response.getFingerCPosition().getRegisterValue() & 0xFF) * (0.5) * Math.PI / 0xFF; //90 degrees
      jointAngles[FINGER_2_JOINT_3.getIndex(robotSide)]    = 0.0;
      
      return jointAngles;
   }

   @Override
   public boolean isCalibrated()
   {
      return response.getGripperStatus().getGact().equals(gACT.GRIPPER_ACTIVATION);
   }

   @Override
   public boolean isConnected()
   {
      return connected;
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BDIBehaviorCommandPacket.java<|end_filename|>
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;

/**
       * Atlas specific message.
       */
public class BDIBehaviorCommandPacket extends Packet<BDIBehaviorCommandPacket> implements Settable<BDIBehaviorCommandPacket>, EpsilonComparable<BDIBehaviorCommandPacket>
{
   public static final byte NONE = (byte) 0;
   public static final byte FREEZE = (byte) 1;
   public static final byte STAND_PREP = (byte) 2;
   public static final byte STAND = (byte) 3;
   public static final byte WALK = (byte) 4;
   public static final byte STEP = (byte) 5;
   public static final byte MANIPULATE = (byte) 6;
   public static final byte USER = (byte) 7;
   public static final byte CALIBRATE = (byte) 8;
   public static final byte SOFT_STOP = (byte) 9;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte atlas_bdi_robot_behavior_ = (byte) 255;
   public boolean stop_;

   public BDIBehaviorCommandPacket()
   {
   }

   public BDIBehaviorCommandPacket(BDIBehaviorCommandPacket other)
   {
      this();
      set(other);
   }

   public void set(BDIBehaviorCommandPacket other)
   {
      sequence_id_ = other.sequence_id_;

      atlas_bdi_robot_behavior_ = other.atlas_bdi_robot_behavior_;

      stop_ = other.stop_;

   }

   /**
            * 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 void setAtlasBdiRobotBehavior(byte atlas_bdi_robot_behavior)
   {
      atlas_bdi_robot_behavior_ = atlas_bdi_robot_behavior;
   }
   public byte getAtlasBdiRobotBehavior()
   {
      return atlas_bdi_robot_behavior_;
   }

   public void setStop(boolean stop)
   {
      stop_ = stop;
   }
   public boolean getStop()
   {
      return stop_;
   }


   public static Supplier<BDIBehaviorCommandPacketPubSubType> getPubSubType()
   {
      return BDIBehaviorCommandPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return BDIBehaviorCommandPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(BDIBehaviorCommandPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.atlas_bdi_robot_behavior_, other.atlas_bdi_robot_behavior_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.stop_, other.stop_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof BDIBehaviorCommandPacket)) return false;

      BDIBehaviorCommandPacket otherMyClass = (BDIBehaviorCommandPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.atlas_bdi_robot_behavior_ != otherMyClass.atlas_bdi_robot_behavior_) return false;

      if(this.stop_ != otherMyClass.stop_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("BDIBehaviorCommandPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("atlas_bdi_robot_behavior=");
      builder.append(this.atlas_bdi_robot_behavior_);      builder.append(", ");
      builder.append("stop=");
      builder.append(this.stop_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/planarRegionEnvironments/VaryingStairsPlanarRegionEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.planarRegionEnvironments;

import us.ihmc.graphicsDescription.appearance.YoAppearance;

public class VaryingStairsPlanarRegionEnvironment extends PlanarRegionEnvironmentInterface
{
   public VaryingStairsPlanarRegionEnvironment(double startX, double startZ, double[] stepTreads, double[] stepRises)
   {
      generator.translate(startX, 0.0, startZ);
      
      for (int i = 0; i < stepTreads.length; i++)
      {
         generator.addCubeReferencedAtBottomNegativeXEdgeCenter(stepTreads[i], 1.0, -0.1);
         generator.translate(stepTreads[i] + 0.01, 0.0, stepRises[i]);
      }

      addPlanarRegionsToTerrain(YoAppearance.Grey());
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/linearAlgebra/DampedNullspaceCalculator.java<|end_filename|>
package us.ihmc.robotics.linearAlgebra;

public interface DampedNullspaceCalculator extends NullspaceCalculator
{
   public void setPseudoInverseAlpha(double alpha);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/DoneWithFinishableStateTransitionCondition.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

@Deprecated
public class DoneWithFinishableStateTransitionCondition implements StateTransitionCondition
{
   private final FinishableState<?> state;

   public DoneWithFinishableStateTransitionCondition(FinishableState<?> state)
   {
      this.state = state;
   }

   @Override
   public boolean checkCondition()
   {
      return state.isDone();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/externalcontroller/ExternalControllerTCPConnection.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.externalcontroller;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.ServerSocket;
import java.net.Socket;

class ExternalControllerTCPConnection
{
   DataInputStream in;
   DataOutputStream os;
   ServerSocket initialConnection;

   Socket clientSocket;
   int id;

   int portNumber = 8085;
   boolean bindRandomPort = false;
   boolean DEBUG = false;
   byte[] tempInBuffer = new byte[1024];
   byte[] tempOutBuffer = new byte[1024];
   int numBytesInDouble = Double.SIZE / 8;

   public ExternalControllerTCPConnection()
   {
      waitForClient();

      if (DEBUG)
         System.out.println("Connection " + id + " established with: " + clientSocket);

      try
      {
         in = new DataInputStream(clientSocket.getInputStream());

         os = new DataOutputStream(clientSocket.getOutputStream());
      }
      catch (IOException e)
      {
         if (DEBUG)
            System.out.println(e);
      }

      if (DEBUG)
         System.out.println("numBytesInDouble " + Double.toString(numBytesInDouble));
   }

   private void waitForClient()
   {
      // find an open port number
      boolean locked = false;
      while (!locked)
      {
         try
         {
            if (bindRandomPort)
            {
               portNumber = (int) Math.round((Math.random() * 8000) + 1000);
            }

            initialConnection = new ServerSocket(portNumber);
            System.out.println("Opening TCP connection on port " + portNumber);
            locked = true;
         }
         catch (IOException e)
         {
            System.out.println("Could not listen on port " + portNumber);
            System.out.println("opening new port");
            bindRandomPort = true;
         }
      }

      // wait for client to connect to make a new tcp connection
      System.out.println("waiting for external controller to connect on port " + portNumber);

      try
      {
         clientSocket = initialConnection.accept();
      }
      catch (IOException e)
      {
         if (DEBUG)
            System.out.println("Accept failed: " + portNumber);
         System.exit(-1);
      }

   }

   public void sendStringToExternalController(String sendMessage)
   {
      if (DEBUG)
         System.out.println("sending a string to the external controller");

      try
      {
         if (DEBUG)
            System.out.println(sendMessage);
         os.writeBytes(sendMessage);
         os.flush();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }

   }

   public String getStringFromExternalController()
   {
      if (DEBUG)
         System.out.println("getting a string from the external controller");

      try
      {
         String recievedString = in.readLine();
         if (DEBUG)
            System.out.println(recievedString);

         return recievedString;
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }

      return null;
   }

   public void sendDoubleArrayToExternalController(double[] doubleArray)
   {
      long tempLongL = 0;
      int iL;

      if (DEBUG)
         System.out.println("\nsending a double[] to the external controller");


      if (tempOutBuffer.length < doubleArray.length * numBytesInDouble)
      {
         tempOutBuffer = new byte[doubleArray.length * numBytesInDouble];
      }

      for (iL = 0; iL < doubleArray.length; iL++)
      {
         tempLongL = Double.doubleToRawLongBits(doubleArray[iL]);

         for (int jL = 0; jL < 8; jL++)
         {
            tempOutBuffer[iL * numBytesInDouble + jL] = (byte) ((tempLongL >> (7 - jL) * 8) & 0xFF);
         }
      }

      try
      {
         if (DEBUG)
         {
            for (iL = 0; iL < doubleArray.length; iL++)
            {
               System.out.println(doubleArray[iL] + ",");
            }
         }

         os.write(tempOutBuffer, 0, doubleArray.length * numBytesInDouble);
      }
      catch (Exception xcp)
      {
         xcp.printStackTrace();
      }
   }

   public double[] getDoubleArrayFromExternalController(int size)
   {
      if (DEBUG)
         System.out.println("\ngetting a double[] from the external controller of size " + size);

      double[] darr = new double[size];

/*
      //    DON'T UNDERSTAND WHY THIS FAILS, BUT CONVERSION TO DOUBLE IS THE PROBLEM.  IT SHOULD WORK BASED ON DOCS, BUT DOESN'T
            long tempLongL;

            if (tempInBuffer.length < size * numBytesInDouble)
            {
               tempInBuffer = new byte[size * numBytesInDouble];
            }

            try
            {
               while (in.available() < size * numBytesInDouble)
               {
                  Thread.sleep(1);
               }

               if (in.read(tempInBuffer, 0, size * numBytesInDouble) != size * numBytesInDouble)
               {
                  System.out.println("Could not read whole packet");

                  return darr;
               }

               for (int iL = 0; iL < size; iL++)
               {
                  tempLongL = 0;

                  for (int jL = 0; jL < 8; jL++)
                  {
                     tempLongL |= tempInBuffer[iL * numBytesInDouble + jL] << ((7 - jL) * 8);
                  }

                  darr[iL] = Double.longBitsToDouble(tempLongL);
               }
            }
            catch (Exception xcp)
            {
               xcp.printStackTrace();
            }
*/


      try
      {
         while (in.available() <= 0)
         {
            try
            {
               Thread.sleep(1);
            }
            catch (InterruptedException e)
            {
            }
         }

         for (int i = 0; i < size; i++)
         {
            try
            {
               darr[i] = in.readDouble();
               if (DEBUG)
                  System.out.print(darr[i] + ",");

            }
            catch (Exception xcp)
            {
               break;
            }
         }

      }
      catch (IOException e)
      {
         e.printStackTrace();
      }


      return darr;
   }

   public void close()
   {
      try
      {
         clientSocket.close();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/concurrent/SynchronousMultiThreadedRobotController.java<|end_filename|>
package us.ihmc.wholeBodyController.concurrent;

import java.util.ArrayList;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.ReentrantLock;

import us.ihmc.simulationConstructionSetTools.robotController.MultiThreadedRobotControlElement;
import us.ihmc.tools.TimestampProvider;
import us.ihmc.commons.thread.ThreadTools;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class SynchronousMultiThreadedRobotController implements MultiThreadedRobotControlElementCoordinator
{
   private final MultiThreadedRobotControlElement sensorReader;
   private final TimestampProvider timestampProvider;
   private final ArrayList<ReentrantLockedControlElementRunner> robotControllerRunners = new ArrayList<>();

   public SynchronousMultiThreadedRobotController(MultiThreadedRobotControlElement sensorReader, TimestampProvider timestampProvider)
   {
      this.sensorReader = sensorReader;
      this.timestampProvider = timestampProvider;
   }

   public void addController(MultiThreadedRobotControlElement robotController, int sensorReaderTicksPerControlTick)
   {
      robotControllerRunners.add(new ReentrantLockedControlElementRunner(robotController, this.timestampProvider, sensorReaderTicksPerControlTick));
   }

   public void read()
   {
      sensorReader.read(timestampProvider.getTimestamp());
      sensorReader.run();
      sensorReader.write(timestampProvider.getTimestamp());

      for(int i = 0; i < robotControllerRunners.size(); i++)
      {
         robotControllerRunners.get(i).tick();
         robotControllerRunners.get(i).runIfReady();
      }
   }

   public void start()
   {
      for (int i = 0; i < robotControllerRunners.size(); i++)
      {
         ReentrantLockedControlElementRunner runner = robotControllerRunners.get(i);

         runner.getController().initialize();
         
         ThreadTools.getNamedThreadFactory(runner.getController().getName()).newThread(runner).start();
      }
   }

   private class ReentrantLockedControlElementRunner implements Runnable
   {
      private final MultiThreadedRobotControlElement controller;
      private final ReentrantLock reentrantLock = new ReentrantLock();
      private final Condition shouldRunCondition;
      private final Condition isRunningCondition;
      private final TimestampProvider timestampProvider;
      private final int ticksPerExecution;
      private int ticks;

      private ReentrantLockedControlElementRunner(MultiThreadedRobotControlElement controller, TimestampProvider timestampProvider, int ticksPerExecution)
      {
         this.controller = controller;
         this.timestampProvider = timestampProvider;
         this.ticksPerExecution = ticksPerExecution;
         this.ticks = 0;

         shouldRunCondition = reentrantLock.newCondition();
         isRunningCondition = reentrantLock.newCondition();
      }

      @Override
      public void run()
      {
         while (true)
         {
            reentrantLock.lock();
            try
            {
               controller.read(timestampProvider.getTimestamp());
               controller.run();
               controller.write(timestampProvider.getTimestamp());
               isRunningCondition.signalAll();
               shouldRunCondition.await();
            }
            catch (InterruptedException e)
            {
               e.printStackTrace();
            }
            finally
            {
               reentrantLock.unlock();
            }
         }
      }

      private void tick()
      {
         reentrantLock.lock();
         try
         {
            this.ticks++;
         }
         finally
         {
            reentrantLock.unlock();
         }
      }

      private void runIfReady()
      {
         reentrantLock.lock();
         try
         {
            if (this.ticks >= this.ticksPerExecution)
            {
               this.ticks = 0;
               shouldRunCondition.signalAll();
               isRunningCondition.await();
            }
         }
         catch (InterruptedException e)
         {
            e.printStackTrace();
         }
         finally
         {
            reentrantLock.unlock();
         }
      }

      private MultiThreadedRobotControlElement getController()
      {
         return controller;
      }
   }
}


<|start_filename|>ihmc-ros-tools/generated-src/ihmc_msgs/GoHomeRosMessage.java<|end_filename|>
package ihmc_msgs;

public interface GoHomeRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_msgs/GoHomeRosMessage";
  static final java.lang.String _DEFINITION = "## GoHomeRosMessage\n# The message commands the controller to bring the given part of the body back to a default\n# configuration called \'home\'. It is useful to get back to a safe configuration before walking.\n\n# Specifies the part of the body the user wants to move back to it home configuration.\nint8 humanoid_body_part\n\n# Needed to identify a side dependent end-effector.\nint8 robot_side\n\n# How long the trajectory will spline from the current desired to the home configuration.\nfloat64 trajectory_time\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  byte getHumanoidBodyPart();
  void setHumanoidBodyPart(byte value);
  byte getRobotSide();
  void setRobotSide(byte value);
  double getTrajectoryTime();
  void setTrajectoryTime(double value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>ihmc-model-file-loader/src/main/java/us/ihmc/modelFileLoaders/assimp/JAssImpExample.java<|end_filename|>
package us.ihmc.modelFileLoaders.assimp;

import java.io.IOException;

import javafx.application.Application;
import javafx.scene.AmbientLight;
import javafx.scene.paint.Color;
import javafx.scene.shape.MeshView;
import javafx.stage.Stage;
import us.ihmc.javaFXToolkit.graphics.JAssImpJavaFXTools;
import us.ihmc.javaFXToolkit.scenes.View3DFactory;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class JAssImpExample extends Application
{


   @Override
   public void start(Stage primaryStage) throws Exception
   {
      primaryStage.setTitle("Jassimp Example");

      View3DFactory view3DFactory = new View3DFactory(1024, 768);
      view3DFactory.addCameraController();
      view3DFactory.addNodeToView(new AmbientLight(Color.WHITE));

      String meshFileName = "sampleMeshes/cinderblock1Meter.obj";


      MeshView[] meshViews = JAssImpJavaFXTools.getJavaFxMeshes(meshFileName);

      for (int i = 0; i < meshViews.length; i++)
      {
         view3DFactory.addNodeToView(meshViews[i]);
      }

      primaryStage.setScene(view3DFactory.getScene());
      primaryStage.show();
   }


   public static void main(String[] args) throws IOException
   {
      launch(args);
   }
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/vision/shapes/OpenCVColoredCircularBlobDetectorFactory.java<|end_filename|>
package us.ihmc.ihmcPerception.vision.shapes;

import us.ihmc.commons.PrintTools;
import us.ihmc.ihmcPerception.OpenCVTools;
import us.ihmc.tools.nativelibraries.NativeLibraryLoader;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class OpenCVColoredCircularBlobDetectorFactory
{
   static
   {
      try
      {
         NativeLibraryLoader.loadLibrary("org.opencv", OpenCVTools.OPEN_CV_LIBRARY_NAME);
      }
      catch (UnsatisfiedLinkError e)
      {
         PrintTools.error("Failed to load the OpenCV library.");
      }
   }

   private String videoFileName = null;
   private int cameraIndex = -1;
   private OpenCVColoredCircularBlobDetector.CaptureSource captureSource = null;

   public OpenCVColoredCircularBlobDetector buildBlobDetector()
   {
      if(captureSource == OpenCVColoredCircularBlobDetector.CaptureSource.CAMERA && cameraIndex == -1)
      {
         throw new RuntimeException("Cannot create detector for camera capture w/out a proper camera index (index >= 0)! Camera index: " + cameraIndex);
      }

      if(captureSource == OpenCVColoredCircularBlobDetector.CaptureSource.FILE && videoFileName == null)
      {
         throw new RuntimeException("Cannot create detector for video file processing, file name is null!");
      }

      return new OpenCVColoredCircularBlobDetector(videoFileName, cameraIndex, captureSource);
   }

   public String getVideoFileName()
   {
      return videoFileName;
   }

   public OpenCVColoredCircularBlobDetectorFactory setVideoFileName(String videoFileName)
   {
      this.videoFileName = videoFileName;
      return this;
   }

   public int getCameraIndex()
   {
      return cameraIndex;
   }

   public OpenCVColoredCircularBlobDetectorFactory setCameraIndex(int cameraIndex)
   {
      this.cameraIndex = cameraIndex;
      return this;
   }

   public OpenCVColoredCircularBlobDetector.CaptureSource getCaptureSource()
   {
      return captureSource;
   }

   public OpenCVColoredCircularBlobDetectorFactory setCaptureSource(OpenCVColoredCircularBlobDetector.CaptureSource captureSource)
   {
      this.captureSource = captureSource;
      return this;
   }
}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/PelvisHeightTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.PelvisHeightTrajectoryMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.PelvisHeightTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;

public class PelvisHeightTrajectoryTask extends BehaviorAction
{
   private final PelvisHeightTrajectoryMessage pelvisHeightTrajectoryMessage;
   private final PelvisHeightTrajectoryBehavior pelvisHeightTrajectoryBehavior;

   public PelvisHeightTrajectoryTask(double heightInWorld, PelvisHeightTrajectoryBehavior pelvisHeightTrajectoryBehavior, double trajectoryTime)
   {
      super(pelvisHeightTrajectoryBehavior);
      this.pelvisHeightTrajectoryBehavior = pelvisHeightTrajectoryBehavior;
      pelvisHeightTrajectoryMessage = HumanoidMessageTools.createPelvisHeightTrajectoryMessage(trajectoryTime, heightInWorld);
   }

   @Override
   protected void setBehaviorInput()
   {
      pelvisHeightTrajectoryBehavior.setInput(pelvisHeightTrajectoryMessage);
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/globalParameters/MultiplicativeDoubleGlobalParameter.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.globalParameters;


public class MultiplicativeDoubleGlobalParameter extends DoubleGlobalParameter
{
   private final DoubleGlobalParameter[] globalParametersToMultiply;

   public MultiplicativeDoubleGlobalParameter(String name, String description, DoubleGlobalParameter[] globalParametersToMultiply,
           GlobalParameterChangedListener listener)
   {
      super(name, description, globalParametersToMultiply, listener);

      this.globalParametersToMultiply = globalParametersToMultiply;
      update("computing initial value from parents.");
   }

   @Override
   public void update(String comment)
   {
      double value = 1.0;

      for (DoubleGlobalParameter doubleGlobalParameter : globalParametersToMultiply)
      {
         value = value * doubleGlobalParameter.getValue();
      }

      this.setDoubleValue(value, comment + ", multiplicative");
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/stiction/ConstantStictionModel.java<|end_filename|>
package us.ihmc.robotics.controllers.stiction;

import us.ihmc.yoVariables.providers.DoubleProvider;

public class ConstantStictionModel implements StictionModel
{
   private final DoubleProvider stictionValue;

   public ConstantStictionModel(DoubleProvider stictionValue)
   {
      this.stictionValue = stictionValue;
   }

   @Override
   public double getStictionMagnitude()
   {
      return stictionValue.getValue();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/genericQuadruped/parameters/GenericQuadrupedSquaredUpInitialPosition.java<|end_filename|>
package us.ihmc.exampleSimulations.genericQuadruped.parameters;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.exampleSimulations.genericQuadruped.model.GenericQuadrupedOrderedJointMap;
import us.ihmc.robotics.partNames.QuadrupedJointName;

public class GenericQuadrupedSquaredUpInitialPosition extends GenericQuadrupedInitialPositionParameters
{
   private final Point3D initialBodyPosition;
   private final Quaternion initialBodyOrientation;

   public GenericQuadrupedSquaredUpInitialPosition()
   {
      initialBodyPosition = new Point3D(0.0, 0.0, 0.566);
      initialBodyOrientation = new Quaternion();
   }

   @Override
   public Point3D getInitialBodyPosition()
   {
      return initialBodyPosition;
   }

   @Override
   public Quaternion getInitialBodyOrientation()
   {
      return initialBodyOrientation;
   }

   @Override
   double getHipRollAngle()
   {
      return -0.1;
   }

   @Override
   double getHipPitchAngle()
   {
      return 0.363;
   }

   @Override
   double getKneePitchAngle()
   {
      return -1.275;
   }

   public static void main(String[] args)
   {
      GenericQuadrupedSquaredUpInitialPosition initialPositionParameters = new GenericQuadrupedSquaredUpInitialPosition();
      System.out.println("Initial position: " + initialPositionParameters.getInitialBodyPosition());
      System.out.println("Initial orientation: " + initialPositionParameters.getInitialBodyOrientation());

      System.out.println("Initial positions:");
      for (GenericQuadrupedOrderedJointMap jointName : GenericQuadrupedOrderedJointMap.values)
      {
         QuadrupedJointName quadrupedJointName = QuadrupedJointName.getName(jointName.getRobotQuadrant(), jointName.getLegJointName());
         double initialJointPosition = initialPositionParameters.getInitialJointPosition(quadrupedJointName);
         System.out.println(quadrupedJointName + "\t\t" + initialJointPosition);
      }
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/communication/packets/NormalOcTreeMessage.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.communication.packets;

import us.ihmc.commons.MathTools;
import us.ihmc.communication.packets.Packet;

public class NormalOcTreeMessage extends Packet<NormalOcTreeMessage>
{
   public int treeDepth = 0;
   public float resolution = Float.NaN;
   public NormalOcTreeNodeMessage root = null;

   public NormalOcTreeMessage()
   {
   }

   @Override
   public void set(NormalOcTreeMessage other)
   {
      treeDepth = other.treeDepth;
      resolution = other.resolution;
      root = new NormalOcTreeNodeMessage();
      root.set(other.root);
      setPacketInformation(other);
   }

   @Override
   public boolean epsilonEquals(NormalOcTreeMessage other, double epsilon)
   {
      if (treeDepth != other.treeDepth)
         return false;
      if (!MathTools.epsilonEquals(resolution, other.resolution, epsilon))
         return false;
      if (!root.epsilonEquals(other.root, epsilon))
         return false;
      return true;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BehaviorControlModePacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class BehaviorControlModePacket extends Packet<BehaviorControlModePacket> implements Settable<BehaviorControlModePacket>, EpsilonComparable<BehaviorControlModePacket>
{
   public static final byte STOP = (byte) 0;
   public static final byte PAUSE = (byte) 1;
   public static final byte RESUME = (byte) 2;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte behavior_control_mode_enum_request_ = (byte) 255;

   public BehaviorControlModePacket()
   {
   }

   public BehaviorControlModePacket(BehaviorControlModePacket other)
   {
      this();
      set(other);
   }

   public void set(BehaviorControlModePacket other)
   {
      sequence_id_ = other.sequence_id_;

      behavior_control_mode_enum_request_ = other.behavior_control_mode_enum_request_;

   }

   /**
            * 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 void setBehaviorControlModeEnumRequest(byte behavior_control_mode_enum_request)
   {
      behavior_control_mode_enum_request_ = behavior_control_mode_enum_request;
   }
   public byte getBehaviorControlModeEnumRequest()
   {
      return behavior_control_mode_enum_request_;
   }


   public static Supplier<BehaviorControlModePacketPubSubType> getPubSubType()
   {
      return BehaviorControlModePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return BehaviorControlModePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(BehaviorControlModePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.behavior_control_mode_enum_request_, other.behavior_control_mode_enum_request_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof BehaviorControlModePacket)) return false;

      BehaviorControlModePacket otherMyClass = (BehaviorControlModePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.behavior_control_mode_enum_request_ != otherMyClass.behavior_control_mode_enum_request_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("BehaviorControlModePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("behavior_control_mode_enum_request=");
      builder.append(this.behavior_control_mode_enum_request_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/NumericTuner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import javafx.application.Platform;
import javafx.geometry.Pos;
import javafx.scene.Node;
import javafx.scene.layout.HBox;
import javafx.scene.layout.Priority;
import javafx.scene.text.Text;
import us.ihmc.commons.MathTools;
import us.ihmc.commons.PrintTools;
import us.ihmc.parameterTuner.guiElements.GuiParameter;

public abstract class NumericTuner<T extends Number> extends HBox implements InputNode
{
   private final NumericSpinner<T> value = createSpinner();
   private final NumericSpinner<T> min = createSpinner();
   private final NumericSpinner<T> max = createSpinner();
   private final NumericSlider<T> slider = createSlider();

   private final String parameterName;
   private boolean haveSliderBoundsBeenInvalid = false;

   public NumericTuner(GuiParameter parameter)
   {
      setupNode();
      parameterName = parameter.getName();

      value.addListener((observable, oldValue, newValue) -> {
         Platform.runLater(() -> parameter.setValue(value.getValueAsText()));
      });

      min.addListener((observable, oldValue, newValue) -> {
         Platform.runLater(() -> parameter.setMin(min.getValueAsText()));
      });

      max.addListener((observable, oldValue, newValue) -> {
         Platform.runLater(() -> parameter.setMax(max.getValueAsText()));
      });

      // Use the slider value when it gets clicked or dragged and released. Otherwise unwanted rounding will happen.
      slider.valueChangingProperty().addListener((observable, oldValue, newValue) -> {
         if (!newValue && oldValue)
         {
            value.setValue(slider.getNumber());
         }
      });
      slider.setOnMouseClicked(mouseEvent -> value.setValue(slider.getNumber()));

      parameter.addChangedListener(p -> {
         // This listener will be triggered by an external change and is called from the animation timer.
         setFromParameter(parameter);
      });
      setFromParameter(parameter);
   }

   private void setupNode()
   {
      setSpacing(10.0);
      setAlignment(Pos.CENTER_LEFT);
      HBox.setHgrow(slider, Priority.ALWAYS);

      getChildren().add(new Text("Value"));
      getChildren().add(value);
      getChildren().add(new Text("Min"));
      getChildren().add(min);
      getChildren().add(slider);
      getChildren().add(new Text("Max"));
      getChildren().add(max);
   }

   private void setFromParameter(GuiParameter parameter)
   {
      setFromString(parameter.getCurrentValue(), value);
      setFromString(parameter.getCurrentMin(), min);
      setFromString(parameter.getCurrentMax(), max);
      slider.updateSlider(value.getValue(), min.getValue(), max.getValue());
   }

   private static <T extends Number> void setFromString(String numberString, NumericSpinner<T> numberToPack)
   {
      T newValue = numberToPack.convertStringToNumber(numberString);
      numberToPack.setValue(newValue);
   }

   public abstract NumericSpinner<T> createSpinner();

   public abstract NumericSlider<T> createSlider();

   @Override
   public Node getSimpleInputNode(double width, double height)
   {
      NumericSpinner<T> duplicate = value.createLinkedDuplicate();
      duplicate.setPrefHeight(height);
      duplicate.setPrefWidth(width);
      return duplicate;
   }

   @Override
   public Node getFullInputNode()
   {
      return this;
   }

   @Override
   public void setValueFromPercent(double percent)
   {
      if (slider.isDisabled())
      {
         if (!haveSliderBoundsBeenInvalid)
         {
            PrintTools.warn("Slider bounds not valid for " + parameterName + ".");
         }
         haveSliderBoundsBeenInvalid = true;
         return;
      }
      haveSliderBoundsBeenInvalid = false;

      double min = slider.toDouble(this.min.getValue());
      double max = slider.toDouble(this.max.getValue());
      double newValue = min + percent * (max - min);

      double rounded = slider.roundToPrecision(newValue);
      rounded = MathTools.clamp(rounded, min, max);
      value.setValue(slider.toNumber(rounded));
   }

   @Override
   public double getValuePercent()
   {
      double min = slider.toDouble(this.min.getValue());
      double max = slider.toDouble(this.max.getValue());
      double value = slider.toDouble(this.value.getValue());
      return MathTools.clamp((value - min) / (max - min), 0.0, 1.0);
   }
}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/PlanarRegionIDRandomizer.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui;

import java.util.Random;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.atomic.AtomicReference;

import us.ihmc.commons.thread.ThreadTools;
import us.ihmc.messager.Messager;
import us.ihmc.messager.MessagerAPIFactory.Topic;
import us.ihmc.robotics.geometry.PlanarRegion;
import us.ihmc.robotics.geometry.PlanarRegionsList;

public class PlanarRegionIDRandomizer
{
   private final Random random = new Random(68654);

   private final boolean isExecutorServiceProvided;
   private final ExecutorService executorService;

   private AtomicReference<PlanarRegionsList> planarRegionsReference = null;
   private Topic<PlanarRegionsList> dataTopic = null;
   private final Messager messager;

   public PlanarRegionIDRandomizer(Messager messager)
   {
      this(messager, null);
   }

   public PlanarRegionIDRandomizer(Messager messager, ExecutorService executorService)
   {
      this.messager = messager;
      isExecutorServiceProvided = executorService == null;

      if (isExecutorServiceProvided)
         this.executorService = Executors.newSingleThreadExecutor(ThreadTools.getNamedThreadFactory(getClass().getSimpleName()));
      else
         this.executorService = executorService;
   }

   public void setTopics(Topic<Boolean> requestTopic, Topic<PlanarRegionsList> dataTopic)
   {
      this.dataTopic = dataTopic;
      planarRegionsReference = messager.createInput(dataTopic, null);
      messager.registerTopicListener(requestTopic, this::randomizeIDsOnThread);
   }

   private void randomizeIDsOnThread(boolean dummyArg)
   {
      PlanarRegionsList planarRegionsList = planarRegionsReference.get();
      if (planarRegionsList != null)
         executorService.execute(() -> this.randomizeIDs(planarRegionsList));
   }

   private void randomizeIDs(PlanarRegionsList planarRegionsList)
   {
      for (PlanarRegion region : planarRegionsList.getPlanarRegionsAsList())
         region.setRegionId(random.nextInt());

      messager.submitMessage(dataTopic, planarRegionsList);
   }

   public void start()
   {
   }

   public void stop()
   {
      if (!isExecutorServiceProvided)
         executorService.shutdownNow();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/contactable/ContactablePlaneBody.java<|end_filename|>
package us.ihmc.robotics.contactable;

import java.util.List;

import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;

public interface ContactablePlaneBody extends ContactableBody
{
   public abstract void setSoleFrameTransformFromParentJoint(RigidBodyTransform transform);

   public abstract ReferenceFrame getSoleFrame();

   public abstract List<FramePoint2D> getContactPoints2d();
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/perturbance/DirectedPerturbance.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.perturbance;

import us.ihmc.euclid.tuple3D.Vector3D;

public interface DirectedPerturbance
{
   public abstract void perturb(Vector3D direction);
   
   public abstract double getBallMass();
   
   public abstract double getBallVelocityMagnitude();

   public abstract void doEveryTick();
}


<|start_filename|>websitedocs/website/sidebars.json<|end_filename|>
{
  "eclipsequickstart": {
    "Quick Start (Eclipse)": ["00-pre-reqs-eclipse", "01-project-eclipse", "02-setup-gradle-project-eclipse",
    "03-new-class-eclipse",  "04-run-valkyrie-eclipse",
    "05-run-valkyrie-scs-eclipse"]
  },
  "intellijquickstart": {
    "Quick Start (IntelliJ)": ["00-pre-reqs-intellij", "01-project-intellij",
    "02-setup-gradle-project-intellij", "03-new-class-intellij",
    "04-run-valkyrie-intellij", "05-run-valkyrie-scs"]
  },
  "IHMCcontroller": {
    "IHMC Humanoid Controller": ["000-summary-control", "00-intro-control", "01-controllercore" , "02-yovariables"]
  }
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/joystick/GraspingFingerPaneController.java<|end_filename|>
package us.ihmc.valkyrie.joystick;

import javafx.event.ActionEvent;
import javafx.event.EventHandler;
import javafx.fxml.FXML;
import javafx.scene.control.Button;
import javafx.scene.control.Slider;
import us.ihmc.javaFXToolkit.messager.JavaFXMessager;
import us.ihmc.javaFXToolkit.messager.MessageBidirectionalBinding.PropertyToMessageTypeConverter;

public class GraspingFingerPaneController
{
   @FXML
   private Slider sliderLeftThumbRoll;

   @FXML
   private Slider sliderRightThumbRoll;

   @FXML
   private Slider sliderLeftThumb;
   
   @FXML
   private Slider sliderLeftThumb2;

   @FXML
   private Slider sliderLeftIndex;

   @FXML
   private Slider sliderLeftMiddle;

   @FXML
   private Slider sliderLeftPinky;

   @FXML
   private Slider sliderRightThumb;
   
   @FXML
   private Slider sliderRightThumb2;

   @FXML
   private Slider sliderRightIndex;

   @FXML
   private Slider sliderRightMiddle;

   @FXML
   private Slider sliderRightPinky;

   @FXML
   private Button btnSendLeftFingerMessage;

   @FXML
   private Button btnSendRightFingerMessage;

   public GraspingFingerPaneController()
   {

   }

   public void initialize(JavaFXMessager messager)
   {
      messager.bindBidirectional(GraspingJavaFXTopics.LeftThumbRoll, sliderLeftThumbRoll.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.RightThumbRoll, sliderRightThumbRoll.valueProperty(), createConverter(), true);

      messager.bindBidirectional(GraspingJavaFXTopics.LeftThumb, sliderLeftThumb.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.LeftThumb2, sliderLeftThumb2.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.LeftIndex, sliderLeftIndex.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.LeftMiddle, sliderLeftMiddle.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.LeftPinky, sliderLeftPinky.valueProperty(), createConverter(), true);

      messager.bindBidirectional(GraspingJavaFXTopics.RightThumb, sliderRightThumb.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.RightThumb2, sliderRightThumb2.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.RightIndex, sliderRightIndex.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.RightMiddle, sliderRightMiddle.valueProperty(), createConverter(), true);
      messager.bindBidirectional(GraspingJavaFXTopics.RightPinky, sliderRightPinky.valueProperty(), createConverter(), true);

      btnSendLeftFingerMessage.setOnAction(new EventHandler<ActionEvent>()
      {
         @Override
         public void handle(ActionEvent event)
         {
            messager.submitMessage(GraspingJavaFXTopics.LeftSendMessage, true);
         }
      });

      btnSendRightFingerMessage.setOnAction(new EventHandler<ActionEvent>()
      {
         @Override
         public void handle(ActionEvent event)
         {
            messager.submitMessage(GraspingJavaFXTopics.RightSendMessage, true);
         }
      });
   }

   private PropertyToMessageTypeConverter<Double, Number> createConverter()
   {
      return new PropertyToMessageTypeConverter<Double, Number>()
      {
         @Override
         public Double convert(Number propertyValue)
         {
            return propertyValue.doubleValue();
         }

         @Override
         public Number interpret(Double messageContent)
         {
            return messageContent;
         }
      };
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ComPositionPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "ComPositionPacket" defined in "ComPositionPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from ComPositionPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit ComPositionPacket_.idl instead.
*
*/
public class ComPositionPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.ComPositionPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::ComPositionPacket_";

   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.ComPositionPacket 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.ComPositionPacket 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 += geometry_msgs.msg.dds.PointPubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ComPositionPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.ComPositionPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += geometry_msgs.msg.dds.PointPubSubType.getCdrSerializedSize(data.getPosition(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.ComPositionPacket data, us.ihmc.idl.CDR cdr)
   {
      geometry_msgs.msg.dds.PointPubSubType.write(data.getPosition(), cdr);   }

   public static void read(controller_msgs.msg.dds.ComPositionPacket data, us.ihmc.idl.CDR cdr)
   {
      geometry_msgs.msg.dds.PointPubSubType.read(data.getPosition(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.ComPositionPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_a("position", new geometry_msgs.msg.dds.PointPubSubType(), data.getPosition());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.ComPositionPacket data)
   {
      ser.read_type_a("position", new geometry_msgs.msg.dds.PointPubSubType(), data.getPosition());
   }

   public static void staticCopy(controller_msgs.msg.dds.ComPositionPacket src, controller_msgs.msg.dds.ComPositionPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.ComPositionPacket createData()
   {
      return new controller_msgs.msg.dds.ComPositionPacket();
   }
   @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.ComPositionPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.ComPositionPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.ComPositionPacket src, controller_msgs.msg.dds.ComPositionPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public ComPositionPacketPubSubType newInstance()
   {
      return new ComPositionPacketPubSubType();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/FootLoadBearingMessage.java<|end_filename|>
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 commands the controller to start loading a foot that was unloaded to support the robot weight.
       * When the robot is performing a 'flamingo stance' (one foot in the air not actually walking) and the user wants the robot to switch back to double support.
       */
public class FootLoadBearingMessage extends Packet<FootLoadBearingMessage> implements Settable<FootLoadBearingMessage>, EpsilonComparable<FootLoadBearingMessage>
{
   public static final byte ROBOT_SIDE_LEFT = (byte) 0;
   public static final byte ROBOT_SIDE_RIGHT = (byte) 1;
   public static final byte LOAD_BEARING_REQUEST_LOAD = (byte) 0;
   public static final byte LOAD_BEARING_REQUEST_UNLOAD = (byte) 1;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Needed to identify a side dependent end-effector.
            */
   public byte robot_side_ = (byte) 255;
   /**
            * Whether the end-effector should be loaded or unloaded.
            */
   public byte load_bearing_request_;
   /**
            * The time to delay this command on the controller side before being executed.
            */
   public double execution_delay_time_;

   public FootLoadBearingMessage()
   {
   }

   public FootLoadBearingMessage(FootLoadBearingMessage other)
   {
      this();
      set(other);
   }

   public void set(FootLoadBearingMessage other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      load_bearing_request_ = other.load_bearing_request_;

      execution_delay_time_ = other.execution_delay_time_;

   }

   /**
            * 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_;
   }

   /**
            * Needed to identify a side dependent end-effector.
            */
   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   /**
            * Needed to identify a side dependent end-effector.
            */
   public byte getRobotSide()
   {
      return robot_side_;
   }

   /**
            * Whether the end-effector should be loaded or unloaded.
            */
   public void setLoadBearingRequest(byte load_bearing_request)
   {
      load_bearing_request_ = load_bearing_request;
   }
   /**
            * Whether the end-effector should be loaded or unloaded.
            */
   public byte getLoadBearingRequest()
   {
      return load_bearing_request_;
   }

   /**
            * The time to delay this command on the controller side before being executed.
            */
   public void setExecutionDelayTime(double execution_delay_time)
   {
      execution_delay_time_ = execution_delay_time;
   }
   /**
            * The time to delay this command on the controller side before being executed.
            */
   public double getExecutionDelayTime()
   {
      return execution_delay_time_;
   }


   public static Supplier<FootLoadBearingMessagePubSubType> getPubSubType()
   {
      return FootLoadBearingMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return FootLoadBearingMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(FootLoadBearingMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.load_bearing_request_, other.load_bearing_request_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.execution_delay_time_, other.execution_delay_time_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof FootLoadBearingMessage)) return false;

      FootLoadBearingMessage otherMyClass = (FootLoadBearingMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if(this.load_bearing_request_ != otherMyClass.load_bearing_request_) return false;

      if(this.execution_delay_time_ != otherMyClass.execution_delay_time_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("FootLoadBearingMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("load_bearing_request=");
      builder.append(this.load_bearing_request_);      builder.append(", ");
      builder.append("execution_delay_time=");
      builder.append(this.execution_delay_time_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-communication/src/test/java/us/ihmc/communication/remote/ClientToServerStringPacketCommunicationTester.java<|end_filename|>
package us.ihmc.communication.remote;

import us.ihmc.commons.thread.ThreadTools;

class ClientToServerStringPacketCommunicationTester extends AbstractStringPacketCommunicationTester
{
   public ClientToServerStringPacketCommunicationTester(int numberOfPackets, long maxPacketWaitTimeMillis)
   {
      super(numberOfPackets, maxPacketWaitTimeMillis);
   }

   void setupClientStreamingDataConsumers(DataObjectTransponder transponder)
   {
   }

   void setupClientDaemons(DataObjectTransponder transponder)
   {
      ThreadTools.startAsDaemon(new CommsTesterRunnable(StringPacket.getSerialVersionUID(), transponder), "Client Comms Tester (String) Daemon");
   }

   void setupServerStreamingDataConsumers(DataObjectTransponder transponder)
   {
      transponder.addStreamingDataConsumer(streamingDataConsumer);
   }

   void setupServerDaemons(DataObjectTransponder transponder)
   {
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/Vertex2DSupplierList.java<|end_filename|>
package us.ihmc.robotics.dataStructures;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Iterator;
import java.util.List;
import java.util.ListIterator;

import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;

public class Vertex2DSupplierList<T extends Point2DReadOnly> implements Vertex2DSupplier, List<T>
{
   private int firstVertexIndex;
   private int lastVertexIndex;
   private final boolean useVertexIndexingAsVertexSupplier;
   private final List<T> vertexList;

   public Vertex2DSupplierList(boolean useVertexIndexing)
   {
      vertexList = new ArrayList<>();
      useVertexIndexingAsVertexSupplier = useVertexIndexing;
   }
   
   public Vertex2DSupplierList()
   {
      this(false);
   }

   public Vertex2DSupplierList(List<T> backingList, boolean useVertexIndexing)
   {
      vertexList = backingList;
      useVertexIndexingAsVertexSupplier = useVertexIndexing;
   }
   
   public Vertex2DSupplierList(List<T> backingList)
   {
      this(backingList, false);
   }

   public List<T> getVertexList()
   {
      return vertexList;
   }
   
   public void setIndicesForVerticesToSupply(int firstIndex, int lastIndex)
   {
      checkIfVertexIndexingIsEnabled();
      if (firstIndex > lastIndex)
         throw new RuntimeException("The first index (" + firstIndex +") cannot be less than last index (" + lastIndex + ")");
      firstVertexIndex = firstIndex;
      lastVertexIndex = lastIndex;
   }

   public void setIndicesFromList()
   {
      checkIfVertexIndexingIsEnabled();
      firstVertexIndex = 0;
      lastVertexIndex = vertexList.size() - 1;
   }
   
   private void checkIfVertexIndexingIsEnabled()
   {
      if(!useVertexIndexingAsVertexSupplier)
         throw new RuntimeException("Cannot use vertex index. Option was disabled at instantiation");
   }
   
   private void checkIfVertexIndexIsValid(int indexToCheck)
   {
      checkIfVertexIndexingIsEnabled();
      if(indexToCheck < 0 && indexToCheck >= getNumberOfVertices())
         throw new IndexOutOfBoundsException("Index (" + indexToCheck + ") is not in bounds");
   }

   @Override
   public T getVertex(int index)
   {
      if(useVertexIndexingAsVertexSupplier)
      {
         checkIfVertexIndexIsValid(index);
         return vertexList.get(firstVertexIndex + index);
      }
      else
         return vertexList.get(index);
   }

   @Override
   public int getNumberOfVertices()
   {
      if (useVertexIndexingAsVertexSupplier)
         return lastVertexIndex - firstVertexIndex + 1;
      else
         return vertexList.size();
   }

   @Override
   public boolean add(T pointToAdd)
   {
      return vertexList.add(pointToAdd);
   }

   @Override
   public void add(int indexToInsertAt, T pointToInsert)
   {
      vertexList.add(indexToInsertAt, pointToInsert);
   }

   @Override
   public void clear()
   {
      vertexList.clear();
   }

   @Override
   public boolean contains(Object objectToCheck)
   {
      return vertexList.contains(objectToCheck);
   }

   @Override
   public boolean containsAll(Collection<?> collectionToCheck)
   {
      return vertexList.contains(collectionToCheck);
   }

   @Override
   public T get(int index)
   {
      return vertexList.get(index);
   }

   @Override
   public int indexOf(Object object)
   {
      return vertexList.indexOf(object);
   }

   @Override
   public boolean isEmpty()
   {
      return vertexList.isEmpty();
   }

   @Override
   public Iterator<T> iterator()
   {
      return vertexList.iterator();
   }

   @Override
   public int lastIndexOf(Object object)
   {
      return vertexList.lastIndexOf(object);
   }

   @Override
   public ListIterator<T> listIterator()
   {
      return vertexList.listIterator();
   }

   @Override
   public ListIterator<T> listIterator(int index)
   {
      return null;
   }

   @Override
   public boolean remove(Object object)
   {
      return vertexList.remove(object);
   }

   @Override
   public T remove(int index)
   {
      return vertexList.remove(index);
   }

   @Override
   public boolean removeAll(Collection<?> collection)
   {
      return vertexList.removeAll(collection);
   }

   @Override
   public boolean retainAll(Collection<?> collection)
   {
      return vertexList.retainAll(collection);
   }

   @Override
   public T set(int index, T element)
   {
      return vertexList.set(index, element);
   }

   @Override
   public int size()
   {
      return vertexList.size();
   }

   @Override
   public List<T> subList(int fromIndex, int toIndex)
   {
      return vertexList.subList(fromIndex, toIndex);
   }

   @Override
   public Object[] toArray()
   {
      return vertexList.toArray();
   }

   @Override
   public <T> T[] toArray(T[] arrayToConvert)
   {
      return vertexList.toArray(arrayToConvert);
   }

   @Override
   public boolean addAll(Collection<? extends T> c)
   {
      return vertexList.addAll(c);
   }

   @Override
   public boolean addAll(int index, Collection<? extends T> c)
   {
      return vertexList.addAll(index, c);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/graphics/ColorSpectrum.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.graphics;

import java.awt.Color;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.YoAppearanceRGBColor;
import us.ihmc.graphicsDescription.color.MutableColor;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.simulationconstructionset.SimulationConstructionSetParameters;

/**
 * <p>Title: ColorSpectrum </p>
 *
 * <p>Description: Provides a mapping from scalar values to colors.
 * Colors are given as an array of RGB values.
 *
 * @author <NAME>
 */
public class ColorSpectrum
{
   private static final double EPSILON = 10e-10;

   // private double minValue;
   // private double maxValue;
   float[][] RGB;
   Color[] colors;

   // RGB values should be between 0.0 and 1.0.
   public ColorSpectrum(float[][] RGB)
   {
      int numColors = RGB.length;
      this.RGB = RGB;
      this.colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = new Color(RGB[i][0], RGB[i][1], RGB[i][2]);
      }
   }

   // RGB values should be between 0.0 and 1.0.
   public ColorSpectrum(double[][] RGB)
   {
      int numColors = RGB.length;
      this.RGB = new float[RGB.length][3];
      this.colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         this.RGB[i][0] = (float) RGB[i][0];
         this.RGB[i][1] = (float) RGB[i][1];
         this.RGB[i][2] = (float) RGB[i][2];
         colors[i] = new Color(this.RGB[i][0], this.RGB[i][1], this.RGB[i][2]);
      }
   }

// public ColorSpectrum(Color[] colors, int intermediateColorsToGenerate)
// {
//    int numColors = intermediateColorsToGenerate;
////     Color[] intermediateColors = new Color[intermediateColorsToGenerate];
//    ArrayList<Color> intermediateColors = new ArrayList<Color>();
//    for (int i = 0; i < intermediateColorsToGenerate; i++)
//    {
//       intermediateColors.add(new Color(0.0f, 0.0f, 0.0f));
//    }
//    ArrayList<ArrayList> arrayOfArrays = JohnsUsefulFunctions.splitArrayIntoEqualishParts(intermediateColors, colors.length - 1);
//
//    int totalIndex = 0;
//    for(int arrayIndex = 0; arrayIndex < arrayOfArrays.size(); arrayIndex++)
//    {
//       for (int indexOfInnerArray = 0; indexOfInnerArray < arrayOfArrays.get(arrayIndex).size(); indexOfInnerArray++)
//       {
//          Color lastColor = colors[arrayIndex];
//          Color nextColor = colors[arrayIndex + 1];
//          double alpha = ((double) indexOfInnerArray) / ((double) arrayOfArrays.get(arrayIndex).size());
//          float r = (float) (lastColor.getRed() + (alpha * (nextColor.getRed() - lastColor.getRed()))) / 256.0f;
//          float g = (float) (lastColor.getGreen() + (alpha * (nextColor.getGreen() - lastColor.getGreen()))) / 256.0f;
//          float b = (float) (lastColor.getBlue() + (alpha * (nextColor.getBlue() - lastColor.getBlue()))) / 256.0f;
//
//          System.err.println("ColorSpectrum::ColorSpectrum : alpha " + alpha);
//          System.err.println("ColorSpectrum::ColorSpectrum : r " + r);
//          System.err.println("ColorSpectrum::ColorSpectrum : g " + g);
//          System.err.println("ColorSpectrum::ColorSpectrum : b " + b);
//
//          Color thisMorphedColor = new Color(r, g, b);
//          intermediateColors.set(totalIndex, thisMorphedColor);
//          totalIndex++;
//       }
//    }
//    this.colors = new Color[intermediateColors.size()];
//    intermediateColors.toArray(this.colors);
//    this.RGB = new float[numColors][3];
//    for (int i = 0; i < this.colors.length; i++)
//    {
//       this.colors[i].getColorComponents(RGB[i]);
//    }
// }


   public ColorSpectrum(Color[] colors)
   {
      int numColors = colors.length;
      this.colors = colors;
      this.RGB = new float[numColors][3];

      for (int i = 0; i < numColors; i++)
      {
         colors[i].getColorComponents(RGB[i]);
      }
   }

   public int size()
   {
      return colors.length;
   }

   public float[][] getRGBCopy()
   {
      float[][] RGBCopy = new float[RGB.length][3];
      for (int i = 0; i < RGB.length; i++)
      {
         for (int j = 0; j < 3; j++)
         {
            RGBCopy[i][j] = RGB[i][j];
         }
      }

      return RGBCopy;
   }

   // Use the value to index pick one of the colors in the spectrum;
   // provide the relative scale.
   public Color getColor(double value, double minValue, double maxValue)
   {
      // Place value within range.
      value = Math.max(value, minValue);
      value = Math.min(value, maxValue);
      int numColors = colors.length;
      int index = (int) (numColors * (value - minValue) / (maxValue - minValue + EPSILON));

      if (index < 0)
         index = 0;
      if (index >= numColors)
         index = numColors - 1;

      return colors[index];
   }

   // Find the color index for the given value along the real line
   // and interpolate between two neighboring colors using the fractional part of the index.
   public Color getColorContinuous(double value, double minValue, double maxValue)
   {
      // Place value within range.
      value = Math.max(value, minValue);
      value = Math.min(value, maxValue);

      int numColors = colors.length;
      float index = (float) (numColors * (value - minValue) / (maxValue - minValue + EPSILON));
      int baseIndex = (int) index;

      // We can't interpolate if the color index is the final one.
      if (baseIndex >= numColors - 1)
         return colors[numColors - 1];


      float fractionalIndex = index - baseIndex;
      float[] RGBMix = new float[3];

      for (int i = 0; i < 3; i++)
      {
         RGBMix[i] = (float) (RGB[baseIndex][i] * (1.0 - fractionalIndex) + RGB[baseIndex + 1][i] * (fractionalIndex));
      }

      return new Color(RGBMix[0], RGBMix[1], RGBMix[2]);
   }

   public static ColorSpectrum jetColors(int numColors)
   {
      return jetColors(numColors, false);
   }

   public static ColorSpectrum jetColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0, 0, 0.6250}, {0, 0, 0.7500}, {0, 0, 0.8750}, {0, 0, 1.0000}, {0, 0.1250, 1.0000}, {0, 0.2500, 1.0000}, {0, 0.3750, 1.0000}, {0, 0.5000, 1.0000},
         {0, 0.6250, 1.0000}, {0, 0.7500, 1.0000}, {0, 0.8750, 1.0000}, {0, 1.0000, 1.0000}, {0.1250, 1.0000, 0.8750}, {0.2500, 1.0000, 0.7500},
         {0.3750, 1.0000, 0.6250}, {0.5000, 1.0000, 0.5000}, {0.6250, 1.0000, 0.3750}, {0.7500, 1.0000, 0.2500}, {0.8750, 1.0000, 0.1250}, {1.0000, 1.0000, 0},
         {1.0000, 0.8750, 0}, {1.0000, 0.7500, 0}, {1.0000, 0.6250, 0}, {1.0000, 0.5000, 0}, {1.0000, 0.3750, 0}, {1.0000, 0.2500, 0}, {1.0000, 0.1250, 0},
         {1.0000, 0, 0}, {0.8750, 0, 0}, {0.7500, 0, 0}, {0.6250, 0, 0}, {0.5000, 0, 0}
      };

      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];
      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }


   public static ColorSpectrum jet2Colors(int numColors)
   {
      return jetColors(numColors, false);
   }

   public static ColorSpectrum jet2Colors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0, 0, 1.0000},    // From end of HSV
         {0, 0.2000, 1.0000}, {0, 0.4375, 1.0000}, {0, 0.6250, 1.0000}, {0, 0.8125, 1.0000}, {0, 1.0000, 1.0000}, {0, 1.0000, 0.8125}, {0, 1.0000, 0.6250},
         {0, 1.0000, 0.4375}, {0, 1.0000, 0.2500}, {0, 1.0000, 0.0625}, {0.1250, 1.0000, 0}, {0.3750, 1.0000, 0},    // From Jet
         {0.5000, 1.0000, 0}, {0.6250, 1.0000, 0}, {0.7500, 1.0000, 0}, {0.8750, 1.0000, 0}, {1.0000, 1.0000, 0}, {1.0000, 0.8750, 0}, {1.0000, 0.7500, 0},
         {1.0000, 0.6250, 0}, {1.0000, 0.5000, 0}, {1.0000, 0.3750, 0}, {1.0000, 0.2500, 0}, {1.0000, 0.1250, 0}, {1.0000, 0, 0}, {0.8750, 0, 0},
         {0.7500, 0, 0}, {0.6250, 0, 0}, {0.5000, 0, 0}
      };

      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];
      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }

   public static ColorSpectrum hsvColors(int numColors)
   {
      return hsvColors(numColors, false);
   }

   public static ColorSpectrum hsvColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {1.0000, 0, 0}, {1.0000, 0.1875, 0}, {1.0000, 0.3750, 0}, {1.0000, 0.5625, 0}, {1.0000, 0.7500, 0}, {1.0000, 0.9375, 0}, {0.8750, 1.0000, 0},
         {0.6875, 1.0000, 0}, {0.5000, 1.0000, 0}, {0.3125, 1.0000, 0}, {0.1250, 1.0000, 0}, {0, 1.0000, 0.0625}, {0, 1.0000, 0.2500}, {0, 1.0000, 0.4375},
         {0, 1.0000, 0.6250}, {0, 1.0000, 0.8125}, {0, 1.0000, 1.0000}, {0, 0.8125, 1.0000}, {0, 0.6250, 1.0000}, {0, 0.4375, 1.0000}, {0, 0.2500, 1.0000},
         {0, 0.0625, 1.0000}, {0, 0, 1.0000}    /*
                                                 *  Removed the last part of HSV since both ends are red.
                                                 * {0.3125, 0, 1.0000},
                                                 * {0.5000, 0, 1.0000},
                                                 * {0.6875, 0, 1.0000},
                                                 * {0.8750, 0, 1.0000},
                                                 * {1.0000, 0, 0.9375},
                                                 * {1.0000, 0, 0.7500},
                                                 * {1.0000, 0, 0.5625},
                                                 * {1.0000, 0, 0.3750},
                                                 * {1.0000, 0, 0.1875}
                                                 */
      };
      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      // By default, reverse the original HSV map to match Jet better.
      if (reverseMap)
      {
         return new ColorSpectrum(colors);
      }

      return new ColorSpectrum(reverseMap(colors));
   }

   public static ColorSpectrum boneColors(int numColors)
   {
      return boneColors(numColors, false);
   }

   public static ColorSpectrum boneColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0, 0, 0.0104}, {0.0282, 0.0282, 0.0491}, {0.0565, 0.0565, 0.0877}, {0.0847, 0.0847, 0.1263}, {0.1129, 0.1129, 0.1650}, {0.1411, 0.1411, 0.2036},
         {0.1694, 0.1694, 0.2423}, {0.1976, 0.1976, 0.2809}, {0.2258, 0.2258, 0.3196}, {0.2540, 0.2540, 0.3582}, {0.2823, 0.2823, 0.3968},
         {0.3105, 0.3105, 0.4355}, {0.3387, 0.3491, 0.4637}, {0.3669, 0.3878, 0.4919}, {0.3952, 0.4264, 0.5202}, {0.4234, 0.4651, 0.5484},
         {0.4516, 0.5037, 0.5766}, {0.4798, 0.5423, 0.6048}, {0.5081, 0.5810, 0.6331}, {0.5363, 0.6196, 0.6613}, {0.5645, 0.6583, 0.6895},
         {0.5927, 0.6969, 0.7177}, {0.6210, 0.7356, 0.7460}, {0.6492, 0.7742, 0.7742}, {0.6930, 0.8024, 0.8024}, {0.7369, 0.8306, 0.8306},
         {0.7807, 0.8589, 0.8589}, {0.8246, 0.8871, 0.8871}, {0.8684, 0.9153, 0.9153}, {0.9123, 0.9435, 0.9435}, {0.9561, 0.9718, 0.9718},
         {1.0000, 1.0000, 1.0000}
      };
      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }

   public static ColorSpectrum copperColors(int numColors)
   {
      return copperColors(numColors, false);
   }

   public static ColorSpectrum copperColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0, 0, 0}, {0.0403, 0.0252, 0.0160}, {0.0806, 0.0504, 0.0321}, {0.1210, 0.0756, 0.0481}, {0.1613, 0.1008, 0.0642}, {0.2016, 0.1260, 0.0802},
         {0.2419, 0.1512, 0.0963}, {0.2823, 0.1764, 0.1123}, {0.3226, 0.2016, 0.1284}, {0.3629, 0.2268, 0.1444}, {0.4032, 0.2520, 0.1605},
         {0.4435, 0.2772, 0.1765}, {0.4839, 0.3024, 0.1926}, {0.5242, 0.3276, 0.2086}, {0.5645, 0.3528, 0.2247}, {0.6048, 0.3780, 0.2407},
         {0.6452, 0.4032, 0.2568}, {0.6855, 0.4284, 0.2728}, {0.7258, 0.4536, 0.2889}, {0.7661, 0.4788, 0.3049}, {0.8065, 0.5040, 0.3210},
         {0.8468, 0.5292, 0.3370}, {0.8871, 0.5544, 0.3531}, {0.9274, 0.5796, 0.3691}, {0.9677, 0.6048, 0.3852}, {1.0000, 0.6300, 0.4012},
         {1.0000, 0.6552, 0.4173}, {1.0000, 0.6804, 0.4333}, {1.0000, 0.7056, 0.4494}, {1.0000, 0.7308, 0.4654}, {1.0000, 0.7560, 0.4815},
         {1.0000, 0.7812, 0.4975}
      };
      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }

   public static ColorSpectrum hotColors(int numColors)
   {
      return hotColors(numColors, false);
   }

   public static ColorSpectrum hotColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0.0833, 0, 0}, {0.1667, 0, 0}, {0.2500, 0, 0}, {0.3333, 0, 0}, {0.4167, 0, 0}, {0.5000, 0, 0}, {0.5833, 0, 0}, {0.6667, 0, 0}, {0.7500, 0, 0},
         {0.8333, 0, 0}, {0.9167, 0, 0}, {1.0000, 0, 0}, {1.0000, 0.0833, 0}, {1.0000, 0.1667, 0}, {1.0000, 0.2500, 0}, {1.0000, 0.3333, 0},
         {1.0000, 0.4167, 0}, {1.0000, 0.5000, 0}, {1.0000, 0.5833, 0}, {1.0000, 0.6667, 0}, {1.0000, 0.7500, 0}, {1.0000, 0.8333, 0}, {1.0000, 0.9167, 0},
         {1.0000, 1.0000, 0}, {1.0000, 1.0000, 0.1250}, {1.0000, 1.0000, 0.2500}, {1.0000, 1.0000, 0.3750}, {1.0000, 1.0000, 0.5000}, {1.0000, 1.0000, 0.6250},
         {1.0000, 1.0000, 0.7500}, {1.0000, 1.0000, 0.8750}, {1.0000, 1.0000, 1.0000}
      };
      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }

   public static ColorSpectrum coolColors(int numColors)
   {
      return coolColors(numColors, false);
   }

   public static ColorSpectrum coolColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0, 1.0000, 1.0000}, {0.0323, 0.9677, 1.0000}, {0.0645, 0.9355, 1.0000}, {0.0968, 0.9032, 1.0000}, {0.1290, 0.8710, 1.0000}, {0.1613, 0.8387, 1.0000},
         {0.1935, 0.8065, 1.0000}, {0.2258, 0.7742, 1.0000}, {0.2581, 0.7419, 1.0000}, {0.2903, 0.7097, 1.0000}, {0.3226, 0.6774, 1.0000},
         {0.3548, 0.6452, 1.0000}, {0.3871, 0.6129, 1.0000}, {0.4194, 0.5806, 1.0000}, {0.4516, 0.5484, 1.0000}, {0.4839, 0.5161, 1.0000},
         {0.5161, 0.4839, 1.0000}, {0.5484, 0.4516, 1.0000}, {0.5806, 0.4194, 1.0000}, {0.6129, 0.3871, 1.0000}, {0.6452, 0.3548, 1.0000},
         {0.6774, 0.3226, 1.0000}, {0.7097, 0.2903, 1.0000}, {0.7419, 0.2581, 1.0000}, {0.7742, 0.2258, 1.0000}, {0.8065, 0.1935, 1.0000},
         {0.8387, 0.1613, 1.0000}, {0.8710, 0.1290, 1.0000}, {0.9032, 0.0968, 1.0000}, {0.9355, 0.0645, 1.0000}, {0.9677, 0.0323, 1.0000}, {1.0000, 0, 1.0000}
      };
      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }

   public static ColorSpectrum grayColors(int numColors)
   {
      return grayColors(numColors, false);
   }

   public static ColorSpectrum grayColors(int numColors, boolean reverseMap)
   {
      double[][] seedRGB = new double[][]
      {
         {0, 0, 0}, {0.0323, 0.0323, 0.0323}, {0.0645, 0.0645, 0.0645}, {0.0968, 0.0968, 0.0968}, {0.1290, 0.1290, 0.1290}, {0.1613, 0.1613, 0.1613},
         {0.1935, 0.1935, 0.1935}, {0.2258, 0.2258, 0.2258}, {0.2581, 0.2581, 0.2581}, {0.2903, 0.2903, 0.2903}, {0.3226, 0.3226, 0.3226},
         {0.3548, 0.3548, 0.3548}, {0.3871, 0.3871, 0.3871}, {0.4194, 0.4194, 0.4194}, {0.4516, 0.4516, 0.4516}, {0.4839, 0.4839, 0.4839},
         {0.5161, 0.5161, 0.5161}, {0.5484, 0.5484, 0.5484}, {0.5806, 0.5806, 0.5806}, {0.6129, 0.6129, 0.6129}, {0.6452, 0.6452, 0.6452},
         {0.6774, 0.6774, 0.6774}, {0.7097, 0.7097, 0.7097}, {0.7419, 0.7419, 0.7419}, {0.7742, 0.7742, 0.7742}, {0.8065, 0.8065, 0.8065},
         {0.8387, 0.8387, 0.8387}, {0.8710, 0.8710, 0.8710}, {0.9032, 0.9032, 0.9032}, {0.9355, 0.9355, 0.9355}, {0.9677, 0.9677, 0.9677},
         {1.0000, 1.0000, 1.0000}
      };
      ColorSpectrum seedSpectrum = new ColorSpectrum(seedRGB);
      Color[] colors = new Color[numColors];

      for (int i = 0; i < numColors; i++)
      {
         colors[i] = seedSpectrum.getColorContinuous(i, 0, numColors);
      }

      if (reverseMap)
      {
         return new ColorSpectrum(reverseMap(colors));
      }

      return new ColorSpectrum(colors);
   }

   private static Color[] reverseMap(Color[] colors)
   {
      int numColors = colors.length;
      Color[] reverse = new Color[numColors];
      for (int i = 0; i < numColors; i++)
      {
         reverse[i] = colors[numColors - i - 1];
      }

      return reverse;
   }

   public static void addCube(double cubeX, double cubeY, double cubeSize, Color color, SimulationConstructionSet scs)
   {
      final double CUBE_HEIGHT = 0.001;
      // Make cubes a tiny bit smaller so they don't blend together.
      cubeSize *= 0.9;

      
      
      Graphics3DObject linkGraphics = new Graphics3DObject();
      linkGraphics.translate(new Vector3D(cubeX, cubeY ,CUBE_HEIGHT));
      linkGraphics.addCube(cubeSize, cubeSize, cubeSize, new YoAppearanceRGBColor(new MutableColor(color), 0.0));
      
      scs.addStaticLinkGraphics(linkGraphics);
   }

   public static void printSpectrum(ColorSpectrum spectrum, double cubeY, double cubeSize, SimulationConstructionSet scs)
   {
      int numColors = spectrum.size();
      double cubeX = -1.5;
      for (int j = 0; j < numColors; j++)
      {
         Color color = spectrum.getColor(j, 0, numColors);
         addCube(cubeX, cubeY, cubeSize, color, scs);
         cubeX += cubeSize;
      }
   }


   /* Print out the various spectra on SCS. */
   public static void main(String[] args)
   {
      // A spectrum size larger than 32 tests color interpolation.
      int spectrumSize = 100;

      double cubeSize = 0.03;
      double cubeY = 0.3;    // 0.5; for reverse maps

      SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters();
      parameters.setDataBufferSize(32000);
      SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("NullBot"), parameters);

      // Print all maps and print their reverse maps.
      boolean reverseMap = true;

      // for (int i = 0; i <= 1; i++)
      {
         reverseMap = !reverseMap;

         // jet
         cubeY -= 3 * cubeSize;
         ColorSpectrum spectrum = ColorSpectrum.jetColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // jet2
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.jet2Colors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // hsv
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.hsvColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // bone
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.boneColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // copper
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.copperColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // hot
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.hotColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // cool
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.coolColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);

         // gray
         cubeY -= 3 * cubeSize;
         spectrum = ColorSpectrum.grayColors(spectrumSize, reverseMap);
         printSpectrum(spectrum, cubeY, cubeSize, scs);
      }

      Thread thread = new Thread(scs);
      thread.start();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/GUISideAbstractCommandListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication;

public interface GUISideAbstractCommandListener
{

   public abstract void doHello(String name, String info);

   public abstract void doAllRegistriesAndVariables(String[] registryNames, String[][] variableNames, float[][] initialValues);

   public abstract void doRegistrySettingsProcessed(int[] registryIndices, boolean[] isSent, boolean[] isDisallowSendingSet, boolean[] isLogged, int registrySettingsIdentifier);

   public abstract void doSet(int index, float value);

   public abstract void doPeriod(int periodmsec);

   public abstract void doDisconnect();

   public abstract void doUserCommand(String command);

   public abstract void doData(float[] data);

   public abstract void doTextMessage(String message);

}

<|start_filename|>ihmc-trajectory-optimization/src/main/java/us/ihmc/trajectoryOptimization/DefaultDiscreteState.java<|end_filename|>
package us.ihmc.trajectoryOptimization;

public enum DefaultDiscreteState
{
   DEFAULT
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/AtlasElectricMotorEnablePacket.java<|end_filename|>
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;

/**
       * Specifies a specific electric motor in the Atlas forearm to power on or off.
       */
public class AtlasElectricMotorEnablePacket extends Packet<AtlasElectricMotorEnablePacket> implements Settable<AtlasElectricMotorEnablePacket>, EpsilonComparable<AtlasElectricMotorEnablePacket>
{
   public static final byte L_ARM_WRY = (byte) 0;
   public static final byte L_ARM_WRX = (byte) 1;
   public static final byte L_ARM_WRY2 = (byte) 2;
   public static final byte R_ARM_WRY = (byte) 3;
   public static final byte R_ARM_WRX = (byte) 4;
   public static final byte R_ARM_WRY2 = (byte) 5;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The Enum value of the motor to enable
            */
   public byte atlas_electric_motor_packet_enum_enable_ = (byte) 255;
   /**
            * Boolean for enable state; true for enable, false for disable.
            */
   public boolean enable_;

   public AtlasElectricMotorEnablePacket()
   {
   }

   public AtlasElectricMotorEnablePacket(AtlasElectricMotorEnablePacket other)
   {
      this();
      set(other);
   }

   public void set(AtlasElectricMotorEnablePacket other)
   {
      sequence_id_ = other.sequence_id_;

      atlas_electric_motor_packet_enum_enable_ = other.atlas_electric_motor_packet_enum_enable_;

      enable_ = other.enable_;

   }

   /**
            * 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_;
   }

   /**
            * The Enum value of the motor to enable
            */
   public void setAtlasElectricMotorPacketEnumEnable(byte atlas_electric_motor_packet_enum_enable)
   {
      atlas_electric_motor_packet_enum_enable_ = atlas_electric_motor_packet_enum_enable;
   }
   /**
            * The Enum value of the motor to enable
            */
   public byte getAtlasElectricMotorPacketEnumEnable()
   {
      return atlas_electric_motor_packet_enum_enable_;
   }

   /**
            * Boolean for enable state; true for enable, false for disable.
            */
   public void setEnable(boolean enable)
   {
      enable_ = enable;
   }
   /**
            * Boolean for enable state; true for enable, false for disable.
            */
   public boolean getEnable()
   {
      return enable_;
   }


   public static Supplier<AtlasElectricMotorEnablePacketPubSubType> getPubSubType()
   {
      return AtlasElectricMotorEnablePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return AtlasElectricMotorEnablePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(AtlasElectricMotorEnablePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.atlas_electric_motor_packet_enum_enable_, other.atlas_electric_motor_packet_enum_enable_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.enable_, other.enable_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof AtlasElectricMotorEnablePacket)) return false;

      AtlasElectricMotorEnablePacket otherMyClass = (AtlasElectricMotorEnablePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.atlas_electric_motor_packet_enum_enable_ != otherMyClass.atlas_electric_motor_packet_enum_enable_) return false;

      if(this.enable_ != otherMyClass.enable_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("AtlasElectricMotorEnablePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("atlas_electric_motor_packet_enum_enable=");
      builder.append(this.atlas_electric_motor_packet_enum_enable_);      builder.append(", ");
      builder.append("enable=");
      builder.append(this.enable_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/model/RobotMotionStatus.java<|end_filename|>
package us.ihmc.sensorProcessing.model;

import us.ihmc.communication.ros.generators.RosEnumValueDocumentation;

public enum RobotMotionStatus
{
   @RosEnumValueDocumentation(documentation = "It is uncertain if the robot is moving or not, it can only be determined when the walking controller is active.")
   UNKNOWN(2),
   @RosEnumValueDocumentation(documentation = "Robot is standing")
   STANDING(3),
   @RosEnumValueDocumentation(documentation = "Robot is in motion")
   IN_MOTION(4);

   public int behaviorId;

   private RobotMotionStatus(int behaviorId)
   {
      this.behaviorId = behaviorId;
   }

   public int getBehaviorId()
   {
      return behaviorId;
   }

   public static RobotMotionStatus[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static RobotMotionStatus fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BDIBehaviorStatusPacket.java<|end_filename|>
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;

/**
       * Atlas specific message.
       */
public class BDIBehaviorStatusPacket extends Packet<BDIBehaviorStatusPacket> implements Settable<BDIBehaviorStatusPacket>, EpsilonComparable<BDIBehaviorStatusPacket>
{
   public static final byte NONE = (byte) 0;
   public static final byte FREEZE = (byte) 1;
   public static final byte STAND_PREP = (byte) 2;
   public static final byte STAND = (byte) 3;
   public static final byte WALK = (byte) 4;
   public static final byte STEP = (byte) 5;
   public static final byte MANIPULATE = (byte) 6;
   public static final byte USER = (byte) 7;
   public static final byte CALIBRATE = (byte) 8;
   public static final byte SOFT_STOP = (byte) 9;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte current_bdi_robot_behavior_ = (byte) 255;

   public BDIBehaviorStatusPacket()
   {
   }

   public BDIBehaviorStatusPacket(BDIBehaviorStatusPacket other)
   {
      this();
      set(other);
   }

   public void set(BDIBehaviorStatusPacket other)
   {
      sequence_id_ = other.sequence_id_;

      current_bdi_robot_behavior_ = other.current_bdi_robot_behavior_;

   }

   /**
            * 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 void setCurrentBdiRobotBehavior(byte current_bdi_robot_behavior)
   {
      current_bdi_robot_behavior_ = current_bdi_robot_behavior;
   }
   public byte getCurrentBdiRobotBehavior()
   {
      return current_bdi_robot_behavior_;
   }


   public static Supplier<BDIBehaviorStatusPacketPubSubType> getPubSubType()
   {
      return BDIBehaviorStatusPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return BDIBehaviorStatusPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(BDIBehaviorStatusPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.current_bdi_robot_behavior_, other.current_bdi_robot_behavior_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof BDIBehaviorStatusPacket)) return false;

      BDIBehaviorStatusPacket otherMyClass = (BDIBehaviorStatusPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.current_bdi_robot_behavior_ != otherMyClass.current_bdi_robot_behavior_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("BDIBehaviorStatusPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("current_bdi_robot_behavior=");
      builder.append(this.current_bdi_robot_behavior_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/ThreadedAtomicPacketSubscriber.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import java.util.ArrayList;
import java.util.concurrent.atomic.AtomicReference;

import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.packets.Packet;

public class ThreadedAtomicPacketSubscriber<T extends Packet<?>> implements PacketConsumer<T>, Runnable
{
   private AtomicReference<T> lastPacket = new AtomicReference<T>();
   private final Object syncObject = new Object();
   private ArrayList<PacketConsumer<T>> listOfListeners = new ArrayList<PacketConsumer<T>>();

   public ThreadedAtomicPacketSubscriber()
   {
      Thread t = new Thread(this, "IHMC-AtomicPacketSubscriber");
      t.start();
   }

   public void registerListener(PacketConsumer<T> listener)
   {
      listOfListeners.add(listener);
   }

   @Override
   public void run()
   {
      while (true)
      {
         synchronized (syncObject)
         {
            try
            {
               syncObject.wait();
            }
            catch (InterruptedException e)
            {
            }
            
            T packet = lastPacket.getAndSet(null);
            if (packet != null)
            {
               for (PacketConsumer<T> listener : listOfListeners)
               {
                  listener.receivedPacket(packet);
               }
            }
         }
      }
   }

   @Override
   public void receivedPacket(T packet)
   {
      synchronized (syncObject)
      {
         lastPacket.set(packet);
         syncObject.notifyAll();
      }
   }

}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tabs/SliderButton.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tabs;

import java.util.Optional;

import javafx.geometry.Insets;
import javafx.scene.control.Button;
import javafx.scene.control.TextInputDialog;
import javafx.scene.image.Image;
import javafx.scene.image.ImageView;
import us.ihmc.parameterTuner.guiElements.ParameterChangeListener;
import us.ihmc.parameterTuner.guiElements.tuners.Tuner;
import us.ihmc.robotics.sliderboard.Sliderboard;

public class SliderButton extends Button
{
   private final ImageView sliderGraphic;
   private boolean isActive = false;

   private ParameterChangeListener listener;
   private int sliderIndex;

   private Tuner tuner;
   private Sliderboard sliderboard;

   public SliderButton(int initialSliderIndex)
   {
      sliderIndex = initialSliderIndex;

      Image sliderImage = new Image(SliderButton.class.getResourceAsStream("/sliders.png"));
      sliderGraphic = new ImageView(sliderImage);

      setPadding(new Insets(TuningBox.buttonPadding));
      sliderGraphic.setFitWidth(TuningBox.imageSize);
      sliderGraphic.setFitHeight(TuningBox.imageSize);
      setGraphic(sliderGraphic);

      double size = TuningBox.imageSize + 2.0 * TuningBox.buttonPadding;
      setPrefSize(size, size);
      setMinSize(size, size);
      setMaxSize(size, size);

      setOnAction(event -> {
         if (!isActive)
         {
            connect();
         }
         else
         {
            disconnect(true);
         }
      });
   }

   private void disconnect(boolean discardIndex)
   {
      setText(null);
      setGraphic(sliderGraphic);
      isActive = false;

      if (tuner != null && sliderboard != null && sliderIndex != -1)
      {
         sliderboard.removeListener(tuner, sliderIndex);
      }
      if (tuner != null && listener != null)
      {
         tuner.removeChangeListener(listener);
      }

      if (discardIndex)
      {
         sliderIndex = -1;
      }
   }

   private void connect()
   {
      TextInputDialog dialog = new TextInputDialog("1");
      dialog.setTitle("Slider Index");
      dialog.setHeaderText("Select Slider Index");
      dialog.setContentText("Enter the index:");

      Optional<String> result = dialog.showAndWait();
      if (!result.isPresent())
      {
         return;
      }

      try
      {
         int sliderIndex = Integer.parseInt(result.get());
         connect(sliderIndex);
      }
      catch (NumberFormatException e)
      {
         return;
      }
   }

   private void connect(int sliderIndex)
   {
      if (isActive)
      {
         disconnect(true);
      }

      if (sliderIndex > 99)
      {
         return;
      }

      if (sliderboard.addListener(tuner, sliderIndex))
      {
         setGraphic(null);
         setText(Integer.toString(sliderIndex));

         listener = p -> updateSlider(sliderIndex);
         tuner.addChangeListener(listener);
         updateSlider(sliderIndex);

         this.sliderIndex = sliderIndex;
         isActive = true;
      }
   }

   private void updateSlider(int sliderIndex)
   {
      double sliderPercent = tuner.getValuePercent();
      sliderboard.setSliderValue(sliderPercent, sliderIndex);
   }

   public void link(Tuner tuner, Sliderboard sliderboard)
   {
      this.tuner = tuner;
      this.sliderboard = sliderboard;

      // Reconnect on Tab switch.
      if (sliderIndex != -1)
      {
         connect(sliderIndex);
      }
   }

   public void unlink()
   {
      disconnect(false);

      this.tuner = null;
      this.sliderboard = null;
   }

   public int getIndex()
   {
      return sliderIndex;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WalkToGoalBehaviorPacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class WalkToGoalBehaviorPacket extends Packet<WalkToGoalBehaviorPacket> implements Settable<WalkToGoalBehaviorPacket>, EpsilonComparable<WalkToGoalBehaviorPacket>
{
   public static final byte WALK_TO_GOAL_ACTION_FIND_PATH = (byte) 0;
   public static final byte WALK_TO_GOAL_ACTION_EXECUTE = (byte) 1;
   public static final byte WALK_TO_GOAL_ACTION_EXECUTE_UNKNOWN = (byte) 2;
   public static final byte WALK_TO_GOAL_ACTION_STOP = (byte) 3;
   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_;
   public byte walk_to_goal_action_ = (byte) 255;
   public double x_goal_;
   public double y_goal_;
   public double theta_goal_;
   public byte goal_robot_side_ = (byte) 255;

   public WalkToGoalBehaviorPacket()
   {
   }

   public WalkToGoalBehaviorPacket(WalkToGoalBehaviorPacket other)
   {
      this();
      set(other);
   }

   public void set(WalkToGoalBehaviorPacket other)
   {
      sequence_id_ = other.sequence_id_;

      walk_to_goal_action_ = other.walk_to_goal_action_;

      x_goal_ = other.x_goal_;

      y_goal_ = other.y_goal_;

      theta_goal_ = other.theta_goal_;

      goal_robot_side_ = other.goal_robot_side_;

   }

   /**
            * 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 void setWalkToGoalAction(byte walk_to_goal_action)
   {
      walk_to_goal_action_ = walk_to_goal_action;
   }
   public byte getWalkToGoalAction()
   {
      return walk_to_goal_action_;
   }

   public void setXGoal(double x_goal)
   {
      x_goal_ = x_goal;
   }
   public double getXGoal()
   {
      return x_goal_;
   }

   public void setYGoal(double y_goal)
   {
      y_goal_ = y_goal;
   }
   public double getYGoal()
   {
      return y_goal_;
   }

   public void setThetaGoal(double theta_goal)
   {
      theta_goal_ = theta_goal;
   }
   public double getThetaGoal()
   {
      return theta_goal_;
   }

   public void setGoalRobotSide(byte goal_robot_side)
   {
      goal_robot_side_ = goal_robot_side;
   }
   public byte getGoalRobotSide()
   {
      return goal_robot_side_;
   }


   public static Supplier<WalkToGoalBehaviorPacketPubSubType> getPubSubType()
   {
      return WalkToGoalBehaviorPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WalkToGoalBehaviorPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WalkToGoalBehaviorPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.walk_to_goal_action_, other.walk_to_goal_action_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.x_goal_, other.x_goal_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.y_goal_, other.y_goal_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.theta_goal_, other.theta_goal_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.goal_robot_side_, other.goal_robot_side_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WalkToGoalBehaviorPacket)) return false;

      WalkToGoalBehaviorPacket otherMyClass = (WalkToGoalBehaviorPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.walk_to_goal_action_ != otherMyClass.walk_to_goal_action_) return false;

      if(this.x_goal_ != otherMyClass.x_goal_) return false;

      if(this.y_goal_ != otherMyClass.y_goal_) return false;

      if(this.theta_goal_ != otherMyClass.theta_goal_) return false;

      if(this.goal_robot_side_ != otherMyClass.goal_robot_side_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WalkToGoalBehaviorPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("walk_to_goal_action=");
      builder.append(this.walk_to_goal_action_);      builder.append(", ");
      builder.append("x_goal=");
      builder.append(this.x_goal_);      builder.append(", ");
      builder.append("y_goal=");
      builder.append(this.y_goal_);      builder.append(", ");
      builder.append("theta_goal=");
      builder.append(this.theta_goal_);      builder.append(", ");
      builder.append("goal_robot_side=");
      builder.append(this.goal_robot_side_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/sensors/PointCloudWithSourcePoseTester.java<|end_filename|>
package us.ihmc.atlas.sensors;

import java.net.URI;
import java.net.URISyntaxException;

import geometry_msgs.Point;
import geometry_msgs.Quaternion;
import scan_to_cloud.PointCloud2WithSource;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.subscriber.AbstractRosTopicSubscriber;

/**
 * Gets a single lidar scan in world and gives the pose of the lidar frame in head. You will need to finish the transformation to world. 
 *
 */
public class PointCloudWithSourcePoseTester extends AbstractRosTopicSubscriber<PointCloud2WithSource>
{

   public PointCloudWithSourcePoseTester(RosMainNode rosMainNode)
   {
      super(PointCloud2WithSource._TYPE);
      rosMainNode.attachSubscriber("/singleScanAsCloudWithSource", this);
   }

   @Override
   public void onNewMessage(PointCloud2WithSource cloudHolder)
   {
      System.out.println(cloudHolder.getCloud().getWidth());
      Point translation = cloudHolder.getTranslation();
      Point3D position = new Point3D(translation.getX(), translation.getY(),translation.getZ());
      System.out.println(position);
      Quaternion orientation = cloudHolder.getOrientation();
      us.ihmc.euclid.tuple4D.Quaternion quaternion = new us.ihmc.euclid.tuple4D.Quaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW());
      System.out.println(quaternion);
   }
   
   public static void main(String[] args) throws URISyntaxException
   {
      URI masterURI = new URI("http://cpu0:11311");
      RosMainNode rosMainNode = new RosMainNode(masterURI, "scanToCLoudJavaTester", true);
      new PointCloudWithSourcePoseTester(rosMainNode);
      rosMainNode.execute();
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/Neuron.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork;

import java.util.ArrayList;

import gnu.trove.map.hash.TObjectDoubleHashMap;
import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.ActivationFunction;

/**
 * Processing unit in a Neural Network
 */
public class Neuron
{
   private final ArrayList<Neuron> inputs = new ArrayList<>();
   private final TObjectDoubleHashMap<Neuron> weights = new TObjectDoubleHashMap<Neuron>();
   private final ActivationFunction activationFunction;
   private double output = 0.0;
   private double bias = 0.0;

   /**
    * Creates a Neuron
    * @param activationFunction the function used to process the output of this Neuron
    * @param bias the constant added to the summed inputs
    */
   public Neuron(ActivationFunction activationFunction, double bias)
   {
      this.activationFunction = activationFunction;
      this.bias = bias; 
   }

   /**
    * Connects a neuron and sets the associated scalar that will be multiplied to it's output
    * @param neuron the input neuron
    * @param weight the scalar
    */
   public void addInputNeuron(Neuron neuron, double weight)
   {
      inputs.add(neuron);
      weights.put(neuron, weight);
   }

   /**
    * Computes the output of this neuron
    */
   public void compute()
   {
      double sum = bias;
      for (int i = 0; i < inputs.size(); i++)
      {
         Neuron neuron = inputs.get(i);
         double weight = weights.get(neuron);
         sum += neuron.getOutput() * weight;
      }
      output = activationFunction.computeOutput(sum);
   }
   
   /**
    * sets the constant added to the output
    * @param bias
    */
   public void setBias(double bias)
   {
      this.bias = bias;
   }

   /**
    * Returns the computed output
    * @return the output after going through the activation function
    */
   public double getOutput()
   {
      return output;
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/simple3DWalker/HipAngleCapturePointCalculator3D.java<|end_filename|>
package us.ihmc.exampleSimulations.simple3DWalker;

public class HipAngleCapturePointCalculator3D
{
   private static double g = 9.81;

   public static double getHipPitchAngle(double bodyVelocity, double legLength)
   {
      double miniumumValue = Double.POSITIVE_INFINITY;
      double bestAngle = 0.0;
      for(double angle=0.0; angle<Math.PI/2.0; angle = angle + 0.001)
      {
         double term1 = g * legLength * legLength * (Math.sin(angle) * Math.sin(angle));
         double term2 = bodyVelocity * bodyVelocity * legLength * Math.cos(angle);

         double value = term1 - term2;

         if (Math.abs(value) < Math.abs(miniumumValue))
         {
            miniumumValue = value;
            bestAngle = angle;
         }
      }
      return bestAngle;
   }

   public static double getHipRollAngle(double bodyVelocity, double legLength)
   {
      double miniumumValue = Double.POSITIVE_INFINITY;
      double bestAngle = 0.0;
      for(double angle=0.0; angle<Math.PI/2.0; angle = angle + 0.001)
      {
         double term1 = g * legLength * legLength * (Math.sin(angle) * Math.sin(angle));
         double term2 = bodyVelocity * bodyVelocity * legLength * Math.cos(angle);

         double value = term1 - term2;

         if (Math.abs(value) < Math.abs(miniumumValue))
         {
            miniumumValue = value;
            bestAngle = angle;
         }
      }
      return bestAngle;
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/DRCHandType.java<|end_filename|>
package us.ihmc.wholeBodyController;

public enum DRCHandType
{
   NONE(true), IROBOT(false), HOOK(false), ROBOTIQ(true), VALKYRIE(false), ROBOTIQ_AND_SRI(false);

   private final boolean simulateHand;

   private DRCHandType(boolean simulateHand)
   {
      this.simulateHand = simulateHand;
   }

   public boolean isHandSimulated()
   {
      return simulateHand;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MovingMidFootZUpGroundFrame.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import us.ihmc.euclid.referenceFrame.FramePose3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;

public class MovingMidFootZUpGroundFrame extends MovingReferenceFrame
{
   private final MovingZUpFrame frameOne, frameTwo;
   private final FramePose3D pose = new FramePose3D();
   private final FramePose3D poseOne = new FramePose3D();
   private final FramePose3D poseTwo = new FramePose3D();

   private final FrameVector3D linearVelocity = new FrameVector3D();
   private final FrameVector3D linearVelocityOne = new FrameVector3D();
   private final FrameVector3D linearVelocityTwo = new FrameVector3D();

   public MovingMidFootZUpGroundFrame(String name, MovingZUpFrame frameOne, MovingZUpFrame frameTwo)
   {
      super(name, frameOne.getRootFrame(), true);

      if (frameOne == frameTwo)
         throw new IllegalArgumentException("The frames have to be different.");
      frameOne.verifySameRoots(frameTwo);

      this.frameOne = frameOne;
      this.frameTwo = frameTwo;
   }

   @Override
   protected void updateTransformToParent(RigidBodyTransform transformToParent)
   {
      poseOne.setToZero(frameOne);
      poseTwo.setToZero(frameTwo);

      poseOne.changeFrame(getParent());
      poseTwo.changeFrame(getParent());

      pose.setToZero(getParent());
      pose.interpolate(poseOne, poseTwo, 0.5);
      pose.setZ(Math.min(poseOne.getZ(), poseTwo.getZ()));
      pose.get(transformToParent);
   }

   @Override
   protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack)
   {
      TwistReadOnly twistOfFrameOne = frameOne.getTwistOfFrame();
      TwistReadOnly twistOfFrameTwo = frameTwo.getTwistOfFrame();

      linearVelocityOne.setIncludingFrame(twistOfFrameOne.getLinearPart());
      linearVelocityTwo.setIncludingFrame(twistOfFrameTwo.getLinearPart());

      linearVelocityOne.changeFrame(this);
      linearVelocityTwo.changeFrame(this);
      linearVelocity.setToZero(this);
      linearVelocity.interpolate(linearVelocityOne, linearVelocityTwo, 0.5);
      twistRelativeToParentToPack.setToZero(this, getParent(), this);
      twistRelativeToParentToPack.getLinearPart().set(linearVelocity);

      if (poseOne.getZ() < poseTwo.getZ())
         twistRelativeToParentToPack.setLinearPartZ(linearVelocityOne.getZ());
      else
         twistRelativeToParentToPack.setLinearPartZ(linearVelocityTwo.getZ());

      double wz = 0.5 * (twistOfFrameOne.getAngularPartZ() + twistOfFrameTwo.getAngularPartZ());
      twistRelativeToParentToPack.setAngularPartZ(wz);
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/exploringSpatial/SpatialNode.java<|end_filename|>
package us.ihmc.manipulation.planning.exploringSpatial;

import controller_msgs.msg.dds.KinematicsToolboxOutputStatus;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.euclid.tools.EuclidCoreTools;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public class SpatialNode
{
   private double time;
   private SpatialData spatialData;

   private SpatialNode parent;

   private boolean validity = true;
   private KinematicsToolboxOutputStatus configuration;

   public SpatialNode()
   {
   }

   public SpatialNode(SpatialData spatialData)
   {
      this(0.0, spatialData);
   }

   public SpatialNode(double time, SpatialData spatialData)
   {
      this.time = time;
      this.spatialData = spatialData;
   }

   public SpatialNode(SpatialNode other)
   {
      time = other.time;
      spatialData = new SpatialData(other.spatialData);

      if (other.parent != null)
         parent = new SpatialNode(other.parent);
      else
         parent = null;

      validity = other.validity;
      if (other.configuration != null)
         configuration = new KinematicsToolboxOutputStatus(other.configuration);
   }

   public void initializeSpatialData()
   {
      spatialData.initializeData();
   }

   public double getTimeGap(SpatialNode other)
   {
      if (getTime() > other.getTime())
         return Double.MAX_VALUE;
      else
         return other.getTime() - getTime();
   }

   public double getPositionDistance(SpatialNode other)
   {
      return spatialData.getPositionDistance(other.getSpatialData());
   }

   public double getOrientationDistance(SpatialNode other)
   {
      return spatialData.getOrientationDistance(other.getSpatialData());
   }

   /**
    * Compute distance from this to other.
    */
   public double computeDistance(double timeWeight, double positionWeight, double orientationWeight, SpatialNode other)
   {
      double timeDistance = timeWeight * getTimeGap(other);
      double positionDistance = positionWeight * getPositionDistance(other);
      double orientationDistance = orientationWeight * getOrientationDistance(other);

      double distance = timeDistance + positionDistance + orientationDistance;
      return distance;
   }

   /**
    * Compute distance within maximum distance
    */
   public double computeDistanceWithinMaxDistance(double timeWeight, double positionWeight, double orientationWeight, SpatialNode other, double maxTimeInterval,
                                                  double maxPositionDistance, double maxOrientationDistance)
   {
      double timeDistance = timeWeight * getTimeGap(other);
      double positionDistance = positionWeight * getPositionDistance(other);
      double orientationDistance = orientationWeight * getOrientationDistance(other);

      double greatestPositionDistance = spatialData.getMaximumPositionDistance(other.getSpatialData());
      double greatestOrientationDistance = spatialData.getMaximumOrientationDistance(other.getSpatialData());

      if (greatestPositionDistance / getTimeGap(other) > maxPositionDistance / maxTimeInterval)
         return Double.MAX_VALUE;
      if (greatestOrientationDistance / getTimeGap(other) > maxOrientationDistance / maxTimeInterval)
         return Double.MAX_VALUE;

      double distance = timeDistance + positionDistance + orientationDistance;

      return distance;
   }

   public void interpolate(SpatialNode nodeOne, SpatialNode nodeTwo, double alpha)
   {
      time = EuclidCoreTools.interpolate(nodeOne.time, nodeTwo.time, alpha);

      spatialData.interpolate(nodeOne.getSpatialData(), nodeTwo.getSpatialData(), alpha);
   }

   public SpatialNode createNodeWithinMaxDistance(double maxTimeInterval, double maxPositionDistance, double maxOrientationDistance, SpatialNode query)
   {
      double alpha = 1.0;

      double timeGap = getTimeGap(query);

      double greatestPositionDistance = spatialData.getMaximumPositionDistance(query.getSpatialData());
      double greatestOrientationDistance = spatialData.getMaximumOrientationDistance(query.getSpatialData());

      if (timeGap > maxTimeInterval)
      {
         alpha = Math.min(alpha, maxTimeInterval / timeGap);
      }

      if (greatestPositionDistance > maxPositionDistance)
      {
         alpha = Math.min(alpha, maxPositionDistance / greatestPositionDistance);
      }

      if (greatestOrientationDistance > maxOrientationDistance)
      {
         alpha = Math.min(alpha, maxOrientationDistance / greatestOrientationDistance);
      }

      if (alpha >= 1.0)
      {
         return new SpatialNode(query);
      }
      else if (alpha <= 0.0)
      {
         return new SpatialNode(this);
      }
      else
      {
         SpatialNode spatialNode = new SpatialNode(this);
         spatialNode.interpolate(this, query, alpha);
         return spatialNode;
      }
   }

   public SpatialNode createNodeWithinTimeStep(double maxTimeInterval, SpatialNode query)
   {
      SpatialNode spatialNode = new SpatialNode(this);

      double alpha = 1.0;
      double timeGap = getTimeGap(query);
      if (timeGap > maxTimeInterval)
      {
         alpha = Math.min(alpha, maxTimeInterval / timeGap);
      }
      spatialNode.interpolate(this, query, alpha);

      return spatialNode;
   }

   public double getConfigurationData(int index)
   {
      return spatialData.getConfigurationData().get(index);
   }

   public String getConfigurationName(int index)
   {
      return spatialData.getConfigurationNames().get(index);
   }

   public SpatialData getSpatialData()
   {
      return spatialData;
   }

   public void setParent(SpatialNode parent)
   {
      this.parent = parent;
   }

   public void clearParent()
   {
      parent = null;
   }

   public SpatialNode getParent()
   {
      return parent;
   }

   public void setTime(double time)
   {
      this.time = time;
   }

   public double getTime()
   {
      return time;
   }

   public void setConfiguration(KinematicsToolboxOutputStatus configuration)
   {
      this.configuration = new KinematicsToolboxOutputStatus(configuration);
   }

   public KinematicsToolboxOutputStatus getConfiguration()
   {
      return configuration;
   }

   public void setValidity(boolean validity)
   {
      this.validity = validity;
   }

   public boolean isValid()
   {
      return validity;
   }

   public int getSize()
   {
      return spatialData.getRigidBodyNames().size();
   }

   public String getName(int index)
   {
      return spatialData.getRigidBodyNames().get(index);
   }

   public Pose3D getSpatialData(int index)
   {
      return spatialData.getRigidBodySpatials().get(index);
   }

   public Pose3D getSpatialData(RigidBodyBasics rigidBody)
   {
      for (int i = 0; i < spatialData.getRigidBodyNames().size(); i++)
      {
         if (spatialData.getRigidBodyNames().get(i).equals(rigidBody.getName()))
            return getSpatialData(i);
      }
      return null;
   }

   public void setSpatialsAndConfiguration(SpatialNode other)
   {
      time = other.time;
      spatialData = new SpatialData(other.getSpatialData());
      configuration = new KinematicsToolboxOutputStatus(other.configuration);
      validity = other.validity;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/DrillDetectionStatusListener.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import controller_msgs.msg.dds.DrillDetectionPacket;

public interface DrillDetectionStatusListener
{
   public abstract void updateStatusPacket(DrillDetectionPacket drillDetectionPacket);
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/lists/ListSorter.java<|end_filename|>
package us.ihmc.tools.lists;

import java.util.List;
import java.util.Comparator;

public class ListSorter
{
   /**
    * Sort an array in place without allocating any memory.
    */
   public static <T> void sort(List<T> ts, Comparator<T> comparator)
   {
      boolean ordered = false;

      while (!ordered)
      {
         ordered = true;
         for (int i = 0; i < ts.size() - 1; i++)
         {
            T a = ts.get(i);
            T b = ts.get(i + 1);

            if(comparator.compare(a, b) > 0)
            {
               ordered = false;
               swap(ts, i, i + 1);
            }
         }
      }
   }

   public static <T> void swap(List<T> ts, int a, int b)
   {
      T tmp = ts.get(a);
      ts.set(a, ts.get(b));
      ts.set(b, tmp);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/whiteBoard/TCPYoWhiteBoard.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.whiteBoard;

import java.io.BufferedInputStream;
import java.io.BufferedOutputStream;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.ConnectException;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.ScheduledFuture;
import java.util.concurrent.ThreadFactory;
import java.util.concurrent.TimeUnit;

import us.ihmc.commons.PrintTools;
import us.ihmc.commons.thread.ThreadTools;

public class TCPYoWhiteBoard extends DataStreamYoWhiteBoard
{
	private static final boolean VERBOSE = false;
	private static final boolean PRINT_ERRORS = false;
	private static final long THREAD_PERIOD = 1L;
	private static final TimeUnit TIME_UNIT = TimeUnit.SECONDS;

   private final ScheduledExecutorService executorService;
	private ScheduledFuture<?> activeScheduled = null;

   private final String ipAddress;
   private int port;

   private ServerSocket serverSocket;
   private Socket tcpSocket;

   /**
    * Server constructor.
    */
   public TCPYoWhiteBoard(String name, final int port)
   {
      super(name, true, true);

      this.port = port;
      this.ipAddress = null;
      ThreadFactory threadFactory = ThreadTools.getNamedThreadFactory(name + "TCPServerThread");
      executorService = Executors.newScheduledThreadPool(1, threadFactory);
   }

   /**
    * Client constructor.
    */
   public TCPYoWhiteBoard(String name, String ipAddress, int port)
   {
      super(name, true, true);

      this.ipAddress = ipAddress;
      this.port = port;
      ThreadFactory threadFactory = ThreadTools.getNamedThreadFactory(name + "TCPClientThread");
      executorService = Executors.newScheduledThreadPool(1, threadFactory);
   }

   public void startTCPThread()
   {
      if (activeScheduled != null)
         throw new RuntimeException("Already running");

      Runnable command;
      if (isAServer())
         command = createServerRunnable();
      else
         command = createClientRunnable();
      activeScheduled = executorService.scheduleAtFixedRate(command, 0, THREAD_PERIOD, TIME_UNIT);
   }

   private Runnable createServerRunnable()
   {
      return new Runnable()
      {
         @Override
         public void run()
         {
            try
            {
               doActionAsServer();
            }
            catch (Exception e)
            {
               if (PRINT_ERRORS)
                  e.printStackTrace();
            }
         }
      };
   }

   private Runnable createClientRunnable()
   {
      return new Runnable()
      {
         @Override
         public void run()
         {
            try
            {
               doActionAsClient();
            }
            catch (Exception e)
            {
               if (PRINT_ERRORS)
                  e.printStackTrace();
            }
         }
      };
   }

   private void doActionAsServer()
   {
      try
      {
         synchronized (getConnectionConch())
         {
            if (VERBOSE)
               PrintTools.debug(this, "runServer(): Accepting on port " + port);

            serverSocket = new ServerSocket(port);
            tcpSocket = serverSocket.accept();

            setupSocket();
         }

         setupAndConnect();
         runHandlingThread();

         closeYoWhiteBoard();
      }
      catch (IOException connectIOException)
      {
         if (PRINT_ERRORS)
            PrintTools.error(this, connectIOException.getMessage());

         try
         {
            closeYoWhiteBoard();
         }
         catch (IOException closeIOException)
         {
            closeIOException.printStackTrace();
         }
      }
   }

   private void doActionAsClient()
   {
      try
      {
         synchronized (getConnectionConch())
         {
            if (VERBOSE)
               PrintTools.debug(this, "runClient(): Connecting to " + ipAddress + ":" + port);

            tcpSocket = new Socket(ipAddress, port);

            setupSocket();
         }

         setupAndConnect();
         runHandlingThread();

         closeYoWhiteBoard();
      }
      catch (ConnectException connectException)
      {
         if (PRINT_ERRORS)
         {
            PrintTools.error(this, "Failed to connect to " + ipAddress + ":" + port);
            PrintTools.error(this, connectException.getMessage());
         }
      }
      catch (IOException connectIOException)
      {
         connectIOException.printStackTrace();

         try
         {
            closeYoWhiteBoard();
         }
         catch (IOException closeIOException)
         {
            closeIOException.printStackTrace();
         }
      }
   }

   private void setupSocket() throws IOException
   {
      tcpSocket.setTcpNoDelay(true);

      DataInputStream dataInputStream = new DataInputStream(new BufferedInputStream(tcpSocket.getInputStream()));
      DataOutputStream dataOutputStream = new DataOutputStream(new BufferedOutputStream(tcpSocket.getOutputStream()));

      if (VERBOSE)
         PrintTools.debug(this, "Connected to " + tcpSocket.getRemoteSocketAddress());

      super.setDataStreams(dataInputStream, dataOutputStream);
   }

   public boolean isTCPSocketConnected()
   {
      return tcpSocket != null && tcpSocket.isConnected();
   }

   @Override
   public void closeYoWhiteBoard() throws IOException
   {
      super.closeYoWhiteBoard();

      if (serverSocket != null)
         serverSocket.close();
      if (tcpSocket != null)
         tcpSocket.close();

      serverSocket = null;
      tcpSocket = null;
   }

   public void close() throws IOException
   {
      if (activeScheduled != null)
      {
         activeScheduled.cancel(true);
         activeScheduled = null;
      }
      executorService.shutdownNow();
      closeYoWhiteBoard();
   }

   @Override
   protected void allowThrowOutStalePacketsIfYouWish()
   {
      // Do nothing. TCP won't through out stale packets. Just UDP.
   }

   public boolean isAServer()
   {
      return ipAddress == null;
   }

   public int getPort()
   {
      return port;
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/keyboard/linux/RepeatingReleasedEventsFixer.java<|end_filename|>
package us.ihmc.tools.inputDevices.keyboard.linux;

import java.awt.AWTEvent;
import java.awt.Component;
import java.awt.EventQueue;
import java.awt.Toolkit;
import java.awt.event.AWTEventListener;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.KeyEvent;
import java.util.LinkedHashMap;
import java.util.Map;

import javax.swing.Timer;

/**
 * Copied verbatim from: <a href="http://tech.stolsvik.com/2010/05/linux-java-repeats-released-keyevents.html">here</a>.
 *
 * This fixes a bug in Linux where KEY_RELEASED events are fired while KEY_PRESSED is engaged, making handling of events that need to change on this switch difficult to manage.
 *
 * @author <NAME>
 *
 */
public class RepeatingReleasedEventsFixer implements AWTEventListener
{
   private final Map<Integer, ReleasedAction> _map = new LinkedHashMap<Integer, ReleasedAction>();

   public void install()
   {
      Toolkit.getDefaultToolkit().addAWTEventListener(this, AWTEvent.KEY_EVENT_MASK);
   }

   public void remove()
   {
      Toolkit.getDefaultToolkit().removeAWTEventListener(this);
   }

   @Override
   public void eventDispatched(AWTEvent event)
   {
      assert event instanceof KeyEvent : "Shall only listen to KeyEvents, so no other events shall come here";
      assert assertEDT(); // REMEMBER THAT THIS IS SINGLE THREADED, so no need for synch.

      // ?: Is this one of our synthetic RELEASED events?
      if (event instanceof Reposted)
      {
         // -> Yes, so we shalln't process it again.
         return;
      }

      // ?: KEY_TYPED event? (We're only interested in KEY_PRESSED and KEY_RELEASED).
      if (event.getID() == KeyEvent.KEY_TYPED)
      {
         // -> Yes, TYPED, don't process.
         return;
      }

      final KeyEvent keyEvent = (KeyEvent) event;

      // ?: Is this already consumed?
      // (Note how events are passed on to all AWTEventListeners even though a previous one consumed it)
      if (keyEvent.isConsumed())
      {
         return;
      }

      // ?: Is this RELEASED? (the problem we're trying to fix!)
      if (keyEvent.getID() == KeyEvent.KEY_RELEASED)
      {
         // -> Yes, so stick in wait

         /**
          * Really just wait until "immediately", as the point is that the subsequent PRESSED shall already have been
          * posted on the event queue, and shall thus be the direct next event no matter which events are posted
          * afterwards. The code with the ReleasedAction handles if the Timer thread actually fires the action due to
          * lags, by cancelling the action itself upon the PRESSED.
          */
         final Timer timer = new Timer(2, null);
         ReleasedAction action = new ReleasedAction(keyEvent, timer);
         timer.addActionListener(action);
         timer.start();

         _map.put(Integer.valueOf(keyEvent.getKeyCode()), action);

         // Consume the original
         keyEvent.consume();
      }
      else if (keyEvent.getID() == KeyEvent.KEY_PRESSED)
      {
         // Remember that this is single threaded (EDT), so we can't have races.
         ReleasedAction action = _map.remove(Integer.valueOf(keyEvent.getKeyCode()));

         // ?: Do we have a corresponding RELEASED waiting?
         if (action != null)
         {
            // -> Yes, so dump it
            action.cancel();
         }

         // System.out.println("PRESSED: [" + keyEvent + "]");
      }
      else
      {
         throw new AssertionError("All IDs should be covered.");
      }
   }

   /**
    * The ActionListener that posts the RELEASED {@link RepostedKeyEvent} if the {@link Timer} times out (and hence the
    * repeat-action was over).
    */
   private class ReleasedAction implements ActionListener
   {
      private final KeyEvent _originalKeyEvent;
      private Timer _timer;

      ReleasedAction(KeyEvent originalReleased, Timer timer)
      {
         _timer = timer;
         _originalKeyEvent = originalReleased;
      }

      void cancel()
      {
         assert assertEDT();
         _timer.stop();
         _timer = null;
         _map.remove(Integer.valueOf(_originalKeyEvent.getKeyCode()));
      }

      @Override
      public void actionPerformed(ActionEvent e)
      {
         assert assertEDT();

         // ?: Are we already cancelled?
         // (Judging by Timer and TimerQueue code, we can theoretically be raced to be posted onto EDT by TimerQueue,
         // due to some lag, unfair scheduling)
         if (_timer == null)
         {
            // -> Yes, so don't post the new RELEASED event.
            return;
         }

         // Stop Timer and clean.
         cancel();

         // Creating new KeyEvent (we've consumed the original).
         KeyEvent newEvent = new RepostedKeyEvent((Component) _originalKeyEvent.getSource(), _originalKeyEvent.getID(), _originalKeyEvent.getWhen(), _originalKeyEvent.getModifiers(), _originalKeyEvent.getKeyCode(), _originalKeyEvent.getKeyChar(), _originalKeyEvent.getKeyLocation());

         // Posting to EventQueue.
         Toolkit.getDefaultToolkit().getSystemEventQueue().postEvent(newEvent);

         // System.out.println("Posted synthetic RELEASED [" + newEvent + "].");
      }
   }

   /**
    * Marker interface that denotes that the {@link KeyEvent} in question is reposted from some
    * {@link AWTEventListener}, including this. It denotes that the event shall not be "hack processed" by this class
    * again. (The problem is that it is not possible to state "inject this event from this point in the pipeline" - one
    * have to inject it to the event queue directly, thus it will come through this {@link AWTEventListener} too.
    */
   public interface Reposted
   {
      // marker
   }

   /**
    * Dead simple extension of {@link KeyEvent} that implements {@link Reposted}.
    */
   @SuppressWarnings("serial")
   public static class RepostedKeyEvent extends KeyEvent implements Reposted
   {
      public RepostedKeyEvent(Component source, int id, long when, int modifiers, int keyCode, char keyChar, int keyLocation)
      {
         super(source, id, when, modifiers, keyCode, keyChar, keyLocation);
      }
   }

   private static boolean assertEDT()
   {
      if (!EventQueue.isDispatchThread())
      {
         throw new AssertionError("Not EDT, but [" + Thread.currentThread() + "].");
      }

      return true;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/quadTree/PointAndDistance.java<|end_filename|>
package us.ihmc.robotics.quadTree;

import us.ihmc.euclid.tuple3D.Point3D;

public class PointAndDistance
{
   private final Point3D point;
   private double distance;

   public PointAndDistance(Point3D point, double distance)
   {
      this.point = point;
      this.distance = distance;
   }
   
   public double getDistance()
   {
      return distance;
   }
   
   public Point3D getPoint()
   {
      return point;
   }
   
   public void setPoint(Point3D point)
   {
      this.point.set(point);
   }

   public void setPointZ(double z)
   {
      this.point.setZ(z);
   }

   public void setDistance(double distance)
   {
      this.distance = distance;
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/resources/gui.css<|end_filename|>
.scroll-pane {
    -fx-background-color: transparent;
}

.heading {
    -fx-font-weight: bold;
    -fx-font-size: 20.0;
}

#parameter-name-in-tree-view {
	-fx-fill: black;
}

#modified-parameter-name-in-tree-view {
	-fx-fill: orange;
}

#default-parameter-name-in-tree-view {
	-fx-fill: darkred;
}

#registry-name-in-tree-view {
	-fx-font-weight: bold;
	-fx-fill: black;
}

#root-registry-name-in-tree-view {
	-fx-font-weight: bold;
	-fx-fill: red;
}

#parameter-name-in-tuner {
	-fx-text-fill: -fx-text-inner-color;
	-fx-font-weight: bold;
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandJointAnglePacket.java<|end_filename|>
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;

/**
       * Message used to report the current joint angles for the fingers of a hand.
       */
public class HandJointAnglePacket extends Packet<HandJointAnglePacket> implements Settable<HandJointAnglePacket>, EpsilonComparable<HandJointAnglePacket>
{
   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_;
   public byte robot_side_ = (byte) 255;
   public us.ihmc.idl.IDLSequence.Double  joint_angles_;
   public boolean connected_;
   public boolean calibrated_;

   public HandJointAnglePacket()
   {
      joint_angles_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6");

   }

   public HandJointAnglePacket(HandJointAnglePacket other)
   {
      this();
      set(other);
   }

   public void set(HandJointAnglePacket other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      joint_angles_.set(other.joint_angles_);
      connected_ = other.connected_;

      calibrated_ = other.calibrated_;

   }

   /**
            * 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 void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   public byte getRobotSide()
   {
      return robot_side_;
   }


   public us.ihmc.idl.IDLSequence.Double  getJointAngles()
   {
      return joint_angles_;
   }

   public void setConnected(boolean connected)
   {
      connected_ = connected;
   }
   public boolean getConnected()
   {
      return connected_;
   }

   public void setCalibrated(boolean calibrated)
   {
      calibrated_ = calibrated;
   }
   public boolean getCalibrated()
   {
      return calibrated_;
   }


   public static Supplier<HandJointAnglePacketPubSubType> getPubSubType()
   {
      return HandJointAnglePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HandJointAnglePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HandJointAnglePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.joint_angles_, other.joint_angles_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.connected_, other.connected_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.calibrated_, other.calibrated_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HandJointAnglePacket)) return false;

      HandJointAnglePacket otherMyClass = (HandJointAnglePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if (!this.joint_angles_.equals(otherMyClass.joint_angles_)) return false;
      if(this.connected_ != otherMyClass.connected_) return false;

      if(this.calibrated_ != otherMyClass.calibrated_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HandJointAnglePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("joint_angles=");
      builder.append(this.joint_angles_);      builder.append(", ");
      builder.append("connected=");
      builder.append(this.connected_);      builder.append(", ");
      builder.append("calibrated=");
      builder.append(this.calibrated_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-parameter-optimization/src/main/java/us/ihmc/utilities/parameterOptimization/DoubleYoVariableParameterToOptimize.java<|end_filename|>
package us.ihmc.utilities.parameterOptimization;

import us.ihmc.yoVariables.variable.YoDouble;

public class DoubleYoVariableParameterToOptimize extends DoubleParameterToOptimize
{
   private final YoDouble yoVariable;
   
   public DoubleYoVariableParameterToOptimize(double min, double max, YoDouble yoVariable, ListOfParametersToOptimize listOfParametersToOptimize)
   {
      super(yoVariable.getName(), min, max, listOfParametersToOptimize);
      this.yoVariable = yoVariable;
      
      this.setCurrentValue(yoVariable.getDoubleValue());
//      System.out.println("Creating DoubleYoVariableParameterToOptimize. min = " + min + ", max = " + max + "currentValue = " + this.getCurrentValue());
   }
   
   public void setCurrentValueGivenZeroToOne(double zeroToOne)
   {
      super.setCurrentValueGivenZeroToOne(zeroToOne);
      setParameterValueIntoYoVariable();
   }

   public void setCurrentValue(double newValue)
   {
      super.setCurrentValue(newValue);
      setParameterValueIntoYoVariable();
   }
   
   public void setCurrentValue(ParameterToOptimize parameterToOptimize)
   {
      super.setCurrentValue(parameterToOptimize);
      setParameterValueIntoYoVariable();
   }
   
   
   private void setParameterValueIntoYoVariable()
   {
      this.yoVariable.set(super.getCurrentValue());
   }

}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/tcpServer/ReconnectingTCPClient.java<|end_filename|>
package us.ihmc.communication.net.tcpServer;

import java.io.DataInputStream;
import java.io.IOException;
import java.net.Socket;
import java.net.UnknownHostException;

import us.ihmc.commons.thread.ThreadTools;

public class ReconnectingTCPClient extends ReconnectingTCPConnection
{
   private final String host;
   private final int port;

   private Socket clientSocket;

   public ReconnectingTCPClient(String host, int port)
   {
      super(2097152);
      this.host = host;
      this.port = port;
   }

   @Override
   public void disconnect()
   {
      synchronized (connectionStatusSync)
      {
         try
         {
            inputStream.close();
            outputStream.close();
            clientSocket.close();
         }
         catch (IOException e)
         {
            e.printStackTrace();
         }
         setStatus(Status.DISCONNECTED);
         notifyDisconnectedListeners();
      }
   }

   @Override
   public void connect()
   {
      if(getStatus() == Status.CLOSED)
      {
         throw new RuntimeException("Connection is closed");
      }
      
      while (getStatus() == Status.DISCONNECTED)
      {
         try
         {
            clientSocket = new Socket(host, port);
            synchronized (connectionStatusSync)
            {
               clientSocket.setTcpNoDelay(true);
               inputStream = new DataInputStream(clientSocket.getInputStream());
               outputStream = clientSocket.getOutputStream();
               reset();
               setStatus(Status.CONNECTED);
               notifyConnectedListeners();               
            }
         }
         catch (UnknownHostException e)
         {
            throw new RuntimeException(e);
         }
         catch (IOException e)
         {
            System.out.println("Reconnecting to " + host + ":" + port + "...");
            ThreadTools.sleep(1000);
         }
      }
      
   }

   @Override
   public void close()
   {
      synchronized (connectionStatusSync)
      {
         disconnect();
         setStatus(Status.CLOSED);
      }
   }

}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/concurrent/MultiThreadedRobotControlElementRunner.java<|end_filename|>
package us.ihmc.wholeBodyController.concurrent;

import us.ihmc.realtime.MonotonicTime;
import us.ihmc.realtime.PriorityParameters;
import us.ihmc.realtime.RealtimeThread;
import us.ihmc.simulationConstructionSetTools.robotController.MultiThreadedRobotControlElement;

public class MultiThreadedRobotControlElementRunner extends RealtimeThread
{
   private final MonotonicTime triggerTime = new MonotonicTime();   
   private final MonotonicTime defaultTriggerTime = new MonotonicTime(0, 100000);
   private final MultiThreadedRobotControlElement controller;
   
   public MultiThreadedRobotControlElementRunner(MultiThreadedRobotControlElement controller, PriorityParameters priorityParameters)
   {
      super(priorityParameters, controller.getName());
      this.controller = controller;
   }
   
   @Override
   public void run()
   {
      while(true)
      {
         controller.read(RealtimeThread.getCurrentMonotonicClockTime());
         controller.run();
         long currentTime = RealtimeThread.getCurrentMonotonicClockTime();
         controller.write(currentTime);
         
         // Checks if there is a non-zero sleep duration. If not, sleep for 0.1ms. This avoids busy waits and preempting kernel tasks.
         if(controller.nextWakeupTime() != Long.MIN_VALUE && controller.nextWakeupTime() >= currentTime)
         {
            triggerTime.set(0, controller.nextWakeupTime());
         }
         else
         {
            triggerTime.setToCurrentTime();
            triggerTime.add(defaultTriggerTime);
         }
         waitUntil(triggerTime);               
      }
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/FootstepMessenger.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator;

import controller_msgs.msg.dds.FootstepDataListMessage;

/**
 * This interface is part of the API of {@link ContinuousStepGenerator}.
 * <p>
 * It is used to provide a protocol for sending generated footsteps to the controller.
 * </p>
 * 
 * @author <NAME>
 */
public interface FootstepMessenger
{
   /**
    * Submits the generated footsteps to the controller.
    * 
    * @param footsteps the generated footsteps from {@code ContinuousStepGenerator}.
    */
   void submitFootsteps(FootstepDataListMessage footsteps);
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/IntegerTuner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import us.ihmc.parameterTuner.guiElements.GuiParameter;

public class IntegerTuner extends NumericTuner<Integer>
{
   public IntegerTuner(GuiParameter parameter)
   {
      super(parameter);
   }

   @Override
   public NumericSpinner<Integer> createSpinner()
   {
      return new IntegerSpinner();
   }

   @Override
   public NumericSlider<Integer> createSlider()
   {
      return new IntegerSlider();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/legConfiguration/LegConfigurationGains.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controlModules.legConfiguration;

public class LegConfigurationGains
{
   private double jointSpaceKp = Double.NaN;
   private double jointSpaceKd = Double.NaN;
   private double actuatorSpaceKp = Double.NaN;
   private double actuatorSpaceKd = Double.NaN;

   public void setJointSpaceKp(double kp)
   {
      jointSpaceKp = kp;
   }

   public void setJointSpaceKd(double kd)
   {
      jointSpaceKd = kd;
   }

   public void setActuatorSpaceKp(double kp)
   {
      actuatorSpaceKp = kp;
   }

   public void setActuatorSpaceKd(double kd)
   {
      actuatorSpaceKd = kd;
   }

   public double getJointSpaceKp()
   {
      return jointSpaceKp;
   }

   public double getJointSpaceKd()
   {
      return jointSpaceKd;
   }

   public double getActuatorSpaceKp()
   {
      return actuatorSpaceKp;
   }

   public double getActuatorSpaceKd()
   {
      return actuatorSpaceKd;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WholeBodyTrajectoryToolboxMessage.java<|end_filename|>
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;

/**
       * Message that carries all the information required to configure the IHMC whole-body trajectory planner.
       * Main usage is the IHMC WholeBodyTrajectoryToolbox.
       */
public class WholeBodyTrajectoryToolboxMessage extends Packet<WholeBodyTrajectoryToolboxMessage> implements Settable<WholeBodyTrajectoryToolboxMessage>, EpsilonComparable<WholeBodyTrajectoryToolboxMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage configuration_;
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.WaypointBasedTrajectoryMessage>  end_effector_trajectories_;
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.RigidBodyExplorationConfigurationMessage>  exploration_configurations_;
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.ReachingManifoldMessage>  reaching_manifolds_;

   public WholeBodyTrajectoryToolboxMessage()
   {
      configuration_ = new controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage();
      end_effector_trajectories_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.WaypointBasedTrajectoryMessage> (10, new controller_msgs.msg.dds.WaypointBasedTrajectoryMessagePubSubType());
      exploration_configurations_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.RigidBodyExplorationConfigurationMessage> (10, new controller_msgs.msg.dds.RigidBodyExplorationConfigurationMessagePubSubType());
      reaching_manifolds_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.ReachingManifoldMessage> (10, new controller_msgs.msg.dds.ReachingManifoldMessagePubSubType());

   }

   public WholeBodyTrajectoryToolboxMessage(WholeBodyTrajectoryToolboxMessage other)
   {
      this();
      set(other);
   }

   public void set(WholeBodyTrajectoryToolboxMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessagePubSubType.staticCopy(other.configuration_, configuration_);
      end_effector_trajectories_.set(other.end_effector_trajectories_);
      exploration_configurations_.set(other.exploration_configurations_);
      reaching_manifolds_.set(other.reaching_manifolds_);
   }

   /**
            * 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 controller_msgs.msg.dds.WholeBodyTrajectoryToolboxConfigurationMessage getConfiguration()
   {
      return configuration_;
   }


   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.WaypointBasedTrajectoryMessage>  getEndEffectorTrajectories()
   {
      return end_effector_trajectories_;
   }


   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.RigidBodyExplorationConfigurationMessage>  getExplorationConfigurations()
   {
      return exploration_configurations_;
   }


   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.ReachingManifoldMessage>  getReachingManifolds()
   {
      return reaching_manifolds_;
   }


   public static Supplier<WholeBodyTrajectoryToolboxMessagePubSubType> getPubSubType()
   {
      return WholeBodyTrajectoryToolboxMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WholeBodyTrajectoryToolboxMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WholeBodyTrajectoryToolboxMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.configuration_.epsilonEquals(other.configuration_, epsilon)) return false;
      if (this.end_effector_trajectories_.size() != other.end_effector_trajectories_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.end_effector_trajectories_.size(); i++)
         {  if (!this.end_effector_trajectories_.get(i).epsilonEquals(other.end_effector_trajectories_.get(i), epsilon)) return false; }
      }

      if (this.exploration_configurations_.size() != other.exploration_configurations_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.exploration_configurations_.size(); i++)
         {  if (!this.exploration_configurations_.get(i).epsilonEquals(other.exploration_configurations_.get(i), epsilon)) return false; }
      }

      if (this.reaching_manifolds_.size() != other.reaching_manifolds_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.reaching_manifolds_.size(); i++)
         {  if (!this.reaching_manifolds_.get(i).epsilonEquals(other.reaching_manifolds_.get(i), epsilon)) return false; }
      }


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WholeBodyTrajectoryToolboxMessage)) return false;

      WholeBodyTrajectoryToolboxMessage otherMyClass = (WholeBodyTrajectoryToolboxMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.configuration_.equals(otherMyClass.configuration_)) return false;
      if (!this.end_effector_trajectories_.equals(otherMyClass.end_effector_trajectories_)) return false;
      if (!this.exploration_configurations_.equals(otherMyClass.exploration_configurations_)) return false;
      if (!this.reaching_manifolds_.equals(otherMyClass.reaching_manifolds_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WholeBodyTrajectoryToolboxMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("configuration=");
      builder.append(this.configuration_);      builder.append(", ");
      builder.append("end_effector_trajectories=");
      builder.append(this.end_effector_trajectories_);      builder.append(", ");
      builder.append("exploration_configurations=");
      builder.append(this.exploration_configurations_);      builder.append(", ");
      builder.append("reaching_manifolds=");
      builder.append(this.reaching_manifolds_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/streamingData/StreamingDataTCPServer.java<|end_filename|>
package us.ihmc.communication.streamingData;

import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.util.ArrayList;
import java.util.concurrent.ConcurrentLinkedQueue;

import org.apache.commons.lang3.tuple.ImmutablePair;

import us.ihmc.commons.thread.ThreadTools;

public class StreamingDataTCPServer implements EstablishedAConnectionListener
{
   private boolean sendingData = true;
   private boolean justSendMostRecentObject = false;
   
   private final ArrayList<StreamingDataProducer> producers = new ArrayList<StreamingDataProducer>();
   private final ConcurrentLinkedQueue<ImmutablePair<Long, Object>> dataToSendToClients = new ConcurrentLinkedQueue<ImmutablePair<Long, Object>>();

   private final ConcurrentLinkedQueue<StreamingDataTCPConnectionToClient> clientConnections = new ConcurrentLinkedQueue<StreamingDataTCPConnectionToClient>();
   
  
   private final PersistentTCPServer server;
   
   public StreamingDataTCPServer(int port)
   {
      this.server = new PersistentTCPServer(port, this);
   }
   
   public void setJustSendMostRecentObject(boolean justSendMostRecentObject)
   {
      this.justSendMostRecentObject = justSendMostRecentObject;
   }
   
   public void registerStreamingDataProducer(StreamingDataProducer producer)
   {
      producers.add(producer);
      
      StreamingDataTCPServerConsumer consumer = new StreamingDataTCPServerConsumer(producer);
      producer.registerConsumer(consumer);
   }
   
   private class StreamingDataTCPServerConsumer implements StreamingDataConsumer
   {
      private final StreamingDataProducer producer;
      
      public StreamingDataTCPServerConsumer(StreamingDataProducer producer)
      {
         this.producer = producer;
      }
      
      public void consume(long dataIdentifier, Object dataObject)
      {
         synchronized(streamingDataTCPServerConnectionsDataSender)
         {
            dataToSendToClients.offer(new ImmutablePair<Long, Object>(dataIdentifier, dataObject)); 
            streamingDataTCPServerConnectionsDataSender.notifyAll();
         }
      }
      
      public boolean canHandle(Object object)
      {
         return true;
      }
      
      public long getDataIdentifier()
      {
         return producer.getDataIdentifier();
      }
   }

   private StreamingDataTCPServerConnectionsDataSender streamingDataTCPServerConnectionsDataSender;
   
   public void startOnAThread()
   {
      server.startOnAThread();

      if (streamingDataTCPServerConnectionsDataSender != null)
      {
         throw new RuntimeException("streamingDataTCPServerConnectionsDataSender != null. Only call startOnAThread() once!");
      }

      streamingDataTCPServerConnectionsDataSender = new StreamingDataTCPServerConnectionsDataSender();
      Thread thread = new Thread(streamingDataTCPServerConnectionsDataSender);
      thread.start();
   }
   
   public void closeAndBlockTillFullyClosed()
   {
      close();
      
      while(!isDoneClosing())
      {
         ThreadTools.sleep(10L);
      }
   }
   
   public void close()
   {
      sendingData = false;
      
      for (StreamingDataTCPConnectionToClient clientConnection : clientConnections)
      {
         clientConnection.close();
      }
      
      server.close();
   }
   
   
   
   public boolean isDoneClosing()
   {
      return server.isDoneClosing();
   }
   

   
   private class StreamingDataTCPServerConnectionsDataSender implements Runnable
   {
      private final ArrayList<StreamingDataTCPConnectionToClient> deadClients = new ArrayList<StreamingDataTCPServer.StreamingDataTCPConnectionToClient>();
      
      public void run()
      {
         while(sendingData)
         {
            synchronized(this)
            {
               if (dataToSendToClients.isEmpty())
               {
                  try
                  {
                     this.wait();
                  } 
                  catch (InterruptedException e)
                  {
                  }
               }
            }
            
            if (!dataToSendToClients.isEmpty())
            {
               ImmutablePair<Long, Object> identifierAndObject = null;

               if (justSendMostRecentObject)
               {
                  while(!dataToSendToClients.isEmpty())
                  {
                     identifierAndObject = dataToSendToClients.poll();
                  }
               }
               else
               {
                  identifierAndObject = dataToSendToClients.poll();
               }
               
               long dataIdentifier = identifierAndObject.getLeft();
               Object dataObject = identifierAndObject.getRight();
               
//               System.out.println("Sending data object to " + clientConnections.size() + " clients.");
               deadClients.clear();
               for (StreamingDataTCPConnectionToClient clientConnection : clientConnections)
               {
                  if (clientConnection.isConnected())
                  {
                     clientConnection.sendData(dataIdentifier, dataObject);
                  }
                  else
                  {
                     deadClients.add(clientConnection);
                  }
               }
               
               for (StreamingDataTCPConnectionToClient deadClient : deadClients)
               {
                  deadClient.close();
                  clientConnections.remove(deadClient);
               }
            }

         }
      }
   }
  
   
   private class StreamingDataTCPConnectionToClient
   {
      private ObjectOutputStream objectOutputStream;
      
      private boolean connected = false;
      
      public StreamingDataTCPConnectionToClient(ObjectOutputStream objectOutputStream)
      {
         this.objectOutputStream = objectOutputStream;
         connected = true;
      }
      
      public void close()
      {
         connected = false;

         if (objectOutputStream != null)
         {
            try
            {
               objectOutputStream.close();
            } 
            catch (IOException e)
            {
            }
         }

         objectOutputStream = null;
      }

      public boolean isConnected()
      {
         return connected;
      }
      
      public void sendData(long dataIdentifier, Object object)
      {
         if (connected)
         {
            try
            {
               objectOutputStream.writeLong(dataIdentifier);
               objectOutputStream.writeObject(object);
               objectOutputStream.flush();
            } 
            catch (IOException e)
            {
               connected = false;
            }
         }
      }
   }


   public void establishedAConnection(ObjectInputStream objectInputStream, ObjectOutputStream objectOutputStream)
   {
      if (objectInputStream != null)
      {
         throw new RuntimeException("Should not be getting objectInputStreams here!");
      }
      
      StreamingDataTCPConnectionToClient streamingDataTCPConnectionToClient = new StreamingDataTCPConnectionToClient(objectOutputStream);
      clientConnections.add(streamingDataTCPConnectionToClient);
   }

}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/ExitWalkingTransitionControllerStateFactory.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories;

import java.util.EnumMap;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.SmoothTransitionControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;

public class ExitWalkingTransitionControllerStateFactory implements HighLevelControllerStateFactory
{
   private SmoothTransitionControllerState transitionControllerState;
   private final HighLevelControllerName targetState;

   public ExitWalkingTransitionControllerStateFactory(HighLevelControllerName targetState)
   {
      this.targetState = targetState;
   }

   @Override
   public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper)
   {
      if (transitionControllerState == null)
      {
         EnumMap<HighLevelControllerName, HighLevelControllerStateFactory> controllerFactoriesMap = controllerFactoryHelper.getControllerFactories();
         HighLevelControllerStateFactory initialControllerStateFactory = controllerFactoriesMap.get(HighLevelControllerName.WALKING);
         HighLevelControllerStateFactory finalControllerStateFactory = controllerFactoriesMap.get(targetState);

         HighLevelControllerState initialControllerState = initialControllerStateFactory.getOrCreateControllerState(controllerFactoryHelper);
         HighLevelControllerState finalControllerState = finalControllerStateFactory.getOrCreateControllerState(controllerFactoryHelper);
         OneDoFJointBasics[] controlledJoints = controllerFactoryHelper.getHighLevelHumanoidControllerToolbox().getControlledOneDoFJoints();

         transitionControllerState = new SmoothTransitionControllerState("exitWalking", HighLevelControllerName.EXIT_WALKING, initialControllerState,
                                                                         finalControllerState, controlledJoints, controllerFactoryHelper.getHighLevelControllerParameters());
      }

      return transitionControllerState;
   }

   @Override
   public HighLevelControllerName getStateEnum()
   {
      return HighLevelControllerName.EXIT_WALKING;
   }
}


<|start_filename|>ihmc-mocap/src/main/java/optiTrack/MocapMarker.java<|end_filename|>
package optiTrack;

import us.ihmc.euclid.tuple3D.Vector3D;

public class MocapMarker
{
   private int id;
   private Vector3D position;
   private float markerSize;

   public MocapMarker(int id, Vector3D position, float markerSize)
   {
      this.id = id;
      this.position = position;
      this.markerSize = markerSize;
   }

   public int getId()
   {
      return id;
   }

   public Vector3D getPosition()
   {
      return position;
   }
   
   public float getMarkerSize()
   {
      return markerSize;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/sensors/blackfly/BlackFlyParameterSetter.java<|end_filename|>
package us.ihmc.avatar.sensors.blackfly;

import java.util.HashMap;
import java.util.Map;

import controller_msgs.msg.dds.BlackFlyParameterPacket;
import controller_msgs.msg.dds.UIConnectedPacket;
import us.ihmc.commons.PrintTools;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.utilities.ros.RosDynamicReconfigure;
import us.ihmc.utilities.ros.RosMainNode;

public class BlackFlyParameterSetter implements PacketConsumer<BlackFlyParameterPacket>
{
   private final PacketCommunicator packetCommunicator;
   private final boolean DEBUG = false;
   RosDynamicReconfigure dynamicReconfigureClient;

   String cameraNodeName;
   RobotSide side;

   public BlackFlyParameterSetter(RosMainNode rosMainNode, RobotSide side, String cameraNode, PacketCommunicator packetCommunicator)
   {
      this.packetCommunicator = packetCommunicator;
      this.dynamicReconfigureClient = new RosDynamicReconfigure(cameraNode, rosMainNode);
      this.side = side;
      this.cameraNodeName = cameraNode;
      packetCommunicator.attachListener(BlackFlyParameterPacket.class, this);
      setupUIOnConnectResponder();

   }

   public void sendDeviceSettingToUI(Map<String, Object> params)
   {
      BlackFlyParameterPacket packet = HumanoidMessageTools.createBlackFlyParameterPacket(false, (Double) params.get("gain"), (Double) params.get("exposure"),
            (Double) params.get("frame_rate"), (Double) params.get("shutter_speed"), (Boolean) params.get("auto_exposure"), (Boolean) params.get("auto_gain"),
            (Boolean) params.get("auto_shutter"), side);
      packetCommunicator.send(packet);
      PrintTools.debug(DEBUG, this, "packet to UI " + packet);
   }

   public Map<String, Object> setBlackFlyParameters(BlackFlyParameterPacket packet)
   {

      PrintTools.debug(DEBUG, this, "packet fr UI " + packet);
      Map<String, Object> parameters = new HashMap<>();
      parameters.put("auto_exposure", packet.getAutoExposure());
      parameters.put("auto_gain", packet.getAutoGain());
      parameters.put("auto_shutter", packet.getAutoShutter());
      parameters.put("exposure", packet.getExposure());
      parameters.put("gain", packet.getGain());
      parameters.put("shutter_speed", packet.getShutter());
      parameters.put("frame_rate", packet.getFrameRate());
      return dynamicReconfigureClient.setParameters(parameters);
   }

   public void setupUIOnConnectResponder()
   {

      packetCommunicator.attachListener(UIConnectedPacket.class, new PacketConsumer<UIConnectedPacket>()
      {
         @Override
         public void receivedPacket(UIConnectedPacket packet)
         {
            sendDeviceSettingToUI(dynamicReconfigureClient.getParameters());
            PrintTools.info(this, "UIConnected - send parameters");
         }
      });
   }

   public void receivedPacket(final BlackFlyParameterPacket packet)
   {

      if (dynamicReconfigureClient.isConnected())
      {
         if (packet.getFromUi() && packet.getRobotSide() == BlackFlyParameterSetter.this.side.toByte()) //avoid hearing my own packet
         {
            new Thread("BlackflyDynamicReconfigureSetter")
            {
               public void run()
               {
                  Map<String, Object> result = setBlackFlyParameters(packet);
                  sendDeviceSettingToUI(result);
               }
            }.start();
         }
         else
         {
            PrintTools.info(this, "blackfly dynamic reconfigure serivce not ready, skipping " + packet);
         }

      }

   }
}


<|start_filename|>ihmc-model-file-loader/src/main/java/us/ihmc/modelFileLoaders/SdfLoader/SDFModelVisual.java<|end_filename|>
package us.ihmc.modelFileLoaders.SdfLoader;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.graphicsDescription.Graphics3DObject;

public class SDFModelVisual extends Graphics3DObject
{
   private final List<String> resourceDirectories;
   
   private final HashSet<SDFLinkHolder> addedLinks = new HashSet<>();
   
   public SDFModelVisual(GeneralizedSDFRobotModel generalizedSDFRobotModel)
   {
      this(generalizedSDFRobotModel, false);
   }
   
   public SDFModelVisual(GeneralizedSDFRobotModel generalizedSDFRobotModel, boolean useCollisionMeshes)
   {
      resourceDirectories = generalizedSDFRobotModel.getResourceDirectories();
      ArrayList<SDFLinkHolder> rootLinks = generalizedSDFRobotModel.getRootLinks();
      
      RigidBodyTransform modelTransform = generalizedSDFRobotModel.getTransformToRoot();
      for(SDFLinkHolder link : rootLinks)
      {  
         recursivelyAddLinks(link, modelTransform, useCollisionMeshes);
      }
   }
   
   private void recursivelyAddLinks(SDFLinkHolder link, RigidBodyTransform modelTransform, boolean useCollisionMeshes)
   {
      if(addedLinks.contains(link))
      {
         return;
      }
      addedLinks.add(link);
      
      if(link.getVisuals() != null)
      {
         
         RigidBodyTransform transformToModel = new RigidBodyTransform(modelTransform);
         transformToModel.multiply(link.getTransformFromModelReferenceFrame());
         
         SDFGraphics3DObject sdfGraphics3DObject;
         if(useCollisionMeshes)
         {
            sdfGraphics3DObject = new SDFGraphics3DObject(link.getCollisions(), resourceDirectories, transformToModel);
         }
         else
         {
            sdfGraphics3DObject = new SDFGraphics3DObject(link.getVisuals(), resourceDirectories, transformToModel);
         }
         getGraphics3DInstructions().addAll(sdfGraphics3DObject.getGraphics3DInstructions());
         
      }
      
      for(SDFJointHolder joint: link.getChildren())
      {
         recursivelyAddLinks(joint.getChildLinkHolder(), modelTransform, useCollisionMeshes);
      }
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/StateVisitedTwiceSimulationDoneCriterion.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util;

import java.util.ArrayList;
import java.util.HashMap;

import us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine.State;
import us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine.StateChangeRecorder;
import us.ihmc.simulationconstructionset.SimulationDoneCriterion;


public class StateVisitedTwiceSimulationDoneCriterion implements SimulationDoneCriterion
{
   private final StateChangeRecorder stateChangeRecorder;
   private final ArrayList<State<?>> exceptions = new ArrayList<>();
   private State<?> stateVisitedTwice;
   private boolean isEnabled = true;

   public StateVisitedTwiceSimulationDoneCriterion(StateChangeRecorder<?> stateChangeRecorder)
   {
      this.stateChangeRecorder = stateChangeRecorder;
   }

   @Override
   public boolean isSimulationDone()
   {
      if (isEnabled)
      {
         HashMap<State, ArrayList<Double>> statesAndSwitchTimes = stateChangeRecorder.getStatesAndSwitchTimes();

         for (State<?> state : exceptions)
         {
            statesAndSwitchTimes.remove(state);
         }

         for (State<?> state : statesAndSwitchTimes.keySet())
         {
            if (statesAndSwitchTimes.get(state).size() > 1)
            {
               stateVisitedTwice = state;

               return true;
            }
         }
      }

      return false;
   }

   public State<?> getStateVisitedTwice()
   {
      return stateVisitedTwice;
   }

   public void addException(State<?> exceptionState)
   {
      this.exceptions.add(exceptionState);
   }

   public void enable()
   {
      isEnabled = true;
   }

   public void disable()
   {
      isEnabled = false;
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SpatialVectorMessage.java<|end_filename|>
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;

/**
       * Represents a 6D vector composed of a 3D anuglar vector and a 3D linear vector.
       */
public class SpatialVectorMessage extends Packet<SpatialVectorMessage> implements Settable<SpatialVectorMessage>, EpsilonComparable<SpatialVectorMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.tuple3D.Vector3D angular_part_;
   public us.ihmc.euclid.tuple3D.Vector3D linear_part_;

   public SpatialVectorMessage()
   {
      angular_part_ = new us.ihmc.euclid.tuple3D.Vector3D();
      linear_part_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public SpatialVectorMessage(SpatialVectorMessage other)
   {
      this();
      set(other);
   }

   public void set(SpatialVectorMessage other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.angular_part_, angular_part_);
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.linear_part_, linear_part_);
   }

   /**
            * 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 us.ihmc.euclid.tuple3D.Vector3D getAngularPart()
   {
      return angular_part_;
   }


   public us.ihmc.euclid.tuple3D.Vector3D getLinearPart()
   {
      return linear_part_;
   }


   public static Supplier<SpatialVectorMessagePubSubType> getPubSubType()
   {
      return SpatialVectorMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SpatialVectorMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SpatialVectorMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.angular_part_.epsilonEquals(other.angular_part_, epsilon)) return false;
      if (!this.linear_part_.epsilonEquals(other.linear_part_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SpatialVectorMessage)) return false;

      SpatialVectorMessage otherMyClass = (SpatialVectorMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.angular_part_.equals(otherMyClass.angular_part_)) return false;
      if (!this.linear_part_.equals(otherMyClass.linear_part_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SpatialVectorMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("angular_part=");
      builder.append(this.angular_part_);      builder.append(", ");
      builder.append("linear_part=");
      builder.append(this.linear_part_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/beetle/parameters/HexapodControllerParameters.java<|end_filename|>
package us.ihmc.exampleSimulations.beetle.parameters;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotics.controllers.pidGains.PIDSE3Gains;
import us.ihmc.robotics.controllers.pidGains.YoPIDSE3Gains;
import us.ihmc.robotics.screwTheory.SelectionMatrix6D;

public interface HexapodControllerParameters
{
   double getTransferTime();

   double getSwingTime();

   double getSwingXYProportionalGain();

   double getSwingZProportionalGain();

   PIDSE3Gains getBodySpatialGains();

   void getBodySpatialLinearQPWeight(Vector3D linearWeight);

   void getBodySpatialAngularQPWeight(Vector3D angularWeight);

   SelectionMatrix6D getBodySpatialSelectionMatrix();

   YoPIDSE3Gains getFootGains();
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/PreviousStateProvider.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

@Deprecated
public interface PreviousStateProvider<E extends Enum<E>, T extends State<E>>
{
   public abstract T getPreviousState();
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/FootstepOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder;

/**
 * Created by agrabertilton on 2/19/15.
 */
public abstract class FootstepOverheadPath implements ReferenceFrameHolder
{
   public abstract FramePose2D getPoseAtDistance(double distanceAlongPath);

   public abstract double getTotalDistance();

   public abstract FootstepOverheadPath changeFrameCopy(ReferenceFrame desiredFrame);
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/packets/PlanarRegionsRequestType.java<|end_filename|>
package us.ihmc.communication.packets;

public enum PlanarRegionsRequestType
{
   SINGLE_UPDATE, CONTINUOUS_UPDATE, STOP_UPDATE, CLEAR;

   public static final PlanarRegionsRequestType[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static PlanarRegionsRequestType fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/sphereICPControl/controllers/GenericSphereController.java<|end_filename|>
package us.ihmc.exampleSimulations.sphereICPControl.controllers;

import us.ihmc.euclid.tuple3D.Vector3D;

public interface GenericSphereController
{
   public void doControl();
   public Vector3D getForces();
}


<|start_filename|>ihmc-simulation-toolkit/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/QuaternionProvider.java<|end_filename|>
package us.ihmc.sensorProcessing.simulatedSensors;

import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;

public interface QuaternionProvider
{
   QuaternionReadOnly getValue();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/conditionBasedStateMachine/StateChangedListener.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.conditionBasedStateMachine;

@Deprecated
public interface StateChangedListener <E extends Enum<E>>
{
   public abstract void stateChanged(State<E> oldState, State<E> newState, double time);
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/ICPControlGainsReadOnly.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.capturePoint;

import us.ihmc.robotics.controllers.pidGains.IntegratorGainsReadOnly;

public interface ICPControlGainsReadOnly extends IntegratorGainsReadOnly
{
   double getKpParallelToMotion();

   double getKpOrthogonalToMotion();

   default double getFeedbackPartMaxValueParallelToMotion()
   {
      return Double.POSITIVE_INFINITY;
   }

   default double getFeedbackPartMaxValueOrthogonalToMotion()
   {
      return Double.POSITIVE_INFINITY;
   }

   default double getFeedbackPartMaxRate()
   {
      return Double.POSITIVE_INFINITY;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controllerAPI/input/MessageCollectionMessenger.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controllerAPI.input;

import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import controller_msgs.msg.dds.MessageCollection;
import controller_msgs.msg.dds.MessageCollectionNotification;
import us.ihmc.commons.PrintTools;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.Packet;

/**
 * Tool for setting up and sending collections of messages that are to be processed by the
 * controller at the same time.
 */
@SuppressWarnings("rawtypes")
public class MessageCollectionMessenger
{
   /**
    * Maximum duration in milliseconds to wait for a {@link MessageCollectionNotification} once a
    * {@link MessageCollection} has been sent.
    */
   private static final int NOTIFICATION_TIMEOUT = 5000;

   private long sequenceID;

   private PublishingTask publishingTask = null;

   private MessageCollection collection = new MessageCollection();
   private List<Packet<?>> packets = new ArrayList<>();
   private final Map<Class, Method> sequenceIDSetterMap = new HashMap<>();

   /**
    * Creates a new messenger ready to register a new collection of messages to be sent to the
    * controller.
    */
   public MessageCollectionMessenger()
   {
      this(500); // FIXME: There is a type mismatch between a long in Java and the unsigned long in DDS.
   }

   /**
    * Creates a new messenger ready to register a new collection of messages to be sent to the
    * controller.
    * 
    * @param startSequenceID the ID of the collection message and start ID for all the messages of
    *           the collection.
    */
   public MessageCollectionMessenger(long startSequenceID)
   {
      sequenceID = startSequenceID;
      collection.setSequenceId(sequenceID);
      incrementSequenceID();
   }

   /**
    * Convenience method for adding several messages.
    * 
    * @param messages the messages to add.
    * @see #addMessage(Packet)
    */
   public void addMessages(Iterable<Packet<?>> messages)
   {
      messages.forEach(this::addMessage);
   }

   /**
    * Convenience method for adding several messages.
    * 
    * @param messages the messages to add.
    * @see #addMessage(Packet)
    */
   public void addMessages(Packet<?>... messages)
   {
      for (Packet<?> message : messages)
      {
         addMessage(message);
      }
   }

   /**
    * Add a message to the current collection.
    * <p>
    * Only messages with a field "{@code long sequence_id_}" and a setter
    * "{@code setUniqueId(long)}" can be added to a collection.
    * </p>
    * 
    * @param message the message to add.
    * @return {@code true} if the message was properly added to the collection, {@code false}
    *         otherwise.
    */
   public boolean addMessage(Packet<?> message)
   {
      Class<? extends Packet> messageType = message.getClass();

      Method sequenceIDSetter = sequenceIDSetterMap.get(messageType);

      if (sequenceIDSetter == null)
      {
         try
         {
            sequenceIDSetter = messageType.getMethod("setSequenceId", long.class);
            sequenceIDSetterMap.put(messageType, sequenceIDSetter);
         }
         catch (NoSuchMethodException | SecurityException e)
         {
            PrintTools.error("The message type: " + messageType.getSimpleName()
                  + " cannot be sent with a MessageCollection, it needs to declare a sequenceID field.");
            return false;
         }
      }

      try
      {
         sequenceIDSetter.invoke(message, sequenceID);
      }
      catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException e)
      {
         throw new RuntimeException("Something went wrong when setting the sequenceID for the message " + messageType.getSimpleName(), e);
      }

      packets.add(message);
      collection.getSequences().add(sequenceID);

      incrementSequenceID();

      return true;
   }

   private void incrementSequenceID()
   {
      sequenceID++;

      if (sequenceID == 0)
         sequenceID++;
   }

   /**
    * Sends, in a safe manner, first a {@code MessageCollection} to indicate that the following
    * messages should be gathered and processed at the same time, and then the collection of
    * messages that have been added via {@link #addMessage(Packet)}.
    * <p>
    * This method is safer than {@link #sendMessageCollectionUnsafe(Messenger)} as before actually
    * sending the messages, it will wait for first receiving a
    * {@link MessageCollectionNotification}. The notification indicates that the controller is ready
    * to collect the collection of messages.
    * </p>
    * <p>
    * If the controller does not send a notification within a given duration, the task is cancelled
    * and the internal memory of {@code this} is cleared.
    * </p>
    * <p>
    * If this method is executed on the local thread, i.e. {@code runOnThread = false}, the
    * execution will be temporarily paused to wait for a notification. If this is undesirable,
    * running the task on a different thread, i.e. {@code runOnThread = true} will prevent this
    * effect.
    * </p>
    * 
    * @param packetCommunicator used to communicate with the controller.
    * @param runOnThread whether this method should be executed on a separate thread.
    * @see #NOTIFICATION_TIMEOUT
    */
   public void sendMessageCollectionSafe(PacketCommunicator packetCommunicator, boolean runOnThread)
   {
      PacketConsumer<MessageCollectionNotification> listener = this::receivedNotification;
      packetCommunicator.attachListener(MessageCollectionNotification.class, listener);

      sendMessageCollectionSafe(packetCommunicator::send, runOnThread, () -> packetCommunicator.detachListener(MessageCollectionNotification.class, listener));
   }

   /**
    * Sends, in a safe manner, first a {@code MessageCollection} to indicate that the following
    * messages should be gathered and processed at the same time, and then the collection of
    * messages that have been added via {@link #addMessage(Packet)}.
    * <p>
    * When using this method, the user should register the method
    * {@link #receivedNotification(MessageCollectionNotification)} as a listener to the
    * communication, so {@code this} can receive the notification from the controller.
    * </p>
    * 
    * @param messenger the protocol for sending messages to the controller.
    * @param runOnThread whether this method should be executed on a separate thread.
    * @see #sendMessageCollectionSafe(PacketCommunicator, boolean)
    */
   public void sendMessageCollectionSafe(Messenger messenger, boolean runOnThread)
   {
      sendMessageCollectionSafe(messenger, runOnThread, null);
   }

   private void sendMessageCollectionSafe(Messenger messenger, boolean runOnThread, Runnable postProcess)
   {
      publishingTask = new PublishingTask(messenger, collection, packets, postProcess);

      if (runOnThread)
      {
         Thread thread = new Thread(publishingTask, getClass().getSimpleName());
         thread.setDaemon(true);
         thread.start();
      }
      else
      {
         publishingTask.run();
      }
   }

   /**
    * Sends immediately a {@code MessageCollection} directly followed by the collection of messages
    * to the controller.
    * <p>
    * This method strongly rely on the reliability of the communication and expect the
    * {@code MessageCollection} to arrive first. Prefer using
    * {@link #sendMessageCollectionSafe(PacketCommunicator, boolean)}.
    * </p>
    * 
    * @param messenger the protocol for sending messages to the controller.
    */
   public void sendMessageCollectionUnsafe(Messenger messenger)
   {
      messenger.sendMessage(collection);

      for (int i = 0; i < packets.size(); i++)
      {
         messenger.sendMessage(packets.get(i));
      }
      reset();
   }

   /**
    * Notifies {@code this} that the controller just received a {@link MessageCollection}.
    * <p>
    * This method triggers the publishing of the collection of messages when using
    * {@link #sendMessageCollectionSafe(Messenger, boolean)}.
    * </p>
    * 
    * @param notification the notification.
    */
   public void receivedNotification(MessageCollectionNotification notification)
   {
      if (collection == null)
         return;

      if (notification.getMessageCollectionSequenceId() == collection.getSequenceId() && publishingTask != null)
      {
         synchronized (publishingTask.notificationSync)
         {
            publishingTask.hasReceivedNotification = true;
            publishingTask.notificationSync.notify();
         }
      }
   }

   private void reset()
   {
      collection.getSequences().reset();
      incrementSequenceID();
      collection.setSequenceId(sequenceID);
      incrementSequenceID();
      packets.clear();
   }

   /**
    * Implements this interface to provide a protocol for sending messages.
    * 
    * @author <NAME>
    */
   public static interface Messenger
   {
      /**
       * Request for sending the given message.
       * 
       * @param message the message to be sent.
       */
      void sendMessage(Packet<?> message);
   }

   private class PublishingTask implements Runnable
   {
      private volatile boolean hasReceivedNotification = false;
      private final Object notificationSync = new Object();
      private final Messenger messenger;
      private final MessageCollection collection;
      private final List<Packet<?>> packets;
      private final Runnable postProcess;

      public PublishingTask(Messenger messenger, MessageCollection collection, List<Packet<?>> packets, Runnable postProcess)
      {
         this.messenger = messenger;
         this.collection = collection;
         this.packets = packets;
         this.postProcess = postProcess;
      }

      @Override
      public void run()
      {
         synchronized (notificationSync)
         {
            messenger.sendMessage(collection);

            try
            {
               notificationSync.wait(NOTIFICATION_TIMEOUT);
            }
            catch (InterruptedException e)
            {
               e.printStackTrace();
               collection.getSequences().reset();
               packets.clear();
               return;
            }

            if (hasReceivedNotification)
            {
               for (int i = 0; i < packets.size(); i++)
               {
                  messenger.sendMessage(packets.get(i));
               }
            }
            else
            {
               PrintTools.error(getClass().getSimpleName() + " did not receive " + MessageCollectionNotification.class.getSimpleName()
                     + ". Clearing the internal messages.");
            }

            hasReceivedNotification = false;
            reset();
            if (postProcess != null)
               postProcess.run();
         }
      }
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/whiteBoard/YoWhiteBoardListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.whiteBoard;

public interface YoWhiteBoardListener
{
   public abstract void receivedWhiteBoardData();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LocalizationStatusPacket.java<|end_filename|>
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;

/**
       * Message part of the localization module
       */
public class LocalizationStatusPacket extends Packet<LocalizationStatusPacket> implements Settable<LocalizationStatusPacket>, EpsilonComparable<LocalizationStatusPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public double overlap_;
   public java.lang.StringBuilder status_;

   public LocalizationStatusPacket()
   {
      status_ = new java.lang.StringBuilder(255);
   }

   public LocalizationStatusPacket(LocalizationStatusPacket other)
   {
      this();
      set(other);
   }

   public void set(LocalizationStatusPacket other)
   {
      sequence_id_ = other.sequence_id_;

      overlap_ = other.overlap_;

      status_.setLength(0);
      status_.append(other.status_);

   }

   /**
            * 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 void setOverlap(double overlap)
   {
      overlap_ = overlap;
   }
   public double getOverlap()
   {
      return overlap_;
   }

   public void setStatus(java.lang.String status)
   {
      status_.setLength(0);
      status_.append(status);
   }

   public java.lang.String getStatusAsString()
   {
      return getStatus().toString();
   }
   public java.lang.StringBuilder getStatus()
   {
      return status_;
   }


   public static Supplier<LocalizationStatusPacketPubSubType> getPubSubType()
   {
      return LocalizationStatusPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return LocalizationStatusPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(LocalizationStatusPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.overlap_, other.overlap_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.status_, other.status_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof LocalizationStatusPacket)) return false;

      LocalizationStatusPacket otherMyClass = (LocalizationStatusPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.overlap_ != otherMyClass.overlap_) return false;

      if (!us.ihmc.idl.IDLTools.equals(this.status_, otherMyClass.status_)) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("LocalizationStatusPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("overlap=");
      builder.append(this.overlap_);      builder.append(", ");
      builder.append("status=");
      builder.append(this.status_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulatedSensors/PerfectSimulatedIMURawSensorReader.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulatedSensors;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly;
import us.ihmc.robotics.math.corruptors.NoisyYoDouble;
import us.ihmc.robotics.sensors.RawIMUSensorsInterface;

public class PerfectSimulatedIMURawSensorReader extends SimulatedIMURawSensorReader
{
   public PerfectSimulatedIMURawSensorReader(RawIMUSensorsInterface rawSensors, int imuIndex, RigidBodyBasics rigidBody, ReferenceFrame imuFrame, RigidBodyBasics rootBody, SpatialAccelerationReadOnly rootAcceleration)
   {
      super(rawSensors, imuIndex, rigidBody, imuFrame, rootBody, rootAcceleration);
   }

   @Override
   protected void initializeNoise()
   {
      rotationMatrix.setIsNoisy(false);
      setIsNoisyToFalse(accelList);
      setIsNoisyToFalse(gyroList);
      setIsNoisyToFalse(compassList);
   }
   
   @Override
   protected void simulateIMU()
   {
      rotationMatrix.update(perfM00.getDoubleValue(), perfM01.getDoubleValue(), perfM02.getDoubleValue(), perfM10.getDoubleValue(), perfM11.getDoubleValue(), perfM12.getDoubleValue(), perfM20.getDoubleValue(), perfM21.getDoubleValue(), perfM22.getDoubleValue());
      
      accelX.update();
      accelY.update();
      accelZ.update();
      
      gyroX.update();
      gyroY.update();
      gyroZ.update();
      
      compassX.update();
      compassY.update();
      compassZ.update();
   }

   private void setIsNoisyToFalse(NoisyYoDouble[] list)
   {
      for (NoisyYoDouble i : list)
      {
         i.setIsNoisy(false);
      }
   }
}

<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/RosSupportPolygonPublisher.java<|end_filename|>
package us.ihmc.avatar.ros;

import java.util.List;

import org.ros.message.MessageFactory;
import org.ros.node.NodeConfiguration;

import ihmc_msgs.Point2dRosMessage;
import ihmc_msgs.SupportPolygonRosMessage;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Point2D32;
import us.ihmc.utilities.ros.publisher.RosTopicPublisher;

/**
 *
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class RosSupportPolygonPublisher extends RosTopicPublisher<SupportPolygonRosMessage>
{
   private final MessageFactory messageFactory = NodeConfiguration.newPrivate().getTopicMessageFactory();

   public RosSupportPolygonPublisher(boolean latched)
   {
      super(SupportPolygonRosMessage._TYPE, latched);
   }

   public void publish(Point2D[] points, int numberOfVertices)
   {
      SupportPolygonRosMessage message = getMessage();
      message.setNumberOfVertices(numberOfVertices);
      List<Point2dRosMessage> vertices = message.getVertices();
      vertices.clear();
      for (int i = 0; i < numberOfVertices; i++)
      {
         Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE);
         point2d.setX((float) points[i].getX());
         point2d.setY((float) points[i].getY());
         vertices.add(point2d);
      }
      publish(message);
   }

   public void publish(Point2D32[] points, int numberOfVertices)
   {
      SupportPolygonRosMessage message = getMessage();
      message.setNumberOfVertices(numberOfVertices);
      List<Point2dRosMessage> vertices = message.getVertices();
      vertices.clear();
      for (int i = 0; i < numberOfVertices; i++)
      {

         Point2dRosMessage point2d = messageFactory.newFromType(Point2dRosMessage._TYPE);
         point2d.setX(points[i].getX());
         point2d.setY(points[i].getY());
         vertices.add(point2d);

      }
      publish(message);
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/voxelPrimitiveShapes/SphereVoxelShape.java<|end_filename|>
package us.ihmc.avatar.reachabilityMap.voxelPrimitiveShapes;

import java.awt.Color;

import us.ihmc.commons.MathTools;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameQuaternion;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.robotics.geometry.SpiralBasedAlgorithm;

/**
 * SphereVoxelShape creates N points uniformly distributed on the surface of a sphere.
 * For each point, a ray, which goes from the point on the surface of the sphere to its origin, is generated.
 * For each ray M rotations are generated by computing the orientation aligning the x-axis to the ray and transforming this orientation by M rotations around the ray.
 * 
 * This class is meant to help discretizing the 3D space of orientations and also to simulate the different possibilities for grasping a spherical object.
 *
 */
public class SphereVoxelShape
{
   public enum SphereVoxelType {graspOrigin, graspAroundSphere};
   private final Quaternion[][] rotations;
   private final Point3D[] pointsOnSphere;
   /** Origin of the sphere in the current voxel coordinate. Should probably always be set to zero. */
   private final Point3D sphereOrigin = new Point3D();
   private final double voxelSize;

   private final int numberOfRays;
   private final int numberOfRotationsAroundRay;

   private final SphereVoxelType type;
   private final ReferenceFrame parentFrame;
   
   public SphereVoxelShape(ReferenceFrame parentFrame, double voxelSize, int numberOfRays, int numberOfRotationsAroundRay, SphereVoxelType type)
   {
      this.voxelSize = voxelSize;
      this.parentFrame = parentFrame;
      this.type = type;
      this.numberOfRays = numberOfRays;
      this.numberOfRotationsAroundRay = numberOfRotationsAroundRay;

      pointsOnSphere = SpiralBasedAlgorithm.generatePointsOnSphere(sphereOrigin, voxelSize, numberOfRays);
      rotations = SpiralBasedAlgorithm.generateOrientations(numberOfRays, numberOfRotationsAroundRay);
   }

   public int getNumberOfRays()
   {
      return numberOfRays;
   }

   public int getNumberOfRotationsAroundRay()
   {
      return numberOfRotationsAroundRay;
   }

   public void getRay(Vector3D rayToPack, int rayIndex)
   {
      MathTools.checkIntervalContains(rayIndex, 0, numberOfRays - 1);

      rayToPack.sub(sphereOrigin, pointsOnSphere[rayIndex]);
      rayToPack.normalize();
   }

   public void getOrientation(FrameQuaternion orientation, int rayIndex, int rotationAroundRayIndex)
   {
      MathTools.checkIntervalContains(rayIndex, 0, numberOfRays - 1);
      MathTools.checkIntervalContains(rotationAroundRayIndex, 0, numberOfRotationsAroundRay - 1);

      orientation.setIncludingFrame(parentFrame, rotations[rayIndex][rotationAroundRayIndex]);
   }

   public void getPose(FrameVector3D translationFromVoxelOrigin, FrameQuaternion orientation, int rayIndex, int rotationAroundRayIndex)
   {
      MathTools.checkIntervalContains(rayIndex, 0, numberOfRays - 1);
      MathTools.checkIntervalContains(rotationAroundRayIndex, 0, numberOfRotationsAroundRay - 1);

      if (type == SphereVoxelType.graspAroundSphere)
         translationFromVoxelOrigin.setIncludingFrame(parentFrame, pointsOnSphere[rayIndex]);
      else
         translationFromVoxelOrigin.setToZero(parentFrame);
      orientation.setIncludingFrame(parentFrame, rotations[rayIndex][rotationAroundRayIndex]);
   }

   public Point3D[] getPointsOnSphere()
   {
      return pointsOnSphere;
   }

   public Graphics3DObject createVisualization(FramePoint3D voxelLocation, double scale, double reachabilityValue)
   {
      ReferenceFrame originalFrame = voxelLocation.getReferenceFrame();
      voxelLocation.changeFrame(ReferenceFrame.getWorldFrame());

      Graphics3DObject voxelViz = new Graphics3DObject();

      AppearanceDefinition appearance = YoAppearance.RGBColorFromHex(Color.HSBtoRGB((float) (0.7 * reachabilityValue), 1.0f, 1.0f));

      voxelViz.translate(voxelLocation.getX(), voxelLocation.getY(), voxelLocation.getZ());
      voxelViz.addSphere(scale * voxelSize / 2.0, appearance);

      voxelLocation.changeFrame(originalFrame);

      return voxelViz;
   }
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/objectDetector/ValveDetectorProcess.java<|end_filename|>
package us.ihmc.ihmcPerception.objectDetector;

import java.awt.Rectangle;
import java.util.List;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.stream.IntStream;

import org.apache.commons.lang3.tuple.Pair;

import controller_msgs.msg.dds.BoundingBoxesPacket;
import controller_msgs.msg.dds.HeatMapPacket;
import controller_msgs.msg.dds.ObjectDetectorResultPacket;
import controller_msgs.msg.dds.VideoPacket;
import us.ihmc.commons.thread.ThreadTools;
import us.ihmc.communication.configuration.NetworkParameterKeys;
import us.ihmc.communication.configuration.NetworkParameters;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.MessageTools;
import us.ihmc.communication.packets.PacketDestination;
import us.ihmc.communication.producers.JPEGDecompressor;
import us.ihmc.communication.util.NetworkPorts;
import us.ihmc.humanoidRobotics.kryo.IHMCCommunicationKryoNetClassList;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class ValveDetectorProcess implements PacketConsumer<VideoPacket>
{
   private final PacketCommunicator valveDetectionServer = PacketCommunicator
         .createTCPPacketCommunicatorServer(NetworkPorts.VALVE_DETECTOR_SERVER_PORT, new IHMCCommunicationKryoNetClassList());
   private final PacketCommunicator detectionStatusClient = PacketCommunicator
         .createTCPPacketCommunicatorClient(NetworkParameters.getHost(NetworkParameterKeys.networkManager), NetworkPorts.VALVE_DETECTOR_FEEDBACK_PORT, new IHMCCommunicationKryoNetClassList());
   private final JPEGDecompressor jpegDecompressor = new JPEGDecompressor();
   private final ValveDetector valveDetector;

   private final ExecutorService executorService = Executors.newSingleThreadExecutor(ThreadTools.getNamedThreadFactory("ValveDetector"));

   public ValveDetectorProcess() throws Exception
   {
      this.valveDetector = new ValveDetector();

      valveDetectionServer.attachListener(VideoPacket.class, this);
      valveDetectionServer.connect();

      detectionStatusClient.connect();
   }

   @Override
   public void receivedPacket(VideoPacket packet)
   {
      executorService.submit(() ->
                             {
                                Pair<List<Rectangle>, ValveDetector.HeatMap> rectanglesAndHeatMaps = valveDetector
                                      .detect(jpegDecompressor.decompressJPEGDataToBufferedImage(packet.getData().toArray()));

                                rectanglesAndHeatMaps.getLeft().sort((r1, r2) -> -Integer.compare(r1.width * r1.height, r2.width * r2.height));

                                HeatMapPacket heatMapPacket = new HeatMapPacket();
                                heatMapPacket.setWidth(rectanglesAndHeatMaps.getRight().w);
                                heatMapPacket.setHeight(rectanglesAndHeatMaps.getRight().h);
                                heatMapPacket.getData().add(rectanglesAndHeatMaps.getRight().data);
                                heatMapPacket.setName("Valve");

                                int[] packedBoxes = rectanglesAndHeatMaps.getLeft().stream()
                                                                         .flatMapToInt(rect -> IntStream.of(rect.x, rect.y, rect.width, rect.height)).toArray();
                                String[] names = new String[rectanglesAndHeatMaps.getLeft().size()];
                                for (int i = 0; i < names.length; i++)
                                {
                                   names[i] = "Valve " + i;
                                }
                                BoundingBoxesPacket boundingBoxesPacket = MessageTools.createBoundingBoxesPacket(packedBoxes, names);

                                ObjectDetectorResultPacket resultPacket = MessageTools.createObjectDetectorResultPacket(heatMapPacket, boundingBoxesPacket);
                                resultPacket.setDestination(PacketDestination.BEHAVIOR_MODULE.ordinal());

                                detectionStatusClient.send(resultPacket);
                             });
   }

   public static void main(String[] args) throws Exception
   {
      new ValveDetectorProcess();
   }
}

<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/YoVariableLogCropper.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger;

import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.channels.FileChannel;

import us.ihmc.robotDataLogger.LogProperties;
import us.ihmc.robotDataLogger.logger.YoVariableLogReader;
import us.ihmc.robotDataVisualizer.logger.util.CustomProgressMonitor;
import us.ihmc.robotDataVisualizer.logger.util.ProgressMonitorInterface;

public class YoVariableLogCropper extends YoVariableLogReader
{
   private final MultiVideoDataPlayer player;


   

   public YoVariableLogCropper(MultiVideoDataPlayer player, File logDirectory, LogProperties logProperties)
   {
      super(logDirectory, logProperties);
      this.player = player;
      
      
   }

   public synchronized void crop(File destination, long inStamp, long outStamp)
   {
      ProgressMonitorInterface monitor = new CustomProgressMonitor("Cropping data file", "Initializing cropper", 0, 100);

      if (!initialize())
      {
         return;
      }

      try
      {
         monitor.setNote("Creating directories");
         monitor.setProgress(3);
         if (destination.exists())
         {
            if (!destination.isDirectory())
            {
               monitor.setProgress(0);
               monitor.setError("Destination " + destination.getAbsolutePath() + " already exists.");
               return;
            }
            else if (destination.list().length > 0)
            {
               monitor.setProgress(0);
               monitor.setError("Destination " + destination.getAbsolutePath() + " is not empty.");
               return;
            }
         }
         else if (!destination.mkdir())
         {
            monitor.setProgress(0);
            monitor.setError("Cannot make directory " + destination.getAbsolutePath());
            return;
         }

         monitor.setNote("Copying description files");
         monitor.setProgress(4);
         copyMetaData(destination);

         monitor.setNote("Seeking variable data");
         monitor.setProgress(10);

         File outputFile = new File(destination, logProperties.getVariables().getDataAsString());
         FileOutputStream fileOutputStream = new FileOutputStream(outputFile);
         FileChannel outputChannel = fileOutputStream.getChannel();

         File indexFile = new File(destination, logProperties.getVariables().getIndexAsString());
         FileOutputStream indexStream = new FileOutputStream(indexFile);
         FileChannel indexChannel = indexStream.getChannel();

         int startPosition = getPosition(inStamp);
         int endPosition = getPosition(outStamp);

         monitor.setNote("Writing variable data");

         ByteBuffer indexBuffer = ByteBuffer.allocateDirect(16);
         for (int i = startPosition; i <= endPosition; i++)
         {
            
            ByteBuffer compressedData = readCompressedData(i);

            indexBuffer.clear();
            indexBuffer.putLong(getTimestamp(i));
            indexBuffer.putLong(outputChannel.position());
            indexBuffer.flip();
            indexChannel.write(indexBuffer);

            outputChannel.write(compressedData);
         }

         outputChannel.close();
         fileOutputStream.close();

         indexChannel.close();
         indexStream.close();

         monitor.setNote("Cropping video files");

         if (player != null)
            player.crop(destination, inStamp, outStamp, monitor);
         monitor.close();
      }
      catch (IOException e)
      {
         throw new RuntimeException(e);
      }
   }


}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/configurations/HighLevelControllerParameters.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.configurations;

import java.util.List;

import us.ihmc.commonWalkingControlModules.controllerCore.parameters.JointAccelerationIntegrationParametersReadOnly;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WholeBodySetpointParameters;
import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.JointAccelerationIntegrationCalculator;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.sensorProcessing.outputData.JointDesiredBehaviorReadOnly;
import us.ihmc.sensorProcessing.outputData.JointDesiredControlMode;

public interface HighLevelControllerParameters
{
   WholeBodySetpointParameters getStandPrepParameters();

   HighLevelControllerName getDefaultInitialControllerState();
   HighLevelControllerName getFallbackControllerState();

   boolean automaticallyTransitionToWalkingWhenReady();

   double getTimeToMoveInStandPrep();
   double getMinimumTimeInStandReady();
   double getTimeInStandTransition();
   double getCalibrationDuration();

   /**
    * Returns a list of joint behaviors for groups of joints. In each {@link GroupParameter} a set
    * of joints can be specified that will share a common tunable parameter. In this way joints can
    * be grouped together and tuned using a single parameter (e.g. left elbow and right elbow).
    * <p>
    * The joint behavior defines parameters for the control law used by a joint level controller to
    * track the desired values of the whole body controller. It contains information such as stiffness
    * and {@link JointDesiredControlMode}. The implementation of this is usually robot specific.
    * </p>
    * <p>
    * As long as a joint is not part of a loaded joint chain these behavior settings will be
    * used for that joint. E.g. all joints of a leg in support will be considered loaded. If a joint is loaded
    * the parameters defined here can be overwritten (this is optional) by defining integration parameters
    * in {@link #getDesiredJointBehaviorsLoaded()}.
    * </p>
    * @return list containing joint behavior parameters and the corresponding joint groups
    */
   public default List<GroupParameter<JointDesiredBehaviorReadOnly>> getDesiredJointBehaviors(HighLevelControllerName state)
   {
      return null;
   }

   /**
    * This method is similar to {@link #getDesiredJointBehaviors()}. The controller will
    * apply the joint settings defined here only to joints that are part of a loaded chain.
    * This can be used if, for example, a different set of parameters should be used for
    * joints that are part of a leg that is swinging versus a leg that supporting weight.
    *
    * @return list containing joint behavior parameters to be used if a joint is loaded
    */
   public default List<GroupParameter<JointDesiredBehaviorReadOnly>> getDesiredJointBehaviorsUnderLoad(HighLevelControllerName state)
   {
      return null;
   }

   /**
    * Returns a list of acceleration integration parameters for groups of joints. In each
    * {@link GroupParameter} a set of joints can be specified that will share a common tunable
    * parameter. In this way joints can be grouped together and tuned using a single parameter (e.g.
    * left elbow and right elbow).
    * <p>
    * If a joint is not contained in this list the controller will not create tunable parameters and use
    * default acceleration integration settings defined in {@link JointAccelerationIntegrationCalculator}.
    * </p>
    * <p>
    * As long as a joint is not part of a loaded joint chain these acceleration integration settings will be
    * used for that joint. E.g. all joints of a leg in support will be considered loaded. If a joint is loaded
    * the parameters defined here can be overwritten (this is optional) by defining integration parameters
    * in {@link #getJointAccelerationIntegrationParametersUnderLoad()}.
    * </p>
    * @return list containing acceleration integration parameters and the corresponding joint groups
    */
   public default List<GroupParameter<JointAccelerationIntegrationParametersReadOnly>> getJointAccelerationIntegrationParameters(HighLevelControllerName state)
   {
      return null;
   }

   /**
    * This method is similar to {@link #getJointAccelerationIntegrationParameters()}. The controller will
    * apply the acceleration integration parameters defined here only to joints that are part of a loaded chain.
    * This can be used if, for example, a different set of acceleration integration parameters should be used for
    * joints that are part of a leg that is swinging versus a leg that supporting weight.
    *
    * @return list containing acceleration integration parameters to be used if a joint is loaded
    */
   public default List<GroupParameter<JointAccelerationIntegrationParametersReadOnly>> getJointAccelerationIntegrationParametersUnderLoad(HighLevelControllerName state)
   {
      return null;
   }

   /**
    * Provides the option to deactivate the integration of the desired accelerations inside the whole body
    * controller. This can be used if the acceleration should be integrated in the robot specific joint
    * controllers (e.g. because they have a higher loop rate).
    *
    * @return whether the acceleration integration should be deactivated in the whole body controller
    */
   public default boolean deactivateAccelerationIntegrationInTheWBC()
   {
      return false;
   }
}

<|start_filename|>ihmc-ros-tools/generated-src/ihmc_atlas_ros/AtlasElectricMotorEnablePacketRosMessage.java<|end_filename|>
package ihmc_atlas_ros;

public interface AtlasElectricMotorEnablePacketRosMessage extends org.ros.internal.message.Message {
  static final java.lang.String _TYPE = "ihmc_atlas_ros/AtlasElectricMotorEnablePacketRosMessage";
  static final java.lang.String _DEFINITION = "## AtlasElectricMotorEnablePacketRosMessage\n# Specifies a specific electric motor in the Atlas forearm to power on or off.\n\n# The Enum value of the motor to enable\nint8 atlas_electric_motor_packet_enum_enable\n\n# Boolean for enable state; true for enable, false for disable.\nbool enable\n\n# A unique id for the current message. This can be a timestamp or sequence number. Only the unique id\n# in the top level message is used, the unique id in nested messages is ignored. Use\n# /output/last_received_message for feedback about when the last message was received. A message with\n# a unique id equals to 0 will be interpreted as invalid and will not be processed by the controller.\nint64 unique_id\n\n\n";
  byte getAtlasElectricMotorPacketEnumEnable();
  void setAtlasElectricMotorPacketEnumEnable(byte value);
  boolean getEnable();
  void setEnable(boolean value);
  long getUniqueId();
  void setUniqueId(long value);
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrieRosControl/ValkyrieCalibrationControllerStateFactory.java<|end_filename|>
package us.ihmc.valkyrieRosControl;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories.HighLevelControllerStateFactory;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.valkyrie.ValkyrieCalibrationParameters;
import us.ihmc.wholeBodyController.diagnostics.TorqueOffsetPrinter;

public class ValkyrieCalibrationControllerStateFactory implements HighLevelControllerStateFactory
{
   private final TorqueOffsetPrinter torqueOffsetPrinter;
   private ValkyrieCalibrationControllerState calibrationControllerState;
   private ValkyrieCalibrationParameters calibrationParameters;

   public ValkyrieCalibrationControllerStateFactory(TorqueOffsetPrinter torqueOffsetPrinter, ValkyrieCalibrationParameters calibrationParameters)
   {
      this.torqueOffsetPrinter = torqueOffsetPrinter;
      this.calibrationParameters = calibrationParameters;
   }

   @Override
   public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper)
   {
      if (calibrationControllerState != null)
         return calibrationControllerState;

      calibrationControllerState = new ValkyrieCalibrationControllerState(controllerFactoryHelper.getHighLevelHumanoidControllerToolbox(),
                                                                          controllerFactoryHelper.getHighLevelControllerParameters(),
                                                                          controllerFactoryHelper.getLowLevelControllerOutput(), calibrationParameters,
                                                                          torqueOffsetPrinter);
      return calibrationControllerState;
   }

   public ValkyrieCalibrationControllerState getCalibrationControllerState()
   {
      return calibrationControllerState;
   }

   @Override
   public HighLevelControllerName getStateEnum()
   {
      return HighLevelControllerName.CALIBRATION;
   }

   @Override
   public boolean isTransitionToControllerRequested()
   {
      return false;
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/GenericChannelMapper.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public class GenericChannelMapper implements MidiChannelMapper
{

   @Override
   public int getKnobChannel(int knob)
   {
      return knob;
   }

   @Override
   public int getSliderChannel(int slider)
   {
      return slider;
   }

   @Override
   public int getButtonChannel(int button)
   {
      return button;
   }

   @Override
   public int getKnobButtonChannel(int knob)
   {
      return knob;
   }

}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/environmentRobots/FloatingObjectBoxRobot.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments.environmentRobots;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Link;
import us.ihmc.simulationconstructionset.Robot;

public class FloatingObjectBoxRobot extends Robot
{
   private final FloatingJoint qrCodeJoint;

   public FloatingObjectBoxRobot(String imageResourcePath)
   {
      super(FloatingObjectBoxRobot.class.getSimpleName());

      qrCodeJoint = new FloatingJoint("object", "object", new Vector3D(), this);
      Link qrCodeLink = new Link("object");
      qrCodeLink.setMassAndRadiiOfGyration(1.0, 0.1, 0.1, 0.1);
      Graphics3DObject qrCodeLinkGraphics = new Graphics3DObject();
      //      qrCodeLinkGraphics.addCoordinateSystem(2.0);
      double cubeLength = 1.0;
      qrCodeLinkGraphics.translate(0.0, 0.0, -0.99 * cubeLength);
      AppearanceDefinition cubeAppearance = YoAppearance.Texture(imageResourcePath);
      qrCodeLinkGraphics.addCube(cubeLength * 0.98, cubeLength * 1.01, cubeLength * 0.98, YoAppearance.Yellow());

      boolean[] textureFaces = new boolean[] { true, true, false, false, false, false };
      qrCodeLinkGraphics.translate(0.0, 0.0, -0.01 * cubeLength);
      qrCodeLinkGraphics.addCube(cubeLength, cubeLength, cubeLength, cubeAppearance, textureFaces);

      qrCodeLink.setLinkGraphics(qrCodeLinkGraphics);
      qrCodeJoint.setLink(qrCodeLink);
      addRootJoint(qrCodeJoint);
      setGravity(0.0);
   }

   public void setPosition(Tuple3DBasics position)
   {
      qrCodeJoint.setPosition(position);
   }

   public void setPosition(double x, double y, double z)
   {
      qrCodeJoint.setPosition(x, y, z);
   }

   public void setYawPitchRoll(double yaw, double pitch, double roll)
   {
      qrCodeJoint.setYawPitchRoll(yaw, pitch, roll);
   }

   public void setAngularVelocity(Vector3D angularVelocityInBody)
   {
      qrCodeJoint.setAngularVelocityInBody(angularVelocityInBody);
   }

   public void setLinearVelocity(Vector3D linearVelocityInWorld)
   {
      qrCodeJoint.setVelocity(linearVelocityInWorld);
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/CommandConversionInterface.java<|end_filename|>
package us.ihmc.communication.controllerAPI;

import us.ihmc.communication.controllerAPI.command.Command;
import us.ihmc.euclid.interfaces.Settable;

/**
 * Helps converts packets to commands
 */
public interface CommandConversionInterface
{
   /**
    * Can the command converter handle a packet, generally just check the type of the message (incoming packet) and the command
    * @param command the command type that is going to be updated
    * @param message the message that was received
    * @return whether it can convert it
    */
   <C extends Command<?, M>, M extends Settable<M>> boolean isConvertible(C command, M message);
   
   /**
    * actually update the command based on the message
    * @param command the command to update
    * @param message the message with the source data
    */
   <C extends Command<?, M>, M extends Settable<M>> void process(C command, M message);
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/AngleTools.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.util.Random;

import us.ihmc.commons.MathTools;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.tuple2D.Point2D32;
import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.euclid.tuple4D.Quaternion32;

public class AngleTools
{

   public static final double PI = Math.PI;
   public static final double TwoPI = 2.0 * PI;
   public static final double EPSILON = 1e-10;

   private AngleTools()
   {

   }

   public static double trimAngleMinusPiToPi(double angle)
   {
      return AngleTools.shiftAngleToStartOfRange(angle, -PI);
   }

   public static float getAngle(Quaternion32 q)
   {
      return 2.0f * (float) Math.acos(q.getS());
   }

   public static double angleMinusPiToPi(Vector2DReadOnly startVector, Vector2DReadOnly endVector)
   {
      double absoluteAngle = Math.acos(startVector.dot(endVector) / startVector.length() / endVector.length());

      Vector3D start3d = new Vector3D(startVector.getX(), startVector.getY(), 0.0);
      Vector3D end3d = new Vector3D(endVector.getX(), endVector.getY(), 0.0);

      Vector3D crossProduct = new Vector3D();
      crossProduct.cross(start3d, end3d);

      if (crossProduct.getZ() >= 0.0)
      {
         return absoluteAngle;
      }
      else
      {
         return -absoluteAngle;
      }
   }

   public static double computeAngleAverage(double angleA, double angleB)
   {
      // average = A + (B-A)/2 = (A+B)/2
      return interpolateAngle(angleA, angleB, 0.5);
   }

   public static double interpolateAngle(double angleA, double angleB, double alpha)
   {
      // A + alpha * (B-A)/2
      double average = angleA + alpha * AngleTools.computeAngleDifferenceMinusPiToPi(angleB, angleA);
      return trimAngleMinusPiToPi(average);
   }

   /**
    * Formula found on <a href="https://en.wikipedia.org/wiki/Mean_of_circular_quantities"> Wikipedia</a>.
    */
   public static double computeAngleAverage(double[] angles)
   {
      double average = 0.0;

      double averageOfSin = 0.0;
      double averageOfCos = 0.0;
      double weight = 1.0 / angles.length;

      for (int i = 0; i < angles.length; i++)
      {
         averageOfSin += weight * Math.sin(angles[i]);
         averageOfCos += weight * Math.cos(angles[i]);
      }

      average = Math.atan2(averageOfSin, averageOfCos);

      return average;
   }

   /**
    * Finds the closest 90 degree yaw and returns number of 90 degrees (0 = 0; 1 = 90; 2 = 180; 3 = 270).
    *
    * @param yawInRadians double
    * @return int between 0 and 3 for the number of 90 degree yawed.
    */
   public static int findClosestNinetyDegreeYaw(double yawInRadians)
   {
      double minDifference = Double.POSITIVE_INFINITY;
      int ret = -1;

      double difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 0.0);

      minDifference = Math.abs(difference);
      ret = 0;

      difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI / 2.0);

      if (Math.abs(difference) < minDifference)
      {
         minDifference = Math.abs(difference);
         ret = 1;
      }

      difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI);

      if (Math.abs(difference) < minDifference)
      {
         minDifference = Math.abs(difference);
         ret = 2;
      }

      difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 3.0 * Math.PI / 2.0);

      if (Math.abs(difference) < minDifference)
      {
         minDifference = Math.abs(difference);
         ret = 3;
      }

      return ret;
   }

   /**
    * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-pi, pi)
    *
    * @param angleA double
    * @param angleB double
    * @return double
    */
   public static double computeAngleDifferenceMinusPiToPi(double angleA, double angleB)
   {
      double difference = angleA - angleB;
      difference = difference % TwoPI;
      difference = AngleTools.shiftAngleToStartOfRange(difference, -PI);

      return difference;
   }

   /**
    * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-2.0*pi, 0.0)
    *
    * @param angleA double
    * @param angleB double
    * @return double
    */
   public static double computeAngleDifferenceMinusTwoPiToZero(double angleA, double angleB)
   {
      double difference = angleA - angleB;
      difference = difference % TwoPI;
      difference = AngleTools.shiftAngleToStartOfRange(difference, -TwoPI);

      return difference;
   }

   /**
    * This will shift an angle to be in the range [<i>startOfAngleRange</i>,
    *  (<i>startOfAngleRange + 2*pi</i>)
    *
    * @param angleToShift the angle to shift
    * @param startOfAngleRange start of the range.
    * @return the shifted angle
    */
   public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange)
   {
      return shiftAngleToStartOfRange(angleToShift, startOfAngleRange, TwoPI);
   }

   /**
    * This will shift an angle to be in the range [<i>startOfAngleRange</i>,
    *  (<i>startOfAngleRange + endOfAngleRange</i>)
    *
    * @param angleToShift the angle to shift
    * @param startOfAngleRange start of the range.
    * @param endOfAngleRange end of the range.
    * @return the shifted angle
    */
   public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange, double endOfAngleRange)
   {
      double ret = angleToShift;
      startOfAngleRange = startOfAngleRange - EPSILON;

      if (angleToShift < startOfAngleRange)
      {
         ret = angleToShift + Math.ceil((startOfAngleRange - angleToShift) / endOfAngleRange) * endOfAngleRange;
      }

      if (angleToShift >= (startOfAngleRange + endOfAngleRange))
      {
         ret = angleToShift - Math.floor((angleToShift - startOfAngleRange) / endOfAngleRange) * endOfAngleRange;
      }

      return ret;
   }

   /**
    * Returns an evenly distributed random number between -2PI and 2PI
    *
    * @param random Random number generator
    * @return number between -2PI and 2PI
    */
   public static double generateRandomAngle(Random random)
   {
      return -2.0 * Math.PI + 4 * Math.PI * random.nextDouble();
   }

   /**
    * Returns array of angles increasing from -2PI to 2PI
    * 
    * @param numberOfAngles
    * @param stayThisFarAwayFromPlusMinus2PI
    * @param includeZero
    * @param includePlusMinusPI
    * @return
    */
   public static double[] generateArrayOfTestAngles(int numberOfAngles, double stayThisFarAwayFromPlusMinus2PI, boolean includeZero, boolean includePlusMinusPI)
   {
      int arraySize = numberOfAngles;
      double epsilon = stayThisFarAwayFromPlusMinus2PI;

      if (includeZero && !includePlusMinusPI)
      {
         arraySize += 1;
      }
      else if (includePlusMinusPI && !includeZero)
      {
         arraySize += 2;
      }
      else if (includeZero && includePlusMinusPI)
      {
         arraySize += 3;
      }

      double thetaMin = -2.0 * Math.PI + stayThisFarAwayFromPlusMinus2PI;
      double thetaMax = 2.0 * Math.PI - stayThisFarAwayFromPlusMinus2PI;
      double deltaTheta = Math.abs(thetaMax - thetaMin) / (numberOfAngles - 1);

      double[] ret = new double[arraySize];

      for (int i = 0; i < numberOfAngles; i++)
      {
         ret[i] = thetaMin + deltaTheta * i;

         boolean epsilonEqualToZero = MathTools.epsilonEquals(Math.abs(ret[i]), 0.0, epsilon);
         boolean epsilonEqualToPlusMinusPI = MathTools.epsilonEquals(Math.abs(ret[i]), Math.PI, epsilon);

         if (epsilonEqualToZero && !includeZero || epsilonEqualToPlusMinusPI && !includePlusMinusPI)
            ret[i] += Math.signum(ret[i]) * Math.max(1.0e-4, Math.abs(epsilon));
      }

      if (includeZero && !includePlusMinusPI)
      {
         ret[numberOfAngles] = 0.0;
      }
      else if (includePlusMinusPI && !includeZero)
      {
         ret[numberOfAngles] = -Math.PI;
         ret[numberOfAngles + 1] = Math.PI;
      }
      else if (includeZero && includePlusMinusPI)
      {
         ret[numberOfAngles] = 0.0;
         ret[numberOfAngles + 1] = -Math.PI;
         ret[numberOfAngles + 2] = Math.PI;
      }

      return ret;
   }

   /**
    * Returns an angle between two points + heading Offset from -PI to PI. 
    * If the x or y components are both under the noTranslationTolerance, 
    * then the initial orientation as given in startPose will be returned. 
    *
    * @param startPose initial position and orientation
    * @param endPoint end position
    * @param headingOffset offset from path angle
    * @param noTranslationTolerance tolerance for determining if path angle should be determined
    * @return number between -PI and PI
    */
   public static double calculateHeading(FramePose2D startPose, FramePoint2D endPoint, double headingOffset, double noTranslationTolerance)
   {
      double deltaX = endPoint.getX() - startPose.getX();
      double deltaY = endPoint.getY() - startPose.getY();
      double heading;
      if (Math.abs(deltaX) < noTranslationTolerance && Math.abs(deltaY) < noTranslationTolerance)
      {
         heading = startPose.getYaw();
      }
      else
      {
         double pathHeading = Math.atan2(deltaY, deltaX);
         heading = AngleTools.trimAngleMinusPiToPi(pathHeading + headingOffset);
      }
      return heading;
   }

   /**
    * Returns an angle between two points + heading Offset from -PI to PI. 
    *
    * @param startPose initial position
    * @param endPoint end position
    * @return number between -PI and PI
    */
   public static double calculateHeading(Point2D32 startPose, Point2D32 endPoint)
   {
      double deltaX = endPoint.getX() - startPose.getX();
      double deltaY = endPoint.getY() - startPose.getY();
      double heading;

      double pathHeading = Math.atan2(deltaY, deltaX);
      heading = AngleTools.trimAngleMinusPiToPi(pathHeading);

      return heading;
   }

   /**
    * Pass in a vector. Get its angle in polar coordinates.
    * 
    * @param vx
    * @param vy
    * @return angle of vector from 0 to 2PI
    */
   public static double angleFromZeroToTwoPi(double vx, double vy)
   {
      double angleFromNegtivePiToPi = Math.atan2(vy, vx);

      if (angleFromNegtivePiToPi < 0.0)
      {
         return 2.0 * Math.PI + angleFromNegtivePiToPi;
      }
      else
      {
         return angleFromNegtivePiToPi;
      }
   }

   public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor)
   {
      double centeredAngleValue = trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor);
      long longValue = (long) (centeredAngleValue / precisionFactor);
      double roundedValue = ((double) longValue) * precisionFactor;
      return trimAngleMinusPiToPi(roundedValue);
   }

   public static void roundToGivenPrecisionForAngles(Tuple3DBasics tuple3d, double precision)
   {
      tuple3d.setX(roundToGivenPrecisionForAngle(tuple3d.getX(), precision));
      tuple3d.setY(roundToGivenPrecisionForAngle(tuple3d.getY(), precision));
      tuple3d.setZ(roundToGivenPrecisionForAngle(tuple3d.getZ(), precision));
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/CTNodeVisualizer.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import java.awt.Color;
import java.util.ArrayList;

import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.graphicsDescription.plotting.artifact.CircleArtifact;
import us.ihmc.graphicsDescription.plotting.artifact.LineArtifact;
import us.ihmc.plotting.Plotter;

public class CTNodeVisualizer
{
   private Plotter plotter;

   private int configurationIndex;

   private int updateCnt;

   private boolean enabled;
   private String ctaskName;

   public CTNodeVisualizer(String ctaskName, int configurationIndex, boolean enabled)
   {
      this.ctaskName = ctaskName;
      this.plotter = new Plotter();

      this.configurationIndex = configurationIndex;

      /*
       * zoom- always normalized.
       */
      plotter.setPreferredSize(700, 700);

      plotter.setViewRange(1.5);
      plotter.setXYZoomEnabled(true);
      plotter.setShowLabels(true);
      plotter.setFocusPointX(0.5);
      plotter.setFocusPointY(0.5);

      this.enabled = enabled;
   }

   public void initialize()
   {
      updateCnt = 0;
      if (enabled)
         plotter.showInNewWindow(ctaskName, false);
   }

   public void updateVisualizer(CTTaskNode newNode)
   {
      String prefix = "wholenodes_" + configurationIndex + "" + updateCnt;
      if (newNode.getValidity())
      {
         if (newNode.getParentNode() != null)
         {
            CTTaskNode parentNode = newNode.getParentNode();
            LineArtifact lineArtifact = new LineArtifact(prefix + "_line",
                                                         new Point2D(parentNode.getNormalizedNodeData(0), parentNode.getNormalizedNodeData(configurationIndex)),
                                                         new Point2D(newNode.getNormalizedNodeData(0), newNode.getNormalizedNodeData(configurationIndex)));

            lineArtifact.setColor(Color.blue);
            plotter.addArtifact(lineArtifact);
         }

         CircleArtifact nodeArtifact = new CircleArtifact(prefix + "_valid", newNode.getNormalizedNodeData(0),
                                                          newNode.getNormalizedNodeData(configurationIndex), 0.0075, true);
         nodeArtifact.setColor(Color.blue);

         plotter.addArtifact(nodeArtifact);

      }
      else
      {
         CircleArtifact nodeArtifact = new CircleArtifact(prefix + "_invalid", newNode.getNormalizedNodeData(0),
                                                          newNode.getNormalizedNodeData(configurationIndex), 0.0075, true);
         nodeArtifact.setColor(Color.red);
         plotter.addArtifact(nodeArtifact);
      }
      plotter.update();
      updateCnt++;
   }

   private void updateVisualizer(CTTaskNode newNode, boolean isShortcut)
   {
      String prefix;
      Color pathColor;
      if(isShortcut)
      {
         prefix = "shortcut_" + configurationIndex + "" + updateCnt;
         pathColor = Color.green;
      }         
      else
      {
         prefix = "path_" + configurationIndex + "" + updateCnt;
         pathColor = Color.black;
      }
               
      if (newNode.getParentNode() != null)
      {
         CTTaskNode parentNode = newNode.getParentNode();
         LineArtifact lineArtifact = new LineArtifact(prefix + "_line",
                                                      new Point2D(parentNode.getNormalizedNodeData(0), parentNode.getNormalizedNodeData(configurationIndex)),
                                                      new Point2D(newNode.getNormalizedNodeData(0), newNode.getNormalizedNodeData(configurationIndex)));

         lineArtifact.setColor(pathColor);
         plotter.addArtifact(lineArtifact);
      }

      CircleArtifact nodeArtifact = new CircleArtifact(prefix + "_valid", newNode.getNormalizedNodeData(0), newNode.getNormalizedNodeData(configurationIndex),
                                                       0.012, true);
      nodeArtifact.setColor(pathColor);

      plotter.addArtifact(nodeArtifact);

      plotter.update();
      updateCnt++;
   }

   public void updateVisualizer(ArrayList<CTTaskNode> path, boolean isShortcut)
   {
      int size = path.size();
      for(int i=0;i<size;i++)
         updateVisualizer(path.get(i), isShortcut);
   }
}

<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/interfaces/RemoteSimulationDescription.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces;

public interface RemoteSimulationDescription extends java.io.Serializable
{
   public abstract void createSimulation(String[] structuralParameterNames, double[] structuralParameterValues);

   public abstract void destroySimulation();

   public abstract void setSimulationState(Object state);
   
   public abstract void startSimulation();

   public abstract boolean isSimulationDone();

   public abstract Object getSimulationState();

   public abstract Object getSimulationData();
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/desiredFootStep/footstepGenerator/DesiredTurningVelocityProvider.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator;

/**
 * This interface is part of the API of {@link ContinuousStepGenerator}.
 * <p>
 * It is used to provide a protocol for obtaining the current desired walking turning velocity.
 * </p>
 * 
 * @author <NAME>
 */
public interface DesiredTurningVelocityProvider
{
   /**
    * Gets the current desired turning velocity.
    * <p>
    * A positive value corresponds to turning to the left.
    * </p>
    * 
    * @return the desired turning velocity.
    */
   double getTurningVelocity();
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/remote/UnsupportedFormatException.java<|end_filename|>
package us.ihmc.communication.remote;

public class UnsupportedFormatException extends Exception
{
   private static final long serialVersionUID = 8845641123167087257L;

   public UnsupportedFormatException()
   {
   }

   public UnsupportedFormatException(String msg)
   {
      super(msg);
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosLidarPublisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import org.ros.message.Time;

import controller_msgs.msg.dds.SimulatedLidarScanPacket;
import gnu.trove.list.array.TFloatArrayList;
import std_msgs.Header;
import us.ihmc.communication.packets.MessageTools;
import us.ihmc.robotics.lidar.LidarScanParameters;

public class RosLidarPublisher extends RosTopicPublisher<sensor_msgs.LaserScan>
{
   private int seq = 0;

   public RosLidarPublisher(boolean latched)
   {
      super(sensor_msgs.LaserScan._TYPE, latched);
   }

   @Override
   public void connected()
   {

   }

   private float[] fakeIntensities = null;

   public void publish(SimulatedLidarScanPacket lidarScan, String frameId, Time timestamp)
   {
      sensor_msgs.LaserScan message = getMessage();

      Header header = message.getHeader();
      header.setStamp(timestamp);
      header.setFrameId(frameId);
      header.setSeq(seq++);
      message.setHeader(header);

      LidarScanParameters parameters = MessageTools.toLidarScanParameters(lidarScan.getLidarScanParameters());
      message.setAngleMin(parameters.getSweepYawMin());
      message.setAngleMax(parameters.getSweepYawMax());
      message.setAngleIncrement(parameters.getAngleIncrement());
      message.setTimeIncrement(parameters.getTimeIncrement());
      message.setScanTime(parameters.getScanTime());
      message.setRangeMin(parameters.getMinRange());
      message.setRangeMax(parameters.getMaxRange());

      if (fakeIntensities == null || fakeIntensities.length != parameters.getPointsPerSweep())
      {
         fakeIntensities = new float[parameters.getPointsPerSweep()];
      }
      TFloatArrayList ranges = lidarScan.getRanges();
      for (int i = 0; i < ranges.size(); i++)
         fakeIntensities[i] = 1 / ((ranges.get(i) * ranges.get(i)) + 1e-10f);

      message.setIntensities(fakeIntensities);

      message.setRanges(lidarScan.getRanges().toArray());

      publish(message);
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/foot/toeOffCalculator/ToeOffEnum.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.controlModules.foot.toeOffCalculator;

public enum ToeOffEnum
{
   SIMPLE, CENTROID_PROJECTION, ICP_PLAN;
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LegCompliancePacket.java<|end_filename|>
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;

/**
       * Atlas specific message.
       * This packet controls the stiffness of LegJoints. i.e., the maximum force a joint puts out when it
       * is (pushed) away from a desired position. This is useful to prevent robot from falling when leg
       * hit things unexpectedly. However, low stiffness (high compliance) can lead to poor joint tracking
       * in the presence of natural joint stiction. Finding a good balance is application specific. In our
       * hybrid velocity+force controller, most force comes from velocity control, therefore, only
       * parameter related to velocity control is exposed.
       */
public class LegCompliancePacket extends Packet<LegCompliancePacket> implements Settable<LegCompliancePacket>, EpsilonComparable<LegCompliancePacket>
{
   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_;
   /**
            * maximum allowed force (ratio) from velocity control in the range of [0.0, 1.0]. 1.0 is the
            * maximum stiffness (default) value tuned for fast walking, 0.0 refers to zero velocity control
            * making the joint very compliant (only force control) but often bad tracking. On Atlas, 0.1-0.3
            * gives decent tracking for slow motion and yet still compliant. The numbers in the array
            * correspond to joints HPZ, HPX, HPY, KNY, AKY, AKX, respectively.
            */
   public us.ihmc.idl.IDLSequence.Float  max_velocity_deltas_;
   public byte robot_side_ = (byte) 255;

   public LegCompliancePacket()
   {
      max_velocity_deltas_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5");

   }

   public LegCompliancePacket(LegCompliancePacket other)
   {
      this();
      set(other);
   }

   public void set(LegCompliancePacket other)
   {
      sequence_id_ = other.sequence_id_;

      max_velocity_deltas_.set(other.max_velocity_deltas_);
      robot_side_ = other.robot_side_;

   }

   /**
            * 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_;
   }


   /**
            * maximum allowed force (ratio) from velocity control in the range of [0.0, 1.0]. 1.0 is the
            * maximum stiffness (default) value tuned for fast walking, 0.0 refers to zero velocity control
            * making the joint very compliant (only force control) but often bad tracking. On Atlas, 0.1-0.3
            * gives decent tracking for slow motion and yet still compliant. The numbers in the array
            * correspond to joints HPZ, HPX, HPY, KNY, AKY, AKX, respectively.
            */
   public us.ihmc.idl.IDLSequence.Float  getMaxVelocityDeltas()
   {
      return max_velocity_deltas_;
   }

   public void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   public byte getRobotSide()
   {
      return robot_side_;
   }


   public static Supplier<LegCompliancePacketPubSubType> getPubSubType()
   {
      return LegCompliancePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return LegCompliancePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(LegCompliancePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.max_velocity_deltas_, other.max_velocity_deltas_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof LegCompliancePacket)) return false;

      LegCompliancePacket otherMyClass = (LegCompliancePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.max_velocity_deltas_.equals(otherMyClass.max_velocity_deltas_)) return false;
      if(this.robot_side_ != otherMyClass.robot_side_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("LegCompliancePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("max_velocity_deltas=");
      builder.append(this.max_velocity_deltas_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/physics/AtlasPhysicsEngineConfiguration.java<|end_filename|>
package us.ihmc.atlas.physics;

import us.ihmc.atlas.AtlasJointMap;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotics.geometry.TransformTools;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.FloatingRootJointRobot;
import us.ihmc.simulationconstructionset.Joint;
import us.ihmc.simulationconstructionset.Link;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.physics.CollisionHandler;
import us.ihmc.simulationconstructionset.physics.CollisionShapeDescription;
import us.ihmc.simulationconstructionset.physics.CollisionShapeFactory;
import us.ihmc.simulationconstructionset.physics.ScsCollisionConfigure;
import us.ihmc.simulationconstructionset.physics.ScsCollisionDetector;

/**
 * @author <NAME>
 */
// TODO create ground plane
// TODO get link for left and right foot
// TODO create box for each foot
public class AtlasPhysicsEngineConfiguration implements ScsCollisionConfigure
{
   private static final int GROUP_GROUND = 0xFFFFFFFF;
   private static final int GROUP_FEET = 0xFFFFFFFF;

   private AtlasJointMap jointMap;
   private FloatingRootJointRobot sdfRobot;

   public AtlasPhysicsEngineConfiguration(AtlasJointMap jointMap, FloatingRootJointRobot sdfRobot)
   {
      this.jointMap = jointMap;
      this.sdfRobot = sdfRobot;
   }

   @Override
   public void setup(Robot robot, ScsCollisionDetector collisionDetector, CollisionHandler collisionHandler)
   {
      String leftFootJointName = jointMap.getJointBeforeFootName(RobotSide.LEFT);
      String rightFootJointName = jointMap.getJointBeforeFootName(RobotSide.RIGHT);

      Joint leftFootJoint = sdfRobot.getJoint(leftFootJointName);
      Joint rightFootJoint = sdfRobot.getJoint(rightFootJointName);

      Link leftLink = leftFootJoint.getLink();
      Link rightLink = rightFootJoint.getLink();

      CollisionShapeFactory factoryShape = collisionDetector.getShapeFactory();
      CollisionShapeDescription collisionFoot = factoryShape.createBox(jointMap.getPhysicalProperties().getActualFootLength() / 2, jointMap.getPhysicalProperties().getActualFootWidth() / 2, 0.05);

      //      public static final double ankleHeight = 0.084;
      RigidBodyTransform ankleToSole = TransformTools.createTranslationTransform(new Vector3D(0.0, 0.0, 0.084));//jointMap.getPhysicalProperties().getAnkle_to_sole_frame_tranform();
      RigidBodyTransform soleToAnkle = new RigidBodyTransform();
      ankleToSole.setAndInvert(soleToAnkle);

      factoryShape.addShape(leftLink, soleToAnkle, collisionFoot, false, GROUP_FEET, GROUP_GROUND);
      factoryShape.addShape(rightLink, soleToAnkle, collisionFoot, false, GROUP_FEET, GROUP_GROUND);

      // HACK.  Add the ground plane
      CollisionShapeDescription collisionGround = factoryShape.createBox(20, 20, 0.1);

      FloatingJoint groundJoint = new FloatingJoint("ground", "ground", new Vector3D(), robot);
      Link linkGround = new Link("Ground Plane Hack");
      linkGround.setMass(1000);
      linkGround.setMomentOfInertia(0.1 * 100, 0.1 * 100, 0.1 * 100);

      factoryShape.addShape(linkGround, null, collisionGround, true, GROUP_GROUND, 0xFFFFFFFF);

      groundJoint.setLink(linkGround);
      groundJoint.setPositionAndVelocity(0.0, 0.0, -0.1, 0.0, 0.0, 0.0);
      groundJoint.setDynamic(false);
      robot.addRootJoint(groundJoint);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/HostsChangedListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;

public interface HostsChangedListener
{
   public void hostsChanged();
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/GainCoupling.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains;

/**
 * This enum is used in the context of 3D PID gains and can be set to determine if
 * multiple axes should be controlled using the same gain. E.g. if orientation gains
 * are created with {@link GainCoupling#XY} the YoVariables to tune the X and Y
 * direction gains are not duplicated, while the Z gains will have individual tuning
 * variables.
 */
public enum GainCoupling
{
   XY,
   YZ,
   XZ,
   XYZ,
   NONE;
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/ButtonState.java<|end_filename|>
package us.ihmc.avatar.joystickBasedJavaFXController;

public enum ButtonState
{
   PRESSED, RELEASED
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/geometry/PointMean.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.geometry;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;

public class PointMean extends Point3D
{
   private int sampleSize = 0;

   public PointMean()
   {
   }

   public void update(Tuple3DBasics tuple)
   {
      update(tuple.getX(), tuple.getY(), tuple.getZ());
   }

   public void update(Tuple3DBasics tuple, int updateSize)
   {
      update(tuple.getX(), tuple.getY(), tuple.getZ(), updateSize);
   }

   public void update(double x, double y, double z)
   {
      update(x, y, z, 1);
   }

   public void update(double x, double y, double z, int updateSize)
   {
      sampleSize += updateSize;
      double nInv = (double) updateSize / (double) sampleSize;
      this.setX(this.getX() + (x - this.getX()) * nInv);
      this.setY(this.getY() + (y - this.getY()) * nInv);
      this.setZ(this.getZ() + (z - this.getZ()) * nInv);
   }

   public void clear()
   {
      sampleSize = 0;
      set(0.0, 0.0, 0.0);
   }

   public int getNumberOfSamples()
   {
      return sampleSize;
   }
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/drillDetection/DrillDetection.java<|end_filename|>
package us.ihmc.sensorProcessing.drillDetection;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.DataInputStream;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.util.concurrent.locks.ReentrantLock;

import org.apache.commons.math3.complex.Complex;
import org.apache.commons.math3.transform.DftNormalization;
import org.apache.commons.math3.transform.FastFourierTransformer;
import org.apache.commons.math3.transform.TransformType;
import org.jtransforms.fft.FloatFFT_1D;

import us.ihmc.commons.MathTools;
import us.ihmc.mecano.spatial.Wrench;

/*
 * Simple helper class to evaluate the amount of noise on force sensor.
 * This gives a good estimation of the state of the Drill (either On of off).
 * 
 * Usage:
 * 
 *  1) Continuously update using data from the force/torque sensors (method updateForceData ).
 *  2) Measure the amount of noise when you are sure that drill is OFF (method calculateNoiseIndicator ).
 *  3) make new measurements and compare the previous noiseIndicator with the current one. 
 *     You should see a considerably larger value.  
 */
public class DrillDetection
{
   final private static int numSamples = 128;
   final private Wrench wrenchToPack = new Wrench();

   final private double[][] data = new double[numSamples][6];
   final private double[][] tempData = new double[numSamples][6];

   final private float[] orderedSamples = new float[numSamples];

   final private FloatFFT_1D fftCalculator = new FloatFFT_1D(numSamples);

   int circularBufferIndex = -1;

   ReentrantLock lock = new ReentrantLock();

   public void updateForceData( double[] forceTorqueValue )
   {  
      lock.lock();

      boolean newValue = false;
      
      if( circularBufferIndex >= 0 )
      {
         // check if the value has been refreshed ( adding old value might affect FFT behavior )      
         for (int i=0; i<6; i++)
         {
            if( MathTools.epsilonEquals( data[circularBufferIndex][i], forceTorqueValue[i], 0.0001 ) == false )
            {
               newValue = true;
               break;
            }
         }
      }
      else{
         newValue = true;
      }

      if( newValue )
      {
         // circular buffer that overwrites old values.     
         circularBufferIndex = (circularBufferIndex + 1) % numSamples;         
         System.arraycopy(forceTorqueValue, 0, data[circularBufferIndex], 0, 6);
      }

      lock.unlock();
   }

   public double[] calculateNoiseIndicator()
   {
      double[] output = new double[6];
      
      lock.lock();
      int startingIndex = (circularBufferIndex + 1 ) % numSamples;

      for (int s=0; s<numSamples; s++ )
      {
         System.arraycopy( data[s] , 0, tempData[s], 0, 6);
      }
      lock.unlock();


      for (int i=0; i<6; i++ )
      {
         for (int s=0; s< numSamples; s++)
         {
            orderedSamples[s] = (float)tempData[ (s + startingIndex ) % numSamples  ][i];
         }

         fftCalculator.realForward( orderedSamples );

         output[i] = 0;

         // note: this number has no physical meaning, it is just a rough amount of "noise".
         for (int s = 1; s < numSamples* 0.75/ 2.0; s++ )
         {
            output[i] += Math.abs( orderedSamples[s*2] ); // + Math.abs( orderedSamples[s*2+1] );
         }
      }
      return output;
   }

   
   public static void main(String[] args) 
   {
      DrillDetection drillDetection = new DrillDetection();
      
      try{

         String[] fileNames = new String[]{"drillOff", "drillOn" };
 
         
         for (String filename: fileNames)
         {
            FileInputStream fstream = new FileInputStream("/home/unknownid/" + filename + ".txt");
            DataInputStream in = new DataInputStream(fstream);
            BufferedReader br = new BufferedReader(new InputStreamReader(in));

            String strLine;
            double[] sensorData = new double[6];

            float[]  sensorX_float  = new float[numSamples]; 
            double[] sensorX_double = new double[numSamples];

            int index = 0;
 
            while ((strLine = br.readLine()) != null)   {
               // Print the content on the console

               String[] elements = strLine.split(",");

               for(int i=0; i<6; i++)
               {
                  sensorData[i] = Double.parseDouble(elements[i]);
               }

               drillDetection.updateForceData( sensorData );

               int off = 500; 
               if(index >= off && index-off < numSamples)
               {
                  sensorX_float[index-off]  = (float) sensorData[4];
                  sensorX_double[index-off] =  sensorData[4];             
               }
               index++;
            }
            in.close();
            
            double[] sensorNoise = drillDetection.calculateNoiseIndicator();
            
            for(int i=0; i<6; i++)
            {
               System.out.print(sensorNoise[i] + " /  ");
            }
            System.out.println() ;
            

            FloatFFT_1D fftCalculator = new FloatFFT_1D( numSamples );
            fftCalculator.realForward( sensorX_float );

            FastFourierTransformer fft = new FastFourierTransformer(DftNormalization.STANDARD);
            Complex[] result =  fft.transform(sensorX_double, TransformType.FORWARD);

            //-------------------------------------------
            File fout = new File("/home/unknownid/" + filename + ".fft.txt");
            FileOutputStream fos = new FileOutputStream(fout);

            BufferedWriter bw = new BufferedWriter(new OutputStreamWriter(fos));

            if( true )
            {
               for (int i = 2; i < sensorX_float.length; i+=2 ) 
               {
                  bw.write( Math.abs(sensorX_float[i]) + " " + Math.abs( sensorX_float[i+1]));
                  bw.newLine();
               }
            }
            else{
               for (int i = 1; i < result.length/2; i++ ) 
               {
                  bw.write( Math.abs(result[i].getReal()) + " " + Math.abs(result[i].getImaginary()));
                  bw.newLine();
               }
            }
            bw.close();
         }
      }
      catch (Exception e){//Catch exception if any
         System.err.println("Error: " + e.getMessage());
      }
   }
    
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/bubo/clouds/detect/alg/ShapeDescription.java<|end_filename|>
/*
 * Copyright (c) 2013-2014, <NAME>. All Rights Reserved.
 *
 * This file is part of Project BUBO.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package us.ihmc.sensorProcessing.bubo.clouds.detect.alg;

import java.util.Stack;

import org.ddogleg.fitting.modelset.DistanceFromModel;
import org.ddogleg.fitting.modelset.ModelCodec;
import org.ddogleg.fitting.modelset.ModelFitter;
import org.ddogleg.fitting.modelset.ModelManager;

import us.ihmc.sensorProcessing.bubo.clouds.detect.shape.CheckShapeParameters;

/**
 * Mathematically describes a shape, how to estimate the shape, how to compute the distance a point
 * is from the shape, and how far away a point can be from the shape to be considered part of it.
 *
 * @author <NAME>
 */
public class ShapeDescription<Model> {

	/**
	 * how close a point needs to be considered part of the model
	 */
	public double thresholdFit;
	/**
	 * Creates new instances and copies
	 */
	public ModelManager<Model> modelManager;
	/**
	 * generates an initial model given a set of points
	 */
	public ModelGeneratorCheck<Model, PointVectorNN> modelGenerator;
	/**
	 * Used to refine an initial model estimate when given an initial estimate
	 */
	public ModelFitter<Model, PointVectorNN> modelFitter;
	/**
	 * computes the distance a point is from the model
	 */
	public DistanceFromModel<Model, PointVectorNN> modelDistance;
	/**
	 * Specifies an optional check which can be used to remove physically impossible shapes.  If null then
	 * no check will be performed
	 */
	public CheckShapeParameters<Model> modelCheck;
	/**
	 * Converts the model parameter into double[]
	 */
	public ModelCodec<Model> codec;

	// storage for models so that they can be recycled
	protected Stack<Model> used = new Stack<Model>();
	protected Stack<Model> unused = new Stack<Model>();

	public void reset() {
		unused.addAll(used);
		used.clear();
	}

	/**
	 * Takes the most recently created model and removes it from the used list and puts it into the unused list
	 */
	public void recycleTail() {
		Model m = used.remove(used.size() - 1);
		unused.add(m);
	}

	public Model createModel() {
		Model m;
		if (unused.isEmpty()) {
			m = modelManager.createModelInstance();
		} else {
			m = unused.pop();
		}
		used.add(m);
		return m;
	}
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/io/DatagramOutputStream.java<|end_filename|>
package us.ihmc.tools.io;

import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.io.OutputStream;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.SocketException;
import java.net.UnknownHostException;

/**
 * DatagramOutputStream
 *
 * Each time you write, it buffers the data. Once you call flush, it sends off a DatagramPacket
 * to the ipAddress and port specified on construction.
 */
public class DatagramOutputStream extends OutputStream
{
   private final ByteArrayOutputStream byteArrayOutputStream;

   private final DatagramSocket datagramSocketToWriteTo;
   private final int port;
   private final InetAddress inetAddress;

   private final boolean VERBOSE = false;
   
   public DatagramOutputStream(int port, String ipAddress) throws SocketException, UnknownHostException
   {
      datagramSocketToWriteTo = new DatagramSocket();

      this.port = port;
      this.inetAddress = InetAddress.getByName(ipAddress);

      byteArrayOutputStream = new ByteArrayOutputStream(DatagramInputStream.BUFFER_SIZE);
   }

   @Override
   public void write(int b) throws IOException
   {
      byteArrayOutputStream.write(b);
   }

   @Override
   public void write(byte b[]) throws IOException
   {
      byteArrayOutputStream.write(b);
   }

   @Override
   public void write(byte[] b, int offset, int length)
   {
      byteArrayOutputStream.write(b, offset, length);
   }

   byte sendIndex = 0;
   @Override
   public void flush()
   {
      byteArrayOutputStream.write(sendIndex); // Add a send index to the end of the datagram before it is sent. Just a single byte that rolls over...
      sendIndex++;
      
      byte[] byteArray = byteArrayOutputStream.toByteArray();

      DatagramPacket datagramPacket = new DatagramPacket(byteArray, byteArray.length, inetAddress, port);

      if (VERBOSE)
      {
         System.out.println("Sending DatagramPacket of length " + datagramPacket.getLength() + " to inetAddress " + datagramPacket.getAddress());
      }
      try
      {
         datagramSocketToWriteTo.send(datagramPacket);
      }
      catch (IOException e)
      {
         System.err.println("IOException in DatagramOutputStream.flush()");
         // UDP is unrealiable, so just drop if an exception...
      }

      byteArrayOutputStream.reset();
   }
   
   @Override
   public void close()
   {
      if (datagramSocketToWriteTo != null)
         datagramSocketToWriteTo.close(); 
   }

}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/pointClouds/combinationQuadTreeOctTree/QuadTreeForGroundHeightMap.java<|end_filename|>
package us.ihmc.sensorProcessing.pointClouds.combinationQuadTreeOctTree;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.locks.ReentrantLock;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.geometry.InclusionFunction;
import us.ihmc.robotics.hyperCubeTree.HyperCubeTreeListener;
import us.ihmc.robotics.quadTree.Box;
import us.ihmc.robotics.quadTree.QuadTreeForGround;
import us.ihmc.robotics.quadTree.QuadTreeForGroundParameters;
import us.ihmc.robotics.quadTree.QuadTreeForGroundPutResult;

public class QuadTreeForGroundHeightMap extends QuadTreeForGround implements QuadTreeHeightMapInterface
{
   private final ReentrantLock lock = new ReentrantLock();
   
   private QuadTreeForGroundReaderAndWriter readerAndWriter = null;
   
   public QuadTreeForGroundHeightMap(Box bounds, QuadTreeForGroundParameters quadTreeParameters)
   {
      super(bounds, quadTreeParameters);
   }
   
   public void setupTreeForGroundReaderAndWriter(QuadTreeForGroundReaderAndWriter readerAndWriter)
   {
      this.readerAndWriter = readerAndWriter;
   }

   @Override
   public boolean addPoint(double x, double y, double z)
   {
      lock();
      if (readerAndWriter != null) readerAndWriter.writePoint(x, y, z);
      
      // Set the default height to the first point you see if it were not set (ie NaN)
      if (super.isEmpty() && Double.isNaN(super.getDefaultHeightWhenNoPoints()))
      {
         super.setDefaultHeightWhenNoPoints(z);
      }
      
      
      QuadTreeForGroundPutResult result = put(x, y, z);
      
      unlock();
      return result.treeChanged;
   }

   @Override
   public boolean addToQuadtree(double x, double y, double z)
   {
      return addPoint(x, y, z);
   }

   @Override
   public boolean containsPoint(double x, double y)
   {
      double zValue = getHeightAtPoint(x, y);
      if ((Double.isNaN(zValue)))
         return true;

      return false;
   }

   @Override
   public List<Point3D> getAllPointsWithinArea(double xCenter, double yCenter, double xExtent, double yExtent)
   {
      return getAllPointsWithinArea(xCenter, yCenter, xExtent, yExtent, null);
   }

   @Override
   public List<Point3D> getAllPointsWithinArea(double xCenter, double yCenter, double xExtent, double yExtent,
           InclusionFunction<Point3D> maskFunctionAboutCenter)
   {
      ArrayList<Point3D> pointsWithinBoundsToPack = new ArrayList<Point3D>();
      ArrayList<Point3D> filteredPoints = new ArrayList<Point3D>();

      lock();
      Box bounds = new Box(xCenter - xExtent, yCenter - yExtent, xCenter + xExtent, yCenter + yExtent);
      super.getAllPointsWithinBounds(bounds, pointsWithinBoundsToPack);
      maskList(pointsWithinBoundsToPack, maskFunctionAboutCenter, filteredPoints);

      // TODO: Magic number 10. Get rid of it somehow...
      if (filteredPoints.size() > 10)
      {
         unlock();
         return filteredPoints;
      }

      // If not enough raw points, then use the heightAt function to do the best you can
      filteredPoints.clear();
      ArrayList<Point3D> pointsAtGridResolution = getPointsAtGridResolution(xCenter, yCenter, xExtent, yExtent);
      maskList(pointsAtGridResolution, maskFunctionAboutCenter, filteredPoints);
      unlock();
      return filteredPoints;
   }

   private ArrayList<Point3D> getPointsAtGridResolution(double centerX, double centerY, double extentX, double extentY)
   {
      ArrayList<Point3D> points = new ArrayList<Point3D>();

      lock();
      for (double x = centerX - extentX; x <= centerX + extentX; x += getQuadTreeParameters().getResolution())
      {
         for (double y = centerY - extentY; y <= centerY + extentY; y += getQuadTreeParameters().getResolution())
         {
            double height = getHeightAtPoint(x, y);
            if (!Double.isNaN(height))
            {
               points.add(new Point3D(x, y, height));
            }
         }
      }
      unlock();

      return points;
   }

   private static void maskList(ArrayList<Point3D> originalPoints, InclusionFunction<Point3D> maskFunctionAboutCenter, ArrayList<Point3D> maskedPointsToPack)
   {
      if (maskFunctionAboutCenter == null)
      {
         maskedPointsToPack.addAll(originalPoints);
      }

      else
      {
         for (Point3D point : originalPoints)
         {
            if (maskFunctionAboutCenter.isIncluded(point))
            {
               maskedPointsToPack.add(point);
            }
         }
      }
   }

   ArrayList<HyperCubeTreeListener<GroundAirDescriptor, GroundOnlyQuadTreeData>> hyperCubeTreeListeners = new ArrayList<HyperCubeTreeListener<GroundAirDescriptor,GroundOnlyQuadTreeData>>();
   
   @Override
   public void clearTree(double defaultGroundHeight)
   {
      lock();
      for (HyperCubeTreeListener<GroundAirDescriptor, GroundOnlyQuadTreeData> listener : this.hyperCubeTreeListeners)
      {
         listener.treeCleared();
      }
      
      this.clear();
      this.setDefaultHeightWhenNoPoints(defaultGroundHeight);
      unlock();
   }
   
   @Override
   public double getDefaultHeightWhenNoPoints()
   {
      return super.getDefaultHeightWhenNoPoints();
   }

   @Override
   public void addListener(HyperCubeTreeListener<GroundAirDescriptor, GroundOnlyQuadTreeData> listener)
   {
      lock();
      hyperCubeTreeListeners.add(listener);
      unlock();
   }

   @Override
   public void lock()
   {
      lock.lock();
   }

   @Override
   public void unlock()
   {
      lock.unlock();
   }

   @Override
   public boolean hasPoints()
   {
      boolean hasPoints = false;
      lock.lock();
      hasPoints = getRootNode().hasChildren();
      lock.unlock();
      
      return hasPoints;
   }

}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/AtlasFinalsROSAPISimulator.java<|end_filename|>
package us.ihmc.atlas;

import java.io.IOException;
import java.util.AbstractMap;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;

import org.ros.internal.message.Message;
import org.ros.message.MessageFactory;
import org.ros.node.NodeConfiguration;

import com.martiansoftware.jsap.JSAPException;

import ihmc_msgs.HandDesiredConfigurationRosMessage;
import us.ihmc.avatar.DRCStartingLocation;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.avatar.environments.DRCFinalsEnvironment;
import us.ihmc.avatar.ros.ROSAPISimulator;
import us.ihmc.avatar.ros.subscriber.IHMCMsgToPacketSubscriber;
import us.ihmc.avatar.simulationStarter.DRCSCStartingLocations;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.PacketDestination;
import us.ihmc.simulationConstructionSetTools.util.environments.CommonAvatarEnvironmentInterface;
import us.ihmc.utilities.ros.publisher.RosTopicPublisher;
import us.ihmc.utilities.ros.subscriber.RosTopicSubscriberInterface;

public class AtlasFinalsROSAPISimulator extends ROSAPISimulator
{
   private static final String ROBOT_NAME = "atlas";
   private static final String DEFAULT_ROBOT_MODEL = "ATLAS_UNPLUGGED_V5_NO_HANDS";

   private static final boolean CREATE_DOOR = true;
   private static final boolean CREATE_DRILL = true;
   private static final boolean CREATE_VALVE = true;
   private static final boolean CREATE_WALKING = true;
   private static final boolean CREATE_STAIRS = true;

   private final AtlasRobotVersion robotVersion;

   public AtlasFinalsROSAPISimulator(AtlasRobotModel robotModel, DRCStartingLocation startingLocation, String nameSpace, String tfPrefix,
         boolean runAutomaticDiagnosticRoutine, boolean disableViz) throws IOException
   {
      super(robotModel, startingLocation, nameSpace, tfPrefix, runAutomaticDiagnosticRoutine, disableViz);
      robotVersion = robotModel.getAtlasVersion();
   }

   @Override
   protected CommonAvatarEnvironmentInterface createEnvironment()
   {
      return new DRCFinalsEnvironment(CREATE_DOOR, CREATE_DRILL, CREATE_VALVE, CREATE_WALKING, CREATE_STAIRS);
   }

   @Override protected List<Map.Entry<String, RosTopicSubscriberInterface<? extends Message>>> createCustomSubscribers(String nameSpace, PacketCommunicator communicator)
   {
      List<Map.Entry<String, RosTopicSubscriberInterface<? extends Message>>> subscribers = new ArrayList<>();
      MessageFactory messageFactory = NodeConfiguration.newPrivate().getTopicMessageFactory();

      if(robotVersion.getHandModel().isHandSimulated())
      {
         HandDesiredConfigurationRosMessage message = messageFactory.newFromType("ihmc_msgs/FingerStateRosMessage");
         RosTopicSubscriberInterface<HandDesiredConfigurationRosMessage> sub = IHMCMsgToPacketSubscriber.createIHMCMsgToPacketSubscriber(message, communicator, PacketDestination.CONTROLLER.ordinal());
         Map.Entry<String, RosTopicSubscriberInterface<? extends Message>> pair = new AbstractMap.SimpleEntry<String, RosTopicSubscriberInterface<? extends Message>>(nameSpace + "/control/finger_state", sub);
         subscribers.add(pair);
      }

      return subscribers;
   }

   @Override protected List<Map.Entry<String, RosTopicPublisher<? extends Message>>> createCustomPublishers(String nameSpace, PacketCommunicator communicator)
   {
      return null;
   }

   public static void main(String[] args) throws JSAPException, IOException
   {
      Options opt = parseArguments(args);

      AtlasRobotModel robotModel;
      try
      {
         if (opt.robotModel.equals(DEFAULT_STRING))
         {
            robotModel = AtlasRobotModelFactory.createDRCRobotModel(DEFAULT_ROBOT_MODEL, RobotTarget.SCS, false);
         }
         else
         {
            robotModel = AtlasRobotModelFactory.createDRCRobotModel(opt.robotModel, RobotTarget.SCS, false);
         }
      }
      catch (IllegalArgumentException e)
      {
         System.err.println("Incorrect robot model " + opt.robotModel);
         System.out.println("Robot models: " + AtlasRobotModelFactory.robotModelsToString());
         return;
      }

      DRCStartingLocation startingLocation;
      try
      {
         startingLocation = DRCSCStartingLocations.valueOf(opt.startingLocation);
      }
      catch (IllegalArgumentException e)
      {
         System.err.println("Incorrect starting location " + opt.startingLocation);
         System.out.println("Starting locations: " + DRCSCStartingLocations.optionsToString());
         return;
      }

      String nameSpace = opt.nameSpace + "/" + ROBOT_NAME;

      new AtlasFinalsROSAPISimulator(robotModel, startingLocation, nameSpace, opt.tfPrefix, opt.runAutomaticDiagnosticRoutine, opt.disableViz);
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/HumanoidRobotDataReceiver.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import controller_msgs.msg.dds.RobotConfigurationData;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotModels.FullRobotModelUtils;
import us.ihmc.robotics.sensors.ForceSensorDataHolder;
import us.ihmc.sensorProcessing.communication.subscribers.RobotDataReceiver;

public class HumanoidRobotDataReceiver extends RobotDataReceiver  implements PacketConsumer<RobotConfigurationData>
{
   protected final HumanoidReferenceFrames referenceFrames;
   private final FullHumanoidRobotModel fullRobotModel;

   
   public HumanoidRobotDataReceiver(FullHumanoidRobotModel fullRobotModel, ForceSensorDataHolder forceSensorDataHolder)
   {
      super(fullRobotModel, FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel), forceSensorDataHolder);
      this.referenceFrames = new HumanoidReferenceFrames(fullRobotModel);
      this.fullRobotModel = fullRobotModel;

   }
   
   public HumanoidReferenceFrames getReferenceFrames()
   {
      return referenceFrames;
   }
   

   public HumanoidReferenceFrames getUpdatedReferenceFramesCopy()
   {
      updateRobotModel();
      HumanoidReferenceFrames ret = new HumanoidReferenceFrames(fullRobotModel);
      return ret;
   }
   
   @Override
   protected void updateFrames()
   {
      referenceFrames.updateFrames();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/DispatchHost.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;

import java.rmi.Naming;

import us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces.RemoteSimulationRunnerInterface;

public class DispatchHost
{
   private final String hostName;
   private final String hostProcess;

   private final String remoteSimulationRunnerName;

   private RemoteSimulationRunnerInterface remoteSim;
   private boolean isBroken = false;
   private SimulationToDispatch dispatchSim;

   private int NumberOfSimulationsRun = 0;
   private double approxTimePerSimulation = 60.0;
   private double lastSimulationTime = 60.0;

   private long inTime = System.currentTimeMillis();

   private boolean isRunningSim = false;
   private String password;

   public DispatchHost(String hostName, String password)
   {
      this.password = password;

      int separatorIndex = hostName.indexOf(":");

      if (separatorIndex < 0)
      {
         this.hostName = hostName;
         this.hostProcess = "0";
      }

      else
      {
         this.hostName = hostName.substring(0, separatorIndex);
         this.hostProcess = hostName.substring(separatorIndex + 1, hostName.length());
      }

      System.out.println("hostName = " + hostName + ", this.hostName = " + this.hostName + ", hostProcess = " + hostProcess);
      this.remoteSimulationRunnerName = "RemoteSimulationRunner" + hostProcess;
      lookUpRemoteSimulationRunner();

   }

   public String getHostName()
   {
      return hostName;
   }

   public String getHostProcess()
   {
      return hostProcess;
   }

   public void reportBroken()
   {
      remoteSim = null;
      isRunningSim = false;
      isBroken = true;
   }


   public int getNumberOfSimulationsRun()
   {
      return this.NumberOfSimulationsRun;
   }

   public int getApporxTimePerSimulation()
   {
      return (int) this.approxTimePerSimulation;
   }

   public int getlastSimulationTime()
   {
      return (int) this.lastSimulationTime;
   }

   public double getTimeSinceLastSimulationStarted()
   {
      long diffTimeMillis = System.currentTimeMillis() - inTime;

      return ((double) diffTimeMillis) / 1000.0;
   }


   public void reportSimulationStarted(SimulationToDispatch dispatchSim)
   {
      this.dispatchSim = dispatchSim;
      inTime = System.currentTimeMillis();
      isRunningSim = true;
   }

   private static final double alpha = 0.9;

   public void reportSimulationFinished()
   {
      NumberOfSimulationsRun++;
      isRunningSim = false;

      lastSimulationTime = getTimeSinceLastSimulationStarted();
      approxTimePerSimulation = alpha * approxTimePerSimulation + (1.0 - alpha) * lastSimulationTime;

      // System.out.println(lastSimulationTime + " seconds. " + hostName + " has run " + NumberOfSimulationsRun + " simulations at an approximate time of " + approxTimePerSimulation + " seconds per simulation");
      inTime = System.currentTimeMillis();
   }


   private Thread lookupThread = new Thread("Dispatch Host Lookup")
   {
      @Override
      public void run()
      {
         while (true)
         {
            if (!isBroken)
            {
               try
               {
                  // System.out.println("Looking up RemoteSimulationRunner for host: " + hostName); System.out.flush();
//                remoteSim = (RemoteSimulationRunnerInterface) Naming.lookup("//" + hostName + "/RemoteSimulationRunnerGoob");
                  remoteSim = (RemoteSimulationRunnerInterface) Naming.lookup("//" + hostName + "/" + remoteSimulationRunnerName);
                  System.out.println("Found RemoteSimulationRunner on Host: " + hostName);
                  System.out.flush();
                  pingRemoteSimulationRunner();

                  if (!remoteSim.isSimulationDone(password))
                  {
                     System.err.println(hostName + " already seems to be running a simulation");
                  }
               }
               catch (Exception e)
               {
                  remoteSim = null;
               }
            }

            try
            {
               Thread.sleep(600000);
            }    // Sleep 10 minutes until interrupted
            catch (InterruptedException e)
            {
            }
         }
      }
   };

   private synchronized void lookUpRemoteSimulationRunner()
   {
      if (!lookupThread.isAlive())
         lookupThread.start();
      else
         lookupThread.interrupt();

      /*
       * try
       * {
       * System.out.println("Looking up RemoteSimulationRunner for host: " + hostName); System.out.flush();
       * remoteSim = (RemoteSimulationRunnerInterface) Naming.lookup("//" + hostName + "/RemoteSimulationRunner");
       * System.out.println("Found RemoteSimulationRunner on Host: " + hostName); System.out.flush();
       * pingRemoteSimulationRunner();
       * }
       * catch(Exception e) {remoteSim = null;}
       */
   }

   private synchronized void pingRemoteSimulationRunner()
   {
      try
      {
         remoteSim.ping(password);

         // System.out.println(hostName + " pinged successfully!");
      }
      catch (Exception e)
      {
         System.out.println("Remote Simulation Runner on host " + hostName + "couldn't be pinged!!");
         remoteSim = null;
         isRunningSim = false;
      }
   }

   public RemoteSimulationRunnerInterface getRemoteSim()
   {
      return remoteSim;
   }

   public SimulationToDispatch getDispatchedSim()
   {
      return dispatchSim;
   }

   public boolean isAlive()
   {
      return (remoteSim != null);
   }

   public boolean isRunningSim()
   {
      return this.isRunningSim;
   }

   protected void setIsRunningSim(boolean isRunningSim)
   {
      this.isRunningSim = isRunningSim;
   }

   public void pingAndOrFind()
   {
      if (remoteSim != null)
      {
         pingRemoteSimulationRunner();
      }
      else
      {
         lookUpRemoteSimulationRunner();
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BoundingBox3DMessage.java<|end_filename|>
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;

public class BoundingBox3DMessage extends Packet<BoundingBox3DMessage> implements Settable<BoundingBox3DMessage>, EpsilonComparable<BoundingBox3DMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.tuple3D.Point3D min_point_;
   public us.ihmc.euclid.tuple3D.Point3D max_point_;

   public BoundingBox3DMessage()
   {
      min_point_ = new us.ihmc.euclid.tuple3D.Point3D();
      max_point_ = new us.ihmc.euclid.tuple3D.Point3D();
   }

   public BoundingBox3DMessage(BoundingBox3DMessage other)
   {
      this();
      set(other);
   }

   public void set(BoundingBox3DMessage other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.min_point_, min_point_);
      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.max_point_, max_point_);
   }

   /**
            * 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 us.ihmc.euclid.tuple3D.Point3D getMinPoint()
   {
      return min_point_;
   }


   public us.ihmc.euclid.tuple3D.Point3D getMaxPoint()
   {
      return max_point_;
   }


   public static Supplier<BoundingBox3DMessagePubSubType> getPubSubType()
   {
      return BoundingBox3DMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return BoundingBox3DMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(BoundingBox3DMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.min_point_.epsilonEquals(other.min_point_, epsilon)) return false;
      if (!this.max_point_.epsilonEquals(other.max_point_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof BoundingBox3DMessage)) return false;

      BoundingBox3DMessage otherMyClass = (BoundingBox3DMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.min_point_.equals(otherMyClass.min_point_)) return false;
      if (!this.max_point_.equals(otherMyClass.max_point_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("BoundingBox3DMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("min_point=");
      builder.append(this.min_point_);      builder.append(", ");
      builder.append("max_point=");
      builder.append(this.max_point_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-model-file-loader/src/main/java/us/ihmc/modelFileLoaders/SdfLoader/SDFRoadVisual.java<|end_filename|>
package us.ihmc.modelFileLoaders.SdfLoader;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.modelFileLoaders.ModelFileLoaderConversionsHelper;
import us.ihmc.modelFileLoaders.SdfLoader.xmlDescription.SDFWorld.Road;

public class SDFRoadVisual extends Graphics3DObject
{
   public SDFRoadVisual(Road road)
   {
      double width = Double.parseDouble(road.getWidth());
      
      AppearanceDefinition appearance = YoAppearance.Texture("models/road1.jpg");
      
      Point3D startLeft = new Point3D();
      Point3D startRight = new Point3D();
      Point3D endLeft = new Point3D();
      Point3D endRight = new Point3D();
      for(int i = 0 ; i < road.getPoints().size() - 1; i++)
      {
         Point3D start = new Point3D(ModelFileLoaderConversionsHelper.stringToVector3d(road.getPoints().get(i)));
         Point3D end = new Point3D(ModelFileLoaderConversionsHelper.stringToVector3d(road.getPoints().get(i+1)));
         
         Vector3D direction = getDirection(start, end);
         
         
         Vector3D toSide = getPerpendicularVectorOfLength(width, direction);
         
         if(i == 0)
         {
            startLeft.add(start, toSide);
            startRight.sub(start, toSide);
         }
         else
         {
            startLeft.set(endLeft);
            startRight.set(endRight);
         }
         
         if(i < road.getPoints().size() - 2)
         {
            Point3D startNext = end;
            Point3D endNext = new Point3D(ModelFileLoaderConversionsHelper.stringToVector3d(road.getPoints().get(i+2)));
            Vector3D directionNext = getDirection(startNext, endNext);
            Vector3D nextSide = getPerpendicularVectorOfLength(width, directionNext);

            Vector2D toSide2d = new Vector2D(toSide.getX(), toSide.getY());
            Vector2D nextSide2d = new Vector2D(nextSide.getX(), nextSide.getY());
            double angle = toSide2d.angle(nextSide2d);
            
            RigidBodyTransform rotZ = new RigidBodyTransform();
            rotZ.setRotationYawAndZeroTranslation(angle/2.0);
            
            rotZ.transform(toSide);
            toSide.normalize();
            toSide.scale((width/2.0)/Math.cos(angle/2.0));
            
            
         }
         
         endLeft.add(end, toSide);
         endRight.sub(end, toSide);
         
         addPolygon(appearance, startRight, endRight, endLeft, startLeft);
         
      }
   }


   private Vector3D getPerpendicularVectorOfLength(double width, Vector3D direction)
   {
      Vector3D toSide = new Vector3D(-direction.getY(), direction.getX(), 0);
      toSide.normalize();
      toSide.scale(width/2.0);
      return toSide;
   }
   
   
   private Vector3D getDirection(Point3D a, Point3D b)
   {
      Vector3D direction = new Vector3D(b);
      direction.sub(a);
      direction.scale(0.5);
      return direction;
   }
   
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tree/ParameterTreeValue.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tree;

import javafx.scene.Node;
import us.ihmc.parameterTuner.guiElements.GuiParameter;

public interface ParameterTreeValue
{
   public boolean isRegistry();

   public String getName();

   public Node getOrCreateNode();

   public GuiParameter getParameter();
}


<|start_filename|>ihmc-robot-models/src/main/java/us/ihmc/robotModels/FullHumanoidRobotModel.java<|end_filename|>
package us.ihmc.robotModels;

import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.partNames.ArmJointName;
import us.ihmc.robotics.partNames.LimbName;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public interface FullHumanoidRobotModel extends FullLeggedRobotModel<RobotSide>
{
   /**
    * Returns the {@link RigidBodyBasics} describing the chest (or trunk) of this robot.
    * The chest is considered to be located right after the spine joints, and right before the arms and head.
    */
   RigidBodyBasics getChest();

   /**
    * Returns the {@link RigidBodyBasics} describing the pelvis of this robot.
    * In the current framework (on the day: 3/1/2018), the pelvis is the the first successor of the root joint.
    */
   RigidBodyBasics getPelvis();

   /**
    * Return the {@link OneDoFJointBasics} describing the the corresponding arm joint.
    * @param robotSide Refers to which arm the joint belongs to (assuming there is only one left arm and one right arm).
    * @param armJointName Refers to the joint's name.
    */
   OneDoFJointBasics getArmJoint(RobotSide robotSide, ArmJointName armJointName);

   /**
    * Returns the {@link RigidBodyBasics} describing the left or right hand of this robot.
    * A hand is most of the time considered as the end-effector of the leg to which it belongs to.
    * However, hand palm and fingers are located after the hand.
    *
    * @param robotSide Refers to which side the foot belongs to (assuming there is only one left foot and one right foot).
    */
   RigidBodyBasics getHand(RobotSide robotSide);

   /**
    * Returns a control frame attached to the right or left hand that the controller uses to control the hand in taskspace.
    * @param robotSide
    * @return
    */
   MovingReferenceFrame getHandControlFrame(RobotSide robotSide);

   @Override
   default RobotSide[] getRobotSegments()
   {
      return RobotSide.values;
   }

   @Override
   SideDependentList<MovingReferenceFrame> getSoleFrames();

   void setJointAngles(RobotSide side, LimbName limb, double[] q);
}

<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/contactable/ContactableBody.java<|end_filename|>
package us.ihmc.robotics.contactable;

import java.util.List;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public interface ContactableBody
{
   public abstract String getName();

   public abstract RigidBodyBasics getRigidBody();

   public abstract MovingReferenceFrame getFrameAfterParentJoint();

   public abstract int getTotalNumberOfContactPoints();

   public abstract List<FramePoint3D> getContactPointsCopy();
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/subscriber/IHMCMsgToPacketSubscriber.java<|end_filename|>
package us.ihmc.avatar.ros.subscriber;

import java.lang.reflect.InvocationTargetException;

import org.ros.internal.message.Message;

import us.ihmc.avatar.ros.IHMCROSTranslationRuntimeTools;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.Packet;
import us.ihmc.utilities.ros.msgToPacket.converter.RosEnumConversionException;
import us.ihmc.utilities.ros.subscriber.AbstractRosTopicSubscriber;

public class IHMCMsgToPacketSubscriber<T extends Message> extends AbstractRosTopicSubscriber<T>
{
   private final PacketCommunicator controllerCommunicator;
   private int packetDestination;

   public IHMCMsgToPacketSubscriber(String messageType, PacketCommunicator communicator, int packetDestination)
   {
      super(messageType);
      this.controllerCommunicator = communicator;
      this.packetDestination = packetDestination;
   }

   @Override
   public void onNewMessage(T message)
   {
      try
      {
         Packet<?> packet = IHMCROSTranslationRuntimeTools.convertToIHMCMessage(message);
         packet.setDestination(packetDestination);
         controllerCommunicator.send(packet);
      }
      catch (ClassNotFoundException | InvocationTargetException | RosEnumConversionException | IllegalAccessException | InstantiationException | NoSuchFieldException | IllegalArgumentException | NoSuchMethodException | SecurityException e)
      {
         System.err.println("Could not convert ROS Message to IHMC Message. Will not publish.");
         System.err.println("Original ROS message: " + message);
         e.printStackTrace();
      }
   }

   public static <U extends Message> IHMCMsgToPacketSubscriber<U> createIHMCMsgToPacketSubscriber(U msg, PacketCommunicator communicator, int packetDestination)
   {
      return new IHMCMsgToPacketSubscriber<U>(msg.toRawMessage().getType(), communicator, packetDestination);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/socketCommunication/UserCommandListener.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.socketCommunication;

public interface UserCommandListener
{
   // public void doUserCommand(String string);
   public void doUserCommand(String command, UserCommandNetworkReader reader);
}


<|start_filename|>websitedocs/website/static/snippetautomation/codesnippets.js<|end_filename|>
hljs.initHighlightingOnLoad();

var script = document.getElementById("snippetscript");
var sources = eval(script.getAttribute('sources'));
var numberOfSources = sources.length;

// Getting the attributes from all the code blocks
var allCodeBlocks = Array.from(document.getElementsByTagName('code'));
var urls = []; // array of data from each source

// Throws error on invalid url
function notValidURL(response) {
	if (!response.ok) {
		throw Error(response.statusText);
	}
	return response;
}

// Get and operate on data from source files
for (i = 0; i < numberOfSources; i++) {
	urls[i] = fetch(sources[i]).then(notValidURL).then(function(response) {
		return response.text()
	});
}

Promise
		.all(urls)
		.then(
				function(values) {
					for (i = 0; i < numberOfSources; i++) {
						var dataFromSource = values[i];
						var matchIndex = allCodeBlocks.findIndex(function(
								element) {
							return element.getAttribute('data-url-index') == i
									.toString();
						});

						// For each URL, find all the code blocks with matching
						// data-url-index
						while (matchIndex != -1) {

							// Change inner HTML of code block
							var codeBlock = allCodeBlocks[matchIndex];
							var typeOfSnippet = codeBlock
									.getAttribute('data-snippet');
							var codeChunk = "";
							var startIndex, endindex;

							if (typeOfSnippet == "complete") {
								codeChunk = dataFromSource;
							} else if (typeOfSnippet == "portion") {
								startIndex = dataFromSource.indexOf(codeBlock
										.getAttribute('data-start'));
								if(startIndex < 0) throw "Start string not found at element id: " + codeBlock.id;

								// Substring from index to the rest of file
								if (codeBlock.getAttribute('data-end') === null) {
									codeChunk = dataFromSource
											.substring(startIndex);
								} else {
									endIndex = dataFromSource.indexOf(codeBlock
											.getAttribute('data-end'),
											startIndex);
									if(endIndex < 0) throw "End string not found at element id: " + codeBlock.id;
									codeChunk = dataFromSource.substring(
											startIndex, endIndex);
								}
							} else
							// If the snippet involves multiple portions,
							// data-snippet="multipleportions"
							{
								var portions = eval(codeBlock
										.getAttribute('data-portions'));
								for (j = 0; j < portions.length; j++) {
									startIndex = dataFromSource
											.indexOf(portions[j][0]);
									if(startIndex < 0) throw "Start string not found at element id: " + codeBlock.id;

									// Substring with start index to rest of
									// file
									if (portions[j].length == 1) {
										codeChunk = codeChunk
												+ "\n"
												+ dataFromSource
														.substring(startIndex);
									} else {
										endIndex = dataFromSource.indexOf(
												portions[j][1], startIndex);
										if(endIndex < 0) throw "End string not found at element id: " + codeBlock.id;
										codeChunk = codeChunk
												+ "\n"
												+ dataFromSource.substring(
														startIndex, endIndex);
									}
								}
							}
							codeBlock.innerHTML = hljs.highlight('java',
									codeChunk).value;
							allCodeBlocks.splice(matchIndex, 1);

							matchIndex = allCodeBlocks
									.findIndex(function(element) {
										return element
												.getAttribute('data-url-index') == i
												.toString();
									});
						}
					}
				});


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/CartRobotRacingEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.List;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class CartRobotRacingEnvironment implements CommonAvatarEnvironmentInterface
{
   private final CombinedTerrainObject3D combinedTerrainObject;

   private final int numberOfPlates = 5;
   private final double plateSize = 2.0;
   private final double plateHeightGap = 0.3;
   private final double plateThickness = 0.1;

   public CartRobotRacingEnvironment(boolean useRampTerrain)
   {
      combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());

      for (int i = 0; i < numberOfPlates; i++)
      {
         combinedTerrainObject.addBox(-0.5 * plateSize + plateSize * i, -0.5 * plateSize, 0.5 * plateSize + plateSize * i, 0.5 * plateSize,
                                      -plateThickness - plateHeightGap * i, -plateHeightGap * i);
      }

      combinedTerrainObject.addSphere(0.5, 0.0, 0.0, 0.05, YoAppearance.AliceBlue());

      RigidBodyTransform cylinderObstacleTransform = new RigidBodyTransform();
      cylinderObstacleTransform.appendRollRotation(0.5 * Math.PI);

      combinedTerrainObject.addCylinder(cylinderObstacleTransform, 0.3, 0.05, YoAppearance.AliceBlue());
      
      if (useRampTerrain)
         combinedTerrainObject.addRamp(0.8, -0.7, 2.0, 0.7, 0.5, YoAppearance.AliceBlue());
   }

   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return null;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {

   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {

   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {

   }

}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasCollisionMeshDefinitionDataHolder.java<|end_filename|>
package us.ihmc.atlas.parameters;

import us.ihmc.atlas.AtlasJointMap;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.BoxCollisionMeshDefinitionData;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.CollisionMeshDefinitionData;
import us.ihmc.robotics.robotDescription.collisionMeshDefinitionData.CollisionMeshDefinitionDataHolder;
import us.ihmc.robotics.robotSide.RobotSide;

public class AtlasCollisionMeshDefinitionDataHolder extends CollisionMeshDefinitionDataHolder
{
   public AtlasCollisionMeshDefinitionDataHolder(AtlasJointMap jointMap, AtlasPhysicalProperties atlasPhysicalProperties)
   {
      double footLength = atlasPhysicalProperties.getActualFootLength();
      double footWidth = atlasPhysicalProperties.getActualFootWidth();
      double footHeight = Math.abs(atlasPhysicalProperties.getSoleToAnkleFrameTransforms().get(RobotSide.RIGHT).getM23());

      RigidBodyTransform transformToAnkle = new RigidBodyTransform(atlasPhysicalProperties.getSoleToAnkleFrameTransforms().get(RobotSide.RIGHT));
      transformToAnkle.appendTranslation(0.0, 0, 0.5 * footHeight);

      CollisionMeshDefinitionData rightFootCollisionMeshData = new BoxCollisionMeshDefinitionData(jointMap.getLegJointName(RobotSide.RIGHT,
                                                                                                                           LegJointName.ANKLE_ROLL),
                                                                                                  footLength, footWidth, footHeight);
      rightFootCollisionMeshData.setTransformToParentJoint(transformToAnkle);

      addCollisionMeshDefinitionData(rightFootCollisionMeshData);

      CollisionMeshDefinitionData leftFootCollisionMeshData = new BoxCollisionMeshDefinitionData(jointMap.getLegJointName(RobotSide.LEFT,
                                                                                                                          LegJointName.ANKLE_ROLL),
                                                                                                 footLength, footWidth, footHeight);
      leftFootCollisionMeshData.setTransformToParentJoint(transformToAnkle);

      addCollisionMeshDefinitionData(leftFootCollisionMeshData);
   }
}


<|start_filename|>ihmc-robot-builder/src/main/java/us/ihmc/robotbuilder/MainWindow.java<|end_filename|>
package us.ihmc.robotbuilder;

import java.io.File;

import javafx.application.Application;
import javafx.fxml.FXML;
import javafx.fxml.FXMLLoader;
import javafx.scene.Group;
import javafx.scene.Node;
import javafx.scene.Parent;
import javafx.scene.PerspectiveCamera;
import javafx.scene.Scene;
import javafx.scene.SceneAntialiasing;
import javafx.scene.SubScene;
import javafx.scene.control.Alert;
import javafx.scene.control.Alert.AlertType;
import javafx.scene.control.ButtonType;
import javafx.scene.control.ChoiceDialog;
import javafx.scene.control.TreeItem;
import javafx.scene.control.TreeView;
import javafx.scene.input.KeyEvent;
import javafx.scene.input.MouseEvent;
import javafx.scene.layout.Pane;
import javafx.scene.paint.Color;
import javafx.scene.paint.PhongMaterial;
import javafx.scene.shape.Sphere;
import javafx.scene.transform.Translate;
import javafx.stage.FileChooser;
import javafx.stage.Stage;
import javaslang.control.Option;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.javaFXToolkit.cameraControllers.SimpleCameraKeyboardEventHandler;
import us.ihmc.javaFXToolkit.cameraControllers.SimpleCameraMouseEventHandler;
import us.ihmc.robotbuilder.model.Loader;
import us.ihmc.robotbuilder.util.JavaFX3DInstructionExecutor;
import us.ihmc.robotbuilder.util.Tree;
import us.ihmc.robotbuilder.util.TreeAdapter;
import us.ihmc.robotbuilder.util.Util;
import us.ihmc.robotics.robotDescription.JointDescription;
import us.ihmc.robotics.robotDescription.RobotDescription;
import us.ihmc.robotics.robotDescription.RobotDescriptionNode;

/**
 *
 */
public class MainWindow extends Application {
    @FXML
    private Pane view3D;

    @FXML
    private TreeView<String> treeView;

    private Stage stage;

    @Override
    public void start(Stage primaryStage) throws Exception {
        Parent root = FXMLLoader.load(getClass().getResource("/main_window.fxml"));

        Scene scene = new Scene(root, 300, 275);

        this.stage = primaryStage;
        stage.setTitle("IHMC Robot Builder");
        stage.setScene(scene);
        stage.setMaximized(true);
        stage.show();
    }

    /**
     * File -> Open action
     */
    public void onOpen() {
        FileChooser chooser = new FileChooser();
        chooser.setTitle("Open Robot Definition File");
        if (System.getProperty("initial.dir") != null)
            chooser.setInitialDirectory(new File(System.getProperty("initial.dir")));
        File file = chooser.showOpenDialog(stage);
        if (file == null)
            return;

        Loader.loadFile(file, options -> Util.runLaterInUI(() -> {
            if (options.isEmpty())
                return Option.none();
            if (options.length() == 1)
                return Option.of(options.get(0));

            // Let the user choose a model to load
            ChoiceDialog<String> dialog = new ChoiceDialog<>(options.get(0), options.toJavaArray(String.class));
            dialog.setTitle("Model Selection");
            dialog.setHeaderText("Please choose a model to open.");
            dialog.setContentText("Model:");
            return Option.ofOptional(dialog.showAndWait());
        })).flatMap(robotDescription -> Util.runLaterInUI(() -> {
            robotDescription.peek(description -> {
                TreeAdapter<RobotDescriptionNode> tree = Tree.of(description, RobotDescriptionNode::getChildrenJoints);
                treeView.setRoot(Tree.map(tree, (node, children) -> {
                    TreeItem<String> item = new TreeItem<>(node.getValue().getName());
                    item.getChildren().addAll(children);
                    return item;
                }));
                populate3DView(description);
            });
            return null;
        })).onFailure(err -> Util.runLaterInUI(() -> {
            Alert alert = new Alert(AlertType.ERROR, "Error loading file: " + err.getMessage(), ButtonType.OK);
            alert.showAndWait();
            return null;
        }));
    }

    private void populate3DView(RobotDescription description) {
        Group group = new Group();
        description.getChildrenJoints().stream().map(this::create3DNodes).forEach(group.getChildren()::add);

        SubScene scene3d = new SubScene(group, view3D.getWidth(), view3D.getHeight(), true, SceneAntialiasing.DISABLED);
        scene3d.setFill(Color.BLACK);
        PerspectiveCamera camera = Util.lookAtNodeFromDirection(group, 60, new Vector3D(1, 0, 0), new Vector3D(0, 1, 0));
        scene3d.setCamera(camera);

        SimpleCameraMouseEventHandler mouseController = new SimpleCameraMouseEventHandler(camera);
        SimpleCameraKeyboardEventHandler keyboardController = new SimpleCameraKeyboardEventHandler(camera);
        scene3d.addEventHandler(MouseEvent.ANY, mouseController);
        scene3d.addEventHandler(KeyEvent.ANY, keyboardController);
        scene3d.setFocusTraversable(true);
        scene3d.requestFocus();

        view3D.getChildren().add(scene3d);
    }

    private Node create3DNodes(JointDescription description) {
        TreeAdapter<JointDescription> tree = Tree.of(description, JointDescription::getChildrenJoints);
        return Tree.map(tree, (node, children) -> {
                Group jointGroup = new Group();
                Vector3D offset = new Vector3D();
                node.getValue().getOffsetFromParentJoint(offset);
                jointGroup.getTransforms().add(new Translate(offset.getX(), offset.getY(), offset.getZ()));

                final PhongMaterial redMaterial = new PhongMaterial();
                redMaterial.setDiffuseColor(Color.DARKRED);
                redMaterial.setSpecularColor(Color.RED);

                Sphere graphics = new Sphere(0.05);
                graphics.setMaterial(redMaterial);
                jointGroup.getChildren().add(graphics);

                JavaFX3DInstructionExecutor executor = new JavaFX3DInstructionExecutor(node.getValue().getLink().getLinkGraphics().getGraphics3DInstructions());
                jointGroup.getChildren().add(executor.getResult());

                jointGroup.getChildren().addAll(children);
                return jointGroup;
        });
    }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/initialSetup/SquaredUpDRCDemo01RobotAtPlatformsInitialSetup.java<|end_filename|>
package us.ihmc.avatar.initialSetup;

import us.ihmc.euclid.tuple3D.Vector3D;

public class SquaredUpDRCDemo01RobotAtPlatformsInitialSetup extends OffsetAndYawRobotInitialSetup
{
   private static final Vector3D firstPlatform = new Vector3D(-1.6, -3.2, 0.0);
   private static final Vector3D lastPlatform = new Vector3D(-6.5, -8.2, 0.0);
   
   private static final double offsetX = -1.8943, offsetY = -1.8745;
   private static final double yaw = Math.atan2(offsetY, offsetX);

   private SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(Vector3D additionalOffset, double yaw)
   {
      super(additionalOffset, yaw);
   }
   
   public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthPlatform(int nthPlatform)
   {
      double alpha = ((double) nthPlatform)/7.0;
      Vector3D startingLocation = morph(firstPlatform, lastPlatform, alpha);
      
      return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw);
   }

   public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(int nthWall)
   {
      double alpha = ((double) nthWall)/7.0;
      Vector3D startingLocation = morph(firstPlatform, lastPlatform, alpha);
      startingLocation.add(new Vector3D(-1.0 - 0.1, 1.0 - 0.1, 0.0));
      return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw - Math.PI/2.0);
   }
   
   public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthPlatformToesTouching(int nthPlatform)
   {
      double alpha = ((double) nthPlatform)/7.0;
      Vector3D startingLocation = morph(firstPlatform, lastPlatform, alpha);
      
      startingLocation.add(new Vector3D(-0.11, -0.16, 0.0));
      
      return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw);
   }
   
   public static Vector3D morph(Vector3D point1, Vector3D point2, double alpha)
   {
      Vector3D framePoint1 = new Vector3D(point1);
      Vector3D framePoint2 = new Vector3D(point2);

      framePoint1.scale(1.0 - alpha);
      framePoint2.scale(alpha);
      framePoint1.add(framePoint2);


      return framePoint1;
   }

}



<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/contactPoints/CenteredFrictionConeCalculator.java<|end_filename|>
package us.ihmc.wholeBodyController.contactPoints;

import java.util.List;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;

public class CenteredFrictionConeCalculator implements FrictionConeRotationCalculator
{
   private final ConvexPolygon2D supportPolygon = new ConvexPolygon2D();
   private final Vector2D centroidToPoint = new Vector2D();
   private final Vector2D xAxis = new Vector2D(1.0, 0.0);

   @Override
   public double computeYawOffset(List<Point2D> contactPoints, int contactIdx, int vectors, int vectorIdx)
   {
      supportPolygon.set(Vertex2DSupplier.asVertex2DSupplier(contactPoints));
      Point2DReadOnly centroid = supportPolygon.getCentroid();
      Point2D contactPoint = contactPoints.get(contactIdx);
      centroidToPoint.sub(contactPoint, centroid);
      double angle = centroidToPoint.angle(xAxis);
      double dotProduct = centroidToPoint.dot(xAxis);
      if (dotProduct < 0.0)
      {
         angle = Math.PI - angle;
      }
      return angle;
   }

}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tabs/TuningBox.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tabs;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;

import javafx.application.Platform;
import javafx.event.EventHandler;
import javafx.geometry.Insets;
import javafx.geometry.Pos;
import javafx.scene.Node;
import javafx.scene.control.Alert;
import javafx.scene.control.Alert.AlertType;
import javafx.scene.control.Button;
import javafx.scene.control.Label;
import javafx.scene.image.Image;
import javafx.scene.image.ImageView;
import javafx.scene.input.ClipboardContent;
import javafx.scene.input.DragEvent;
import javafx.scene.input.Dragboard;
import javafx.scene.input.MouseEvent;
import javafx.scene.input.TransferMode;
import javafx.scene.layout.HBox;
import javafx.scene.layout.Region;
import javafx.scene.layout.VBox;
import javafx.scene.shape.Rectangle;
import javafx.util.Pair;
import us.ihmc.commons.PrintTools;
import us.ihmc.parameterTuner.guiElements.GuiElement;
import us.ihmc.parameterTuner.guiElements.tuners.Tuner;
import us.ihmc.robotics.sliderboard.Sliderboard;

public class TuningBox extends VBox
{
   public static final double imageSize = 20.0;
   public static final double buttonPadding = 5.0;

   private final Image closeImage;
   private final Image moveImage;

   private final List<String> parametersBeingTuned = new ArrayList<>();
   private final Map<String, Button> removeButtons = new HashMap<>();
   private final Map<String, SliderButton> sliderButtons = new HashMap<>();
   private final Map<String, Node> dragNodes = new HashMap<>();
   private final Map<String, EventHandler<DragEvent>> dragOverEvents = new HashMap<>();

   private Map<String, Tuner> tunerMap;

   private Sliderboard sliderboard;

   private final Rectangle placeholder = new Rectangle(100, 3);

   public TuningBox()
   {
      setFillWidth(true);
      setSpacing(10.0);
      closeImage = new Image(TuningBox.class.getResourceAsStream("/close.png"));
      moveImage = new Image(TuningBox.class.getResourceAsStream("/move.png"));
   }

   public void handleNewParameter(String uniqueName, int sliderIndex)
   {
      if (parametersBeingTuned.contains(uniqueName))
      {
         Platform.runLater(() -> {
            Alert alert = new Alert(AlertType.INFORMATION);
            alert.setTitle("Information Dialog");
            alert.setHeaderText("Can not add parameter");
            alert.setContentText("A parameter with the same name is being tuned.");
            alert.showAndWait();
         });
         return;
      }

      String[] split = uniqueName.split(GuiElement.SEPERATOR);
      String simpleName = split[split.length - 1];

      Button remove = new Button(null);
      remove.setPadding(new Insets(buttonPadding));
      ImageView removeGraphic = new ImageView(closeImage);
      removeGraphic.setFitWidth(imageSize);
      removeGraphic.setFitHeight(imageSize);
      remove.setGraphic(removeGraphic);
      removeButtons.put(uniqueName, remove);
      remove.setOnAction(event -> {
         parametersBeingTuned.remove(uniqueName);
         updateView();
      });

      sliderButtons.put(uniqueName, new SliderButton(sliderIndex));

      Label drag = new Label();
      ImageView moveGraphic = new ImageView(moveImage);
      moveGraphic.setFitWidth(imageSize);
      moveGraphic.setFitHeight(imageSize);
      drag.setGraphic(moveGraphic);
      dragNodes.put(uniqueName, drag);
      // When a drag starts remove the parameter from the tuning box and replace it with the placeholder.
      drag.setOnDragDetected(new EventHandler<MouseEvent>()
      {
         @Override
         public void handle(MouseEvent event)
         {
            Dragboard dragboard = drag.startDragAndDrop(TransferMode.MOVE);
            dragboard.setDragView(TextImage.create(simpleName), -15.0, 5.0);
            ClipboardContent clipboardContent = new ClipboardContent();
            clipboardContent.putString(uniqueName);
            dragboard.setContent(clipboardContent);

            int oldIndex = parametersBeingTuned.lastIndexOf(uniqueName);
            parametersBeingTuned.remove(uniqueName);
            updateView();
            getChildren().add(oldIndex, placeholder);
         }
      });
      // When a drag ends insert the dragged parameter at the placeholder position.
      drag.setOnDragDone(new EventHandler<DragEvent>()
      {
         @Override
         public void handle(DragEvent event)
         {
            String uniqueNameToBeDropped = event.getDragboard().getString();
            int newIndex = getChildren().lastIndexOf(placeholder);
            if (uniqueNameToBeDropped == null || uniqueNameToBeDropped.isEmpty() || newIndex == -1)
            {
               event.consume();
               return;
            }

            parametersBeingTuned.add(newIndex, uniqueNameToBeDropped);
            updateView();
            event.consume();
         }
      });
      // As the drag passes over the parameters being tunes move the placeholder accordingly.
      dragOverEvents.put(uniqueName, new EventHandler<DragEvent>()
      {
         @Override
         public void handle(DragEvent event)
         {
            event.acceptTransferModes(TransferMode.MOVE);

            String uniqueNameToBeDropped = event.getDragboard().getString();
            int newIndex = parametersBeingTuned.lastIndexOf(uniqueName);
            if (uniqueNameToBeDropped == null || uniqueNameToBeDropped.isEmpty() || newIndex == -1)
            {
               event.consume();
               return;
            }
            if (event.getGestureTarget() == null)
            {
               event.consume();
               return;
            }
            if (!(event.getGestureTarget() instanceof Region))
            {
               PrintTools.warn("Gesture Target should be a region. Is a " + event.getGestureTarget().getClass().getSimpleName());
               event.consume();
               return;
            }
            if (2.0 * event.getY() > ((Region) event.getGestureTarget()).getHeight())
            {
               newIndex++;
            }

            getChildren().remove(placeholder);
            getChildren().add(newIndex, placeholder);
            event.consume();
         }
      });

      parametersBeingTuned.add(uniqueName);
      updateView();
   }

   public void setTunerMap(Map<String, Tuner> tunerMap)
   {
      this.tunerMap = tunerMap;
      List<String> invalidParameters = parametersBeingTuned.stream().filter(name -> !tunerMap.containsKey(name)).collect(Collectors.toList());
      invalidParameters.forEach(name -> parametersBeingTuned.remove(name));
      updateView();
   }

   public void setSliderboard(Sliderboard sliderboard)
   {
      this.sliderboard = sliderboard;
      updateView();
   }

   public void updateView()
   {
      getChildren().clear();
      parametersBeingTuned.forEach(uniqueName -> {
         Tuner tuner = tunerMap.get(uniqueName);
         HBox box = new HBox(10.0);
         box.setAlignment(Pos.CENTER_LEFT);
         box.getChildren().add(dragNodes.get(uniqueName));
         if (sliderboard != null && sliderboard.isConnected())
         {
            SliderButton sliderButton = sliderButtons.get(uniqueName);
            sliderButton.link(tuner, sliderboard);
            box.getChildren().add(sliderButton);
         }
         box.getChildren().add(removeButtons.get(uniqueName));
         box.getChildren().add(tuner);
         box.setOnDragOver(dragOverEvents.get(uniqueName));

         getChildren().add(box);
      });
   }

   public void hide()
   {
      parametersBeingTuned.forEach(uniqueName -> {
         sliderButtons.get(uniqueName).unlink();
      });
   }

   public List<Pair<String, Integer>> getParameterSavingInfo()
   {
      List<Pair<String, Integer>> info = new ArrayList<>();
      parametersBeingTuned.forEach(uniqueName -> {
         int sliderIndex = sliderButtons.get(uniqueName).getIndex();
         info.add(new Pair<>(uniqueName, new Integer(sliderIndex)));
      });
      return Collections.unmodifiableList(info);
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/HumanoidRobotKickMessenger.java<|end_filename|>
package us.ihmc.avatar.joystickBasedJavaFXController;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SegmentDependentList;

public interface HumanoidRobotKickMessenger
{
   void sendFlamingoHomeStance(RobotSide robotSide, double trajectoryDuration, double stanceWidth, SegmentDependentList<RobotSide, ? extends ReferenceFrame> soleFrames);

   void sendKick(RobotSide robotSide, double trajectoryDuration, double stanceWidth, SegmentDependentList<RobotSide, ? extends ReferenceFrame> segmentDependentList);

   void sendPutFootDown(RobotSide robotSide, double trajectoryDuration, double stanceWidth, SegmentDependentList<RobotSide, ? extends ReferenceFrame> soleFrames);
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/NetClassList.java<|end_filename|>
package us.ihmc.communication.net;

import java.util.ArrayList;

import com.esotericsoftware.kryo.Kryo;

import controller_msgs.msg.dds.IMUPacket;
import controller_msgs.msg.dds.SpatialVectorMessage;
import gnu.trove.map.TIntObjectMap;
import gnu.trove.map.TObjectIntMap;
import gnu.trove.map.hash.TIntObjectHashMap;
import gnu.trove.map.hash.TObjectIntHashMap;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.idl.IDLSequence;

public class NetClassList
{
   public static interface PacketTrimmer<T>
   {
      T trim(T messageToTrim);
   }

   private final ArrayList<Class<?>> classList = new ArrayList<Class<?>>();
   private final ArrayList<Class<?>> typeList = new ArrayList<Class<?>>();

   public int uniquePacketID = 0;
   public final TObjectIntMap<Class<?>> registrationIDs = new TObjectIntHashMap<>();
   public final TIntObjectMap<Class<?>> registrationClasses = new TIntObjectHashMap<>();
   public final TIntObjectMap<PacketTrimmer<?>> classIDsToTrimmerMap = new TIntObjectHashMap<>();
   
   public NetClassList()
   {
   }

   public NetClassList(Class<?>... classes)
   {
      registerPacketClasses(classes);
   }

   public void registerPacketClass(Class<?> clazz)
   {
      registerPacketClass(clazz, null);
   }

   public void registerPacketClass(Class<?> clazz, PacketTrimmer<?> packetTrimmer)
   {
      classList.add(clazz);
      
      int id = ++uniquePacketID;
      registrationIDs.put(clazz, id);
      registrationClasses.put(id, clazz);
      classIDsToTrimmerMap.put(id, packetTrimmer);
   }

   public void registerPacketClasses(Class<?>... classes)
   {
      for (Class<?> clazz : classes)
      {
         registerPacketClass(clazz);
      }
   }

   public void registerPacketField(Class<?> type)
   {
      typeList.add(type);
   }

   public void registerPacketFields(Class<?>... types)
   {
      for (Class<?> type : types)
      {
         registerPacketField(type);
      }
   }

   public void getPacketClassList(ArrayList<Class<?>> listToPack)
   {
      listToPack.addAll(classList);
   }

   public ArrayList<Class<?>> getPacketClassList()
   {
      return classList;
   }

   public ArrayList<Class<?>> getPacketFieldList()
   {
      return typeList;
   }

   public void registerWithKryo(Kryo kryo)
   {
      kryo.addDefaultSerializer(Quaternion.class, QuaternionSerializer.class);
      kryo.addDefaultSerializer(Vector3D.class, Vector3DSerializer.class);
      kryo.addDefaultSerializer(Point3D.class, Point3DSerializer.class);
      kryo.addDefaultSerializer(IMUPacket.class, IMUPacketSerializer.class);
      kryo.addDefaultSerializer(SpatialVectorMessage.class, SpatialVectorMessageSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Object.class, IDLSequenceObjectSerializer.class);
      kryo.addDefaultSerializer(RecyclingArrayList.class, RecyclingArrayListPubSubSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Boolean.class, IDLSequenceBooleanSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Double.class, IDLSequenceDoubleSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Float.class, IDLSequenceFloatSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Integer.class, IDLSequenceIntegerSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Byte.class, IDLSequenceByteSerializer.class);
      kryo.addDefaultSerializer(IDLSequence.Long.class, IDLSequenceLongSerializer.class);

      for (Class<?> clazz : getPacketClassList())
      {
         kryo.register(clazz);
      }

      for (Class<?> type : getPacketFieldList())
      {
         kryo.register(type);
      }
   }
   
   public int getID(Class<?> clazz)
   {
      return registrationIDs.get(clazz);
   }
   
   public Class<?> getClass(int id)
   {
      return registrationClasses.get(id);
   }

   public PacketTrimmer<?> getPacketTrimmer(Class<?> clazz)
   {
      if (registrationIDs.containsKey(clazz))
      {
         int id = registrationIDs.get(clazz);
         return classIDsToTrimmerMap.get(id);
      }
      else
      {
         return null;
      }
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/TwistCalculator.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;

import org.apache.commons.lang3.mutable.MutableInt;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
import us.ihmc.mecano.tools.MultiBodySystemTools;

/**
 * This class is a tool that can be used to compute the twist of each {@code RigidBody} composing a
 * rigid-body system.
 * <p>
 * A new twist calculator can be constructed via the constructor
 * {@link #TwistCalculator(ReferenceFrame, RigidBodyBasics)}. Every time the system's state is changing,
 * the twist calculator can be notified via {@link #compute()}. Finally, the twist of any rigid-body
 * can be obtained as follows:
 * <ul>
 * <li>{@link #getTwistOfBody(RigidBodyBasics, Twist)} provides the twist of any rigid-body with respect
 * to the {@code inertialFrame}.
 * <li>{@link #getRelativeTwist(RigidBodyBasics, RigidBodyBasics, Twist)} provides the twist of any rigid-body
 * with respect to another rigid-body of the same system.
 * <li>{@link #getLinearVelocityOfBodyFixedPoint(RigidBodyBasics, FramePoint3D, FrameVector3D)} provides the
 * linear velocity of a point of a rigid-body with respect to the {@code inertialFrame}.
 * <li>{@link #getLinearVelocityOfBodyFixedPoint(RigidBodyBasics, RigidBodyBasics, FramePoint3D, FrameVector3D)}
 * provides the linear velocity of a point of a rigid-body with respect to another rigid-body of the
 * same system.
 * </ul>
 * </p>
 */
public class TwistCalculator
{
   /**
    * Non-moving frame with respect to which the twists are computed. Typically set to
    * {@link ReferenceFrame#getWorldFrame()}.
    */
   private final ReferenceFrame inertialFrame;
   /** The root body of the system for which this {@code TwistCalculator} is available. */
   private final RigidBodyBasics rootBody;
   /** Twist of the root body. */
   private final Twist rootTwist;

   /**
    * Internal storage of the twist of each body the system. This is the map from rigid-bodies to
    * indices to use with {@code rigidBodiesWithAssignedTwist} and {@code assignedTwists} for
    * retrieving their twist if already computed. If no twist has been computed yet, the index is
    * equal to {@code -1}.
    */
   private final HashMap<RigidBodyBasics, MutableInt> rigidBodyToAssignedTwistIndex = new HashMap<>();
   /**
    * List of the rigid-bodies with an up-to-date twist. This list allows a garbage free clearance
    * of the {@code rigidBodyToAssignedTwistIndex} map.
    */
   private final List<RigidBodyBasics> rigidBodiesWithAssignedTwist;
   /**
    * The list of up-to-date twists assigned to rigid-bodies. The association twist <-> rigid-body
    * can be retrieved using the map {@code rigidBodyToAssignedTwistIndex}.
    */
   private final List<Twist> assignedTwists;
   /**
    * List of out-of-date twists used to recycle memory.
    */
   private final List<Twist> unnassignedTwists = new ArrayList<>();

   /**
    * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies
    * of the system to which {@code body} belongs.
    * 
    * @param inertialFrame non-moving frame with respect to which the twists are computed. Typically
    *           {@link ReferenceFrame#getWorldFrame()} is used here.
    * @param body a body that belongs to the system this twist calculator will be available for.
    */
   public TwistCalculator(ReferenceFrame inertialFrame, RigidBodyBasics body)
   {
      this.inertialFrame = inertialFrame;
      this.rootBody = MultiBodySystemTools.getRootBody(body);
      this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame());

      int numberOfRigidBodies = MultiBodySystemTools.collectSubtreeSuccessors(MultiBodySystemTools.collectSubtreeJoints(rootBody)).length;
      while (unnassignedTwists.size() < numberOfRigidBodies)
         unnassignedTwists.add(new Twist());
      assignedTwists = new ArrayList<>(numberOfRigidBodies);
      rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies);

      assignedTwists.add(rootTwist);
      rigidBodiesWithAssignedTwist.add(rootBody);
      rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0));
   }

   /**
    * Notifies the system has changed state (configuration and velocity).
    * <p>
    * This method has to be called once every time the system state has changed, and before calling
    * the methods {@link #getTwistOfBody(RigidBodyBasics, Twist)} and
    * {@link #getRelativeTwist(RigidBodyBasics, RigidBodyBasics, Twist)}.
    * </p>
    */
   // TODO rename to reset
   public void compute()
   {
      while (rigidBodiesWithAssignedTwist.size() > 1)
         rigidBodyToAssignedTwistIndex.get(rigidBodiesWithAssignedTwist.remove(rigidBodiesWithAssignedTwist.size() - 1)).setValue(-1);

      while (assignedTwists.size() > 1)
         unnassignedTwists.add(assignedTwists.remove(assignedTwists.size() - 1));

      rigidBodyToAssignedTwistIndex.get(rootBody).setValue(0);
   }

   /**
    * Updates if necessary and packs the twist of the given {@code body}.
    * <p>
    * The result is the twist of the {@code body.getBodyFixedFrame()}, with respect to the
    * {@code inertialFrame}, expressed in the {@code body.getBodyFixedFrame()}.
    * </p>
    * <p>
    * WARNING: This method assumes that the internal memory of this {@code TwistCalculator} is
    * up-to-date. The user has to notify this calculator every time the system state has changed,
    * this is done through the method {@link #compute()}.
    * </p>
    * <p>
    * In the case the twist of the given {@code body} has been computed already no extra computation
    * is done. However, if there is no up-to-date twist for this rigid-body, it is then updated by a
    * recursive approach using the following relation: <br>
    * T<sup>b, b</sup><sub>i</sub> = T<sup>p, b</sup><sub>i</sub> + T<sup>b, b</sup><sub>p</sub>
    * </br>
    * where 'b' is the {@code body} frame, 'p' the predecessor frame, and 'i' the inertial frame.
    * <br>
    * Starting from the given {@code body}, its twist can be updated using the twist of the
    * predecessor to the parent joint. The twist of the predecessor is updated in the same manner.
    * This is done recursively until the predecessor has an up-to-date twist or is the root body.
    * </p>
    * 
    * @param body the rigid-body to get the twist of.
    * @param twistToPack the twist of the {@code body} to pack. Modified.
    */
   public void getTwistOfBody(RigidBodyBasics body, Twist twistToPack)
   {
      twistToPack.setIncludingFrame(computeOrGetTwistOfBody(body));
   }

   /**
    * Temporary twist used for intermediate garbage free operations. To use only in the method
    * {@link #getRelativeTwist(RigidBodyBasics, RigidBodyBasics, Twist)}.
    */
   private final Twist twistForGetRelativeTwist = new Twist();

   /**
    * Computes and packs the twist of the given {@code body} relative to the given {@code base}.
    * <p>
    * The resulting twist is the twist of the {@code body.getBodyFixedFrame()}, with respect to
    * {@code base.getBodyFixedFrame()}, expressed in {@code body.getBodyFixedFrame()}.
    * </p>
    * <p>
    * WARNING: This method assumes that the internal memory of this {@code TwistCalculator} is
    * up-to-date. The user has to notify this calculator every time the system state has changed,
    * this is done through the method {@link #compute()}.
    * </p>
    * <p>
    * The relative twist between the two rigid-bodies is calculated knowing their twists with
    * respect to the inertial frame using the method {@link #getTwistOfBody(RigidBodyBasics, Twist)}: <br>
    * T<sup>b2, b2</sup><sub>b1</sub> = T<sup>b2, b2</sup><sub>i</sub> - T<sup>b1,
    * b2</sup><sub>i</sub> </br>
    * with 'b1' being the {@code base}, 'b2' the {@code body}, and 'i' the {@code inertialFrame}.
    * </p>
    * 
    * @param base the rigid-body with respect to which the twist is to be computed.
    * @param body the rigid-body to compute the twist of.
    * @param twistToPack the twist of the {@code body} relative to the {@code base}. Modified.
    */
   public void getRelativeTwist(RigidBodyBasics base, RigidBodyBasics body, Twist twistToPack)
   {
      twistToPack.setIncludingFrame(computeOrGetTwistOfBody(body));
      twistForGetRelativeTwist.setIncludingFrame(computeOrGetTwistOfBody(base));
      twistForGetRelativeTwist.changeFrame(twistToPack.getReferenceFrame());
      twistToPack.sub(twistForGetRelativeTwist);
   }

   /**
    * Temporary twist used for intermediate garbage free operations. To use only in the method
    * {@link #getAngularVelocityOfBody(RigidBodyBasics, FrameVector3D)}.
    */
   private final Twist twistForGetAngularVelocityOfBody = new Twist();

   /**
    * Computes and packs the angular velocity of the given {@code body} with respect to
    * {@code inertialFrame}.
    * <p>
    * The result will be the angular velocity of {@code body.getBodyFixedFrame()} with respect to
    * {@code inertialFrame} expressed in {@code body.getBodyFixedFrame()}.
    * </p>
    * 
    * @param body the rigid-body to compute the angular velocity of.
    * @param angularVelocityToPack the angular velocity of the given {@code body}. Modified.
    */
   public void getAngularVelocityOfBody(RigidBodyBasics body, FrameVector3D angularVelocityToPack)
   {
      getTwistOfBody(body, twistForGetAngularVelocityOfBody);
      angularVelocityToPack.setIncludingFrame(twistForGetAngularVelocityOfBody.getAngularPart());
   }

   /**
    * Computes and packs the angular velocity of the given {@code body} with respect to the given
    * {@code base}.
    * <p>
    * The result will be the angular velocity of {@code body.getBodyFixedFrame()} with respect to
    * {@code base.getBodyFixedFrame()} expressed in {@code body.getBodyFixedFrame()}.
    * </p>
    * 
    * @param base the rigid-body with respect to which the angular velocity is to be computed.
    * @param body the rigid-body to compute the angular velocity of.
    * @param angularVelocityToPack the angular velocity of {@code body} relative to the
    *           {@code base}. Modified.
    */
   public void getRelativeAngularVelocity(RigidBodyBasics base, RigidBodyBasics body, FrameVector3D angularVelocityToPack)
   {
      getRelativeTwist(base, body, twistForGetAngularVelocityOfBody);
      angularVelocityToPack.setIncludingFrame(twistForGetAngularVelocityOfBody.getAngularPart());
   }

   /**
    * Temporary twist used for intermediate garbage free operations. To use only in the method
    * {@link #getLinearVelocityOfBodyFixedPoint(RigidBodyBasics, RigidBodyBasics, FramePoint3D, FrameVector3D)}.
    */
   private final Twist twistForGetLinearVelocityOfBodyFixedPoint = new Twist();
   /**
    * Temporary point used for intermediate garbage free operations. To use only in the method
    * {@link #getLinearVelocityOfBodyFixedPoint(RigidBodyBasics, RigidBodyBasics, FramePoint3D, FrameVector3D)}.
    */
   private final FramePoint3D pointForGetLinearVelocityOfBodyFixedPoint = new FramePoint3D();

   /**
    * Computes and packs the linear velocity of a point defined by {@code bodyFixedPoint} that is
    * fixed to the given {@code body}.
    * <p>
    * The result will be the linear velocity of the {@code bodyFixedPoint} with respect to the
    * {@code inertialFrame}. The vector is expressed in {@code inertialFrame}.
    * </p>
    * 
    * @param body the rigid-body to which {@code bodyFixedPoint} is attached to.
    * @param bodyFixedPoint the coordinates of the point attached to {@code body} that linear
    *           velocity is to be computed. Not modified.
    * @param linearVelocityToPack the linear velocity of the body fixed point with respect to the
    *           inertial frame. Modified.
    */
   public void getLinearVelocityOfBodyFixedPoint(RigidBodyBasics body, FramePoint3D bodyFixedPoint, FrameVector3D linearVelocityToPack)
   {
      FramePoint3D localPoint = pointForGetLinearVelocityOfBodyFixedPoint;
      Twist localTwist = twistForGetLinearVelocityOfBodyFixedPoint;

      getTwistOfBody(body, localTwist);
      ReferenceFrame baseFrame = localTwist.getBaseFrame();
      localPoint.setIncludingFrame(bodyFixedPoint);

      localTwist.changeFrame(baseFrame);
      localPoint.changeFrame(baseFrame);

      localTwist.getLinearVelocityAt(localPoint, linearVelocityToPack);
   }

   /**
    * Computes and packs the linear velocity of a point defined by {@code bodyFixedPoint} that is
    * fixed to the given {@code body} with respect to the given {@code base}.
    * <p>
    * The result will be the linear velocity of the {@code bodyFixedPoint} with respect to the
    * {@code base.getBodyFixedFrame()}. The vector is expressed in {@code base.getBodyFixedFrame()}.
    * </p>
    * 
    * @param base the rigid-body with respect to which the linear velocity is to be computed.
    * @param body the rigid-body to which {@code bodyFixedPoint} is attached to.
    * @param bodyFixedPoint the coordinates of the point attached to {@code body} that linear
    *           velocity is to be computed. Not modified.
    * @param linearVelocityToPack the linear velocity of the body fixed point with respect to the
    *           inertial frame. Modified.
    */
   public void getLinearVelocityOfBodyFixedPoint(RigidBodyBasics base, RigidBodyBasics body, FramePoint3D bodyFixedPoint, FrameVector3D linearVelocityToPack)
   {
      FramePoint3D localPoint = pointForGetLinearVelocityOfBodyFixedPoint;
      Twist localTwist = twistForGetLinearVelocityOfBodyFixedPoint;

      getRelativeTwist(base, body, localTwist);
      ReferenceFrame baseFrame = localTwist.getBaseFrame();
      localPoint.setIncludingFrame(bodyFixedPoint);

      localTwist.changeFrame(baseFrame);
      localPoint.changeFrame(baseFrame);

      localTwist.getLinearVelocityAt(localPoint, linearVelocityToPack);
   }

   /**
    * Returns the reference to the root body of the system for which this twist calculator is
    * available.
    * 
    * @return the root body.
    */
   public RigidBodyBasics getRootBody()
   {
      return rootBody;
   }

   /**
    * Gets the inertial frame: non-moving frame with respect to which the twists are computed.
    * Typically set to {@link ReferenceFrame#getWorldFrame()}.
    * 
    * @return the inertial frame used by this twist calculator.
    */
   public ReferenceFrame getInertialFrame()
   {
      return inertialFrame;
   }

   /**
    * Temporary twist used for intermediate garbage free operations. To use only in the method
    * {@link #computeOrGetTwistOfBody(RigidBodyBasics)}.
    */
   private final Twist twistForComputeOrGetTwistOfBody = new Twist();

   /**
    * Retrieves or computes the twist with respect to the inertial frame of the given {@code body}.
    * <p>
    * WARNING: The returned {@code Twist} object will remain associated with the given {@code body}
    * and <b> should NOT be modified </b>.
    * </p>
    * <p>
    * In the case the twist of the given {@code body} has been computed already no extra computation
    * is done. However, if there is no up-to-date twist for this rigid-body, it is then updated by a
    * recursive approach using the following relation: <br>
    * T<sup>b, b</sup><sub>i</sub> = T<sup>p, b</sup><sub>i</sub> + T<sup>b, b</sup><sub>p</sub>
    * </br>
    * where 'b' is the {@code body} frame, 'p' the predecessor frame, and 'i' the inertial frame.
    * <br>
    * Starting from the given {@code body}, its twist can be updated using the twist of the
    * predecessor to the parent joint. The twist of the predecessor is updated in the same manner.
    * This is done recursively until the predecessor has an up-to-date twist or is the root body.
    * </p>
    * 
    * @param body the rigid-body to get the twist of.
    * @return the twist of the rigid-body with respect to the inertial frame.
    */
   private TwistReadOnly computeOrGetTwistOfBody(RigidBodyBasics body)
   {
      Twist twist = retrieveAssignedTwist(body);

      if (twist == null)
      {
         /*
          * The body twist has not been computed yet. Going up toward the root until we get the
          * twist of a rigidBody that is up-to-date, and then going back step-by-step to this body
          * while updating twists.
          */
         ReferenceFrame bodyFrame = body.getBodyFixedFrame();
         JointBasics parentJoint = body.getParentJoint();
         RigidBodyBasics predecessor = parentJoint.getPredecessor();
         TwistReadOnly twistOfPredecessor = computeOrGetTwistOfBody(predecessor);
         twist = assignAndGetEmptyTwist(body);

         parentJoint.getSuccessorTwist(twistForComputeOrGetTwistOfBody);

         twist.setIncludingFrame(twistOfPredecessor);
         twist.changeFrame(bodyFrame);
         twist.add(twistForComputeOrGetTwistOfBody);
      }

      return twist;
   }

   /**
    * Retrieves in the internal memory, the up-to-date twist associated with the given {@code body}.
    * 
    * @param body the query.
    * @return the up-to-date twist of the given {@code body}, returns {@code null} if no twist could
    *         be found.
    */
   private Twist retrieveAssignedTwist(RigidBodyBasics body)
   {
      MutableInt mutableIndex = rigidBodyToAssignedTwistIndex.get(body);

      if (mutableIndex == null)
      {
         mutableIndex = new MutableInt(-1);
         rigidBodyToAssignedTwistIndex.put(body, mutableIndex);
      }

      int assignedTwistIndex = mutableIndex.intValue();
      if (assignedTwistIndex == -1)
         return null;
      else
         return assignedTwists.get(assignedTwistIndex);
   }

   /**
    * Assigned an empty twist to the given {@code body} and returns it.
    * 
    * @param body the rigid-body to assign a twist to.
    * @return the twist newly assigned to the rigid-body.
    */
   private Twist assignAndGetEmptyTwist(RigidBodyBasics body)
   {
      Twist newAssignedTwist;

      if (unnassignedTwists.isEmpty())
         newAssignedTwist = new Twist();
      else
         newAssignedTwist = unnassignedTwists.remove(unnassignedTwists.size() - 1);

      MutableInt mutableIndex = rigidBodyToAssignedTwistIndex.get(body);

      if (mutableIndex == null)
      {
         mutableIndex = new MutableInt();
         rigidBodyToAssignedTwistIndex.put(body, mutableIndex);
      }

      mutableIndex.setValue(assignedTwists.size());

      rigidBodiesWithAssignedTwist.add(body);
      assignedTwists.add(newAssignedTwist);
      return newAssignedTwist;
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/overheadPath/TurnInPlaceFootstepOverheadPath.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath;

import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.FootstepOverheadPath;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.InterpolatedFootstepOverheadPath;
import us.ihmc.robotics.geometry.AngleTools;

/**
 * Created by agrabertilton on 3/6/15.
 */
public class TurnInPlaceFootstepOverheadPath extends FootstepOverheadPath
{
   FramePose2D startPose;
   FramePose2D endPose;
   double turnToStraightEquivalence;
   double distance;
   double angleChange;

   public TurnInPlaceFootstepOverheadPath(FramePose2D startPose, FramePose2D endPose){
      this(startPose, endPose, 1.0);
   }

   public TurnInPlaceFootstepOverheadPath(FramePose2D startPose, FramePose2D endPose, double turnToStraightEquivalence)
   {
      this.turnToStraightEquivalence = turnToStraightEquivalence;
      startPose.checkReferenceFrameMatch(endPose);
      this.startPose = new FramePose2D(startPose);
      this.endPose = new FramePose2D(endPose);
      double deltaX = endPose.getX() - startPose.getX();
      double deltaY = endPose.getY() - startPose.getY();
      double straightDistance = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
      if (straightDistance > 1e-8){
         throw new RuntimeException(this.getClass().getSimpleName() + ": Not a pure turn");
      }
      angleChange = AngleTools.trimAngleMinusPiToPi(endPose.getYaw() - startPose.getYaw());
      distance = Math.abs(angleChange) * turnToStraightEquivalence;
   }

   @Override
   public FramePose2D getPoseAtDistance(double distanceAlongPath)
   {
      double interpolationValue = distanceAlongPath / distance;
      interpolationValue = Math.max(interpolationValue, 0.0);
      interpolationValue = Math.min(interpolationValue, 1.0);
      double x = (1-interpolationValue) * startPose.getX() + (interpolationValue) * endPose.getX();
      double y = (1-interpolationValue) * startPose.getY() + (interpolationValue) * endPose.getY();
      double yaw = AngleTools.trimAngleMinusPiToPi(startPose.getYaw() + (interpolationValue) * angleChange);
      return new FramePose2D(startPose.getReferenceFrame(), new Point2D(x,y), yaw);
   }

   @Override
   public double getTotalDistance()
   {
      return distance;
   }

   @Override
   public FootstepOverheadPath changeFrameCopy(ReferenceFrame desiredFrame)
   {
      FramePose2D newStartPose = new FramePose2D(startPose);
      newStartPose.changeFrame(desiredFrame);
      FramePose2D newEndPose = new FramePose2D(endPose);
      newEndPose.changeFrame(desiredFrame);
      return new InterpolatedFootstepOverheadPath(newStartPose, newEndPose);
   }

   @Override
   public ReferenceFrame getReferenceFrame()
   {
      return startPose.getReferenceFrame();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LocalizationPointMapPacket.java<|end_filename|>
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;

/**
       * Message part of the localization module
       */
public class LocalizationPointMapPacket extends Packet<LocalizationPointMapPacket> implements Settable<LocalizationPointMapPacket>, EpsilonComparable<LocalizationPointMapPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public long timestamp_;
   public us.ihmc.idl.IDLSequence.Float  localization_point_map_;

   public LocalizationPointMapPacket()
   {
      localization_point_map_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5");

   }

   public LocalizationPointMapPacket(LocalizationPointMapPacket other)
   {
      this();
      set(other);
   }

   public void set(LocalizationPointMapPacket other)
   {
      sequence_id_ = other.sequence_id_;

      timestamp_ = other.timestamp_;

      localization_point_map_.set(other.localization_point_map_);
   }

   /**
            * 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 void setTimestamp(long timestamp)
   {
      timestamp_ = timestamp;
   }
   public long getTimestamp()
   {
      return timestamp_;
   }


   public us.ihmc.idl.IDLSequence.Float  getLocalizationPointMap()
   {
      return localization_point_map_;
   }


   public static Supplier<LocalizationPointMapPacketPubSubType> getPubSubType()
   {
      return LocalizationPointMapPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return LocalizationPointMapPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(LocalizationPointMapPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.timestamp_, other.timestamp_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.localization_point_map_, other.localization_point_map_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof LocalizationPointMapPacket)) return false;

      LocalizationPointMapPacket otherMyClass = (LocalizationPointMapPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.timestamp_ != otherMyClass.timestamp_) return false;

      if (!this.localization_point_map_.equals(otherMyClass.localization_point_map_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("LocalizationPointMapPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("timestamp=");
      builder.append(this.timestamp_);      builder.append(", ");
      builder.append("localization_point_map=");
      builder.append(this.localization_point_map_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/visibilityGraphs/interfaces/ObstacleRegionFilter.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.interfaces;

import us.ihmc.robotics.geometry.PlanarRegion;

public interface ObstacleRegionFilter
{
   /**
    * Whether the {@code query} is an actual obstacle when navigating the {@code navigableRegion}.
    * 
    * @param query the potential obstacle region.
    * @param navigableRegion the region in which the {@code query} might be an obstacle.
    * @return {@code true} if the {@code query} is a legit obstacle, {@code false} if it can be
    *         ignored.
    */
   boolean isRegionValidObstacle(PlanarRegion query, PlanarRegion navigableRegion);
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/offline/ParameterTunerOffline.java<|end_filename|>
package us.ihmc.parameterTuner.offline;

import us.ihmc.parameterTuner.guiElements.main.ParameterGuiInterface;
import us.ihmc.parameterTuner.guiElements.main.ParameterTuningApplication;

public class ParameterTunerOffline extends ParameterTuningApplication
{
   @Override
   protected ParameterGuiInterface createInputManager()
   {
      return new FileInputManager();
   }

   public static void main(String[] args)
   {
      launch(args);
   }
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/parameters/ValkyrieCollisionBoxProvider.java<|end_filename|>
package us.ihmc.valkyrie.parameters;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import us.ihmc.euclid.axisAngle.AxisAngle;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.ihmcPerception.depthData.CollisionBoxProvider;
import us.ihmc.ihmcPerception.depthData.collisionShapes.CollisionBox;
import us.ihmc.ihmcPerception.depthData.collisionShapes.CollisionCylinder;
import us.ihmc.ihmcPerception.depthData.collisionShapes.CollisionShape;
import us.ihmc.ihmcPerception.depthData.collisionShapes.CollisionSphere;
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.robotics.partNames.ArmJointName;
import us.ihmc.robotics.partNames.LegJointName;
import us.ihmc.robotics.robotSide.RobotSide;

/**
 * There is no collision provided in Valkyrie's SDF/URDF models, so it is created and hardcoded here.
 *
 */
public class ValkyrieCollisionBoxProvider implements CollisionBoxProvider
{
   private final Map<String, List<CollisionShape>> collisions = new HashMap<>();

   public ValkyrieCollisionBoxProvider(FullHumanoidRobotModel robotModel)
   {
      collisions.put(robotModel.getRootJoint().getName(), new ArrayList<>());
      for (OneDoFJointBasics joint : robotModel.getOneDoFJoints())
         collisions.put(joint.getName(), new ArrayList<>());

      for (RobotSide robotSide : RobotSide.values)
      {
         { // Shoulder roll
            OneDoFJointBasics shoulderRollJoint = robotModel.getArmJoint(robotSide, ArmJointName.SHOULDER_ROLL);
            if (shoulderRollJoint != null)
            {
               String jointName = shoulderRollJoint.getName();

               { // Shoulder
                  AxisAngle rotation = new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0);
                  Vector3D translation = new Vector3D(-0.01, 0.0, 0.0);
                  RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
                  collisions.get(jointName).add(new CollisionCylinder(pose, 0.09, 1.3 * 0.23));
               }
               { // Upper-arm
                  AxisAngle rotation = new AxisAngle(1.0, 0.0, 0.0, Math.PI / 2.0);
                  Vector3D translation = new Vector3D(0.0, robotSide.negateIfRightSide(0.16), 0.0);
                  RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
                  collisions.get(jointName).add(new CollisionCylinder(pose, 0.09, 0.32));
               }
            }
         }

         { // Elbow
            OneDoFJointBasics elbowPitchJoint = robotModel.getArmJoint(robotSide, ArmJointName.ELBOW_PITCH);

            if (elbowPitchJoint != null)
            {
               String jointName = elbowPitchJoint.getName();

               { // Elbow
                  AxisAngle rotation = new AxisAngle();
                  Vector3D translation = new Vector3D(0.0, 0.0, 0.0);
                  RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
                  collisions.get(jointName).add(new CollisionCylinder(pose, 0.06, 0.15));
               }

               { // Forearm
                  AxisAngle rotation = new AxisAngle(1.0, 0.0, 0.0, Math.PI / 2.0);
                  Vector3D translation = new Vector3D(-0.02, robotSide.negateIfRightSide(0.14), 0.0);
                  RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
                  collisions.get(jointName).add(new CollisionCylinder(pose, 0.09, 0.28));
               }
            }
         }

         { // Wrist
            RigidBodyBasics hand = robotModel.getHand(robotSide);
            if (hand != null)
            {
               String jointName = hand.getParentJoint().getName();

               { // Conservative sphere to wrap the hand plus fingers. Can be improved
                  AxisAngle rotation = new AxisAngle();
                  Vector3D translation = new Vector3D(0.0, robotSide.negateIfRightSide(0.07), 0.02);
                  RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
                  collisions.get(jointName).add(new CollisionSphere(pose, 1.3 * 0.12));
               }
            }
         }

         { // Hip yaw
            String jointName = robotModel.getLegJoint(robotSide, LegJointName.HIP_YAW).getName();

            { // The coconut shells
               AxisAngle rotation = new AxisAngle();
               Vector3D translation = new Vector3D(0.0, robotSide.negateIfRightSide(0.00), -0.03);
               RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
               collisions.get(jointName).add(new CollisionSphere(pose, 0.18));
            }
         }

         { // Hip pitch
            String jointName = robotModel.getLegJoint(robotSide, LegJointName.HIP_PITCH).getName();

            { // Thigh
               AxisAngle rotation = new AxisAngle();
               Vector3D translation = new Vector3D(0.02, robotSide.negateIfRightSide(0.08), -0.20);
               RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
               collisions.get(jointName).add(new CollisionBox(pose, 0.13, 0.1, 0.29));
            }
         }

         { // Knee
            String jointName = robotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH).getName();

            { // Shin
               AxisAngle rotation = new AxisAngle();
               Vector3D translation = new Vector3D(-0.01, robotSide.negateIfRightSide(0.0), -0.18);
               RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
               collisions.get(jointName).add(new CollisionBox(pose, 0.12, 0.1, 0.20));
            }
         }

         { // Ankle
            String jointName = robotModel.getFoot(robotSide).getParentJoint().getName();

            { // Foot
               AxisAngle rotation = new AxisAngle();
               Vector3D translation = new Vector3D(0.05, robotSide.negateIfRightSide(0.0), -0.04);
               RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
               collisions.get(jointName).add(new CollisionBox(pose, 0.16, 0.09, 0.06));
            }
         }
      }

      { // Head
         String jointName = robotModel.getHead().getParentJoint().getName();

         { // Head
            AxisAngle rotation = new AxisAngle();
            Vector3D translation = new Vector3D(0.08, 0.0, 0.0);
            RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
            collisions.get(jointName).add(new CollisionSphere(pose, 0.18));
         }
      }

      { // Torso
         String jointName = robotModel.getChest().getParentJoint().getName();

         { // Torso
            AxisAngle rotation = new AxisAngle();
            Vector3D translation = new Vector3D(-0.08, 0.0, 0.2);
            RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
            collisions.get(jointName).add(new CollisionBox(pose, 1.3 * 0.23, 1.3 * 0.2, 0.22));
         }

         { // Cylinder to remove the ropes from the hoist.
            AxisAngle rotation = new AxisAngle();
            Vector3D translation = new Vector3D(-0.05, 0.0, 0.54);
            RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
            collisions.get(jointName).add(new CollisionCylinder(pose, 0.22, 0.60));
         }
      }

      { // Pelvis
         String jointName = robotModel.getPelvis().getParentJoint().getName();

         { // Pelvis pt1: The upper part
            AxisAngle rotation = new AxisAngle();
            Vector3D translation = new Vector3D(0.0, 0.0, -0.08);
            RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
            collisions.get(jointName).add(new CollisionBox(pose, 1.3 * 0.13, 0.18, 0.08));
         }

         { // Pelvis pt2: The crotch part
            AxisAngle rotation = new AxisAngle();
            Vector3D translation = new Vector3D(0.0, 0.0, -0.25);
            RigidBodyTransform pose = new RigidBodyTransform(rotation, translation);
            collisions.get(jointName).add(new CollisionBox(pose, 0.17, 0.05, 0.09));
         }
      }
   }

   @Override
   public List<CollisionShape> getCollisionMesh(String jointName)
   {
      return collisions.get(jointName);
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/skippy/SkippyFallingChecker.java<|end_filename|>
package us.ihmc.exampleSimulations.skippy;

import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.controllers.ControllerFailureListener;
import us.ihmc.simulationConstructionSetTools.robotController.SimpleRobotController;

public class SkippyFallingChecker extends SimpleRobotController
{
   private final ControllerFailureListener controllerFailureListener;
   private final SkippyRobot skippyRobot;

   public SkippyFallingChecker(ControllerFailureListener controllerFailureListener, SkippyRobot skippyRobot)
   {
      this.controllerFailureListener = controllerFailureListener;
      this.skippyRobot = skippyRobot;
   }

   @Override
   public void doControl()
   {
      Point3D footLocation = skippyRobot.computeFootLocation();
      Point3D com = new Point3D();
      skippyRobot.computeCenterOfMass(com);

      boolean skippyFalling = com.getZ() < footLocation.getZ() + 0.1;
      if (skippyFalling)
         controllerFailureListener.controllerFailed(null);
   }
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/logger/util/FileSelectionDialog.java<|end_filename|>
package us.ihmc.robotDataVisualizer.logger.util;

import java.awt.FileDialog;
import java.awt.Frame;
import java.io.File;
import java.io.FilenameFilter;

public class FileSelectionDialog
{
   public static File loadDirectoryWithFileNamed(final String acceptedFileName)
   {
      final FileDialog fileDialog = new FileDialog((Frame) null, "Choose logging directory");
      fileDialog.setMode(FileDialog.LOAD);
      
      if(acceptedFileName != null)
      {
         fileDialog.setFilenameFilter(new FilenameFilter()
         {
            
            @Override
            public boolean accept(File dir, String name)
            {
               return (acceptedFileName.equals(name));
            }
         });        
      }
      
      fileDialog.setVisible(true);         
      String filename = fileDialog.getDirectory();
      fileDialog.dispose();
      if(filename != null)
      {
         return new File(filename);
      }
      else
      {
         return null;
      }

   }

   public static File loadFile()
   {
      return loadFile(null);
   }
   
   public static File loadFile(final String acceptedFileName)
   {
      final FileDialog fileDialog = new FileDialog((Frame) null, "Choose logging directory");
      fileDialog.setMode(FileDialog.LOAD);
      
      if(acceptedFileName != null)
      {
         fileDialog.setFilenameFilter(new FilenameFilter()
         {
            
            @Override
            public boolean accept(File dir, String name)
            {
               return (acceptedFileName.equals(name));
            }
         });        
      }
      
      fileDialog.setVisible(true);         
      String filename = fileDialog.getDirectory() + fileDialog.getFile();
      fileDialog.dispose();
      if(filename != null)
      {
         return new File(filename);
      }
      else
      {
         return null;
      }
      
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/RobotPhysicalProperties.java<|end_filename|>
package us.ihmc.avatar.drcRobot;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.robotics.robotSide.SideDependentList;

public interface RobotPhysicalProperties
{
   SideDependentList<RigidBodyTransform> getHandAttachmentPlateToWristTransforms();

   SideDependentList<RigidBodyTransform> getSoleToAnkleFrameTransforms();


   double getThighLength();

   double getShinLength();


   double getActualFootLength();

   double getActualFootWidth();


   double getFootForwardForControl();

   double getFootBackForControl();

   double getFootLengthForControl();

   double getToeWidthForControl();

   double getFootWidthForControl();
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/dataExporter/DataExporterReadmeWriter.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.dataExporter;

import java.awt.Dimension;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;

import javax.swing.JOptionPane;
import javax.swing.JScrollPane;
import javax.swing.JTextArea;

import us.ihmc.commons.PrintTools;

public class DataExporterReadmeWriter
{
   public void writeReadMe(File directory, String name, long revisionNumber)
   {
      try
      {
//         String revisionNumberLine = "Revision number: " + revisionNumber + "\n\n";
//         String readmeText = revisionNumberLine + getUserInputReadMeText();
         String readmeText = getUserInputReadMeText();

         File file = new File(directory, name + ".txt");
         FileWriter out = new FileWriter(file);

         out.write(readmeText);
         out.close();
      }
      catch (IOException ex)
      {
         PrintTools.error(this, "IOException in DataExporterReadmeWriter. " + ex.getMessage());
      }
   }

   private String getUserInputReadMeText()
   {
      JTextArea message = new JTextArea();
      JScrollPane scrollPane = new JScrollPane(message);
      scrollPane.setPreferredSize(new Dimension(350, 150));
      int option = JOptionPane.showConfirmDialog(null, scrollPane, "ReadMe Text", JOptionPane.OK_CANCEL_OPTION, JOptionPane.PLAIN_MESSAGE);
      String readme = "";
      if (option == 0)
         readme = message.getText();

      return readme;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BoundingBoxesPacket.java<|end_filename|>
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;

public class BoundingBoxesPacket extends Packet<BoundingBoxesPacket> implements Settable<BoundingBoxesPacket>, EpsilonComparable<BoundingBoxesPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.Integer  bounding_boxes_x_coordinates_;
   public us.ihmc.idl.IDLSequence.Integer  bounding_boxes_y_coordinates_;
   public us.ihmc.idl.IDLSequence.Integer  bounding_boxes_widths_;
   public us.ihmc.idl.IDLSequence.Integer  bounding_boxes_heights_;
   public us.ihmc.idl.IDLSequence.StringBuilderHolder  labels_;

   public BoundingBoxesPacket()
   {
      bounding_boxes_x_coordinates_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_2");

      bounding_boxes_y_coordinates_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_2");

      bounding_boxes_widths_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_2");

      bounding_boxes_heights_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_2");

      labels_ = new us.ihmc.idl.IDLSequence.StringBuilderHolder (100, "type_d");
   }

   public BoundingBoxesPacket(BoundingBoxesPacket other)
   {
      this();
      set(other);
   }

   public void set(BoundingBoxesPacket other)
   {
      sequence_id_ = other.sequence_id_;

      bounding_boxes_x_coordinates_.set(other.bounding_boxes_x_coordinates_);
      bounding_boxes_y_coordinates_.set(other.bounding_boxes_y_coordinates_);
      bounding_boxes_widths_.set(other.bounding_boxes_widths_);
      bounding_boxes_heights_.set(other.bounding_boxes_heights_);
      labels_.set(other.labels_);
   }

   /**
            * 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 us.ihmc.idl.IDLSequence.Integer  getBoundingBoxesXCoordinates()
   {
      return bounding_boxes_x_coordinates_;
   }


   public us.ihmc.idl.IDLSequence.Integer  getBoundingBoxesYCoordinates()
   {
      return bounding_boxes_y_coordinates_;
   }


   public us.ihmc.idl.IDLSequence.Integer  getBoundingBoxesWidths()
   {
      return bounding_boxes_widths_;
   }


   public us.ihmc.idl.IDLSequence.Integer  getBoundingBoxesHeights()
   {
      return bounding_boxes_heights_;
   }


   public us.ihmc.idl.IDLSequence.StringBuilderHolder  getLabels()
   {
      return labels_;
   }


   public static Supplier<BoundingBoxesPacketPubSubType> getPubSubType()
   {
      return BoundingBoxesPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return BoundingBoxesPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(BoundingBoxesPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsIntegerSequence(this.bounding_boxes_x_coordinates_, other.bounding_boxes_x_coordinates_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsIntegerSequence(this.bounding_boxes_y_coordinates_, other.bounding_boxes_y_coordinates_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsIntegerSequence(this.bounding_boxes_widths_, other.bounding_boxes_widths_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsIntegerSequence(this.bounding_boxes_heights_, other.bounding_boxes_heights_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilderSequence(this.labels_, other.labels_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof BoundingBoxesPacket)) return false;

      BoundingBoxesPacket otherMyClass = (BoundingBoxesPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.bounding_boxes_x_coordinates_.equals(otherMyClass.bounding_boxes_x_coordinates_)) return false;
      if (!this.bounding_boxes_y_coordinates_.equals(otherMyClass.bounding_boxes_y_coordinates_)) return false;
      if (!this.bounding_boxes_widths_.equals(otherMyClass.bounding_boxes_widths_)) return false;
      if (!this.bounding_boxes_heights_.equals(otherMyClass.bounding_boxes_heights_)) return false;
      if (!this.labels_.equals(otherMyClass.labels_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("BoundingBoxesPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("bounding_boxes_x_coordinates=");
      builder.append(this.bounding_boxes_x_coordinates_);      builder.append(", ");
      builder.append("bounding_boxes_y_coordinates=");
      builder.append(this.bounding_boxes_y_coordinates_);      builder.append(", ");
      builder.append("bounding_boxes_widths=");
      builder.append(this.bounding_boxes_widths_);      builder.append(", ");
      builder.append("bounding_boxes_heights=");
      builder.append(this.bounding_boxes_heights_);      builder.append(", ");
      builder.append("labels=");
      builder.append(this.labels_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ChestHybridJointspaceTaskspaceTrajectoryMessage.java<|end_filename|>
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 commands the controller to move the chest in both taskspace and jointspace
       * to the desired orientation and joint angles while going through the specified trajectory points.
       */
public class ChestHybridJointspaceTaskspaceTrajectoryMessage extends Packet<ChestHybridJointspaceTaskspaceTrajectoryMessage> implements Settable<ChestHybridJointspaceTaskspaceTrajectoryMessage>, EpsilonComparable<ChestHybridJointspaceTaskspaceTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * The taskspace trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage taskspace_trajectory_message_;
   /**
            * The jointspace trajectory information.
            * The indexing for the joints goes increasingly from the joint the closest to the pelvis to the joint the closest to the chest.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage jointspace_trajectory_message_;

   public ChestHybridJointspaceTaskspaceTrajectoryMessage()
   {
      taskspace_trajectory_message_ = new controller_msgs.msg.dds.SO3TrajectoryMessage();
      jointspace_trajectory_message_ = new controller_msgs.msg.dds.JointspaceTrajectoryMessage();
   }

   public ChestHybridJointspaceTaskspaceTrajectoryMessage(ChestHybridJointspaceTaskspaceTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(ChestHybridJointspaceTaskspaceTrajectoryMessage other)
   {
      sequence_id_ = other.sequence_id_;

      controller_msgs.msg.dds.SO3TrajectoryMessagePubSubType.staticCopy(other.taskspace_trajectory_message_, taskspace_trajectory_message_);
      controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.staticCopy(other.jointspace_trajectory_message_, jointspace_trajectory_message_);
   }

   /**
            * 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_;
   }


   /**
            * The taskspace trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage getTaskspaceTrajectoryMessage()
   {
      return taskspace_trajectory_message_;
   }


   /**
            * The jointspace trajectory information.
            * The indexing for the joints goes increasingly from the joint the closest to the pelvis to the joint the closest to the chest.
            */
   public controller_msgs.msg.dds.JointspaceTrajectoryMessage getJointspaceTrajectoryMessage()
   {
      return jointspace_trajectory_message_;
   }


   public static Supplier<ChestHybridJointspaceTaskspaceTrajectoryMessagePubSubType> getPubSubType()
   {
      return ChestHybridJointspaceTaskspaceTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ChestHybridJointspaceTaskspaceTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ChestHybridJointspaceTaskspaceTrajectoryMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.taskspace_trajectory_message_.epsilonEquals(other.taskspace_trajectory_message_, epsilon)) return false;
      if (!this.jointspace_trajectory_message_.epsilonEquals(other.jointspace_trajectory_message_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ChestHybridJointspaceTaskspaceTrajectoryMessage)) return false;

      ChestHybridJointspaceTaskspaceTrajectoryMessage otherMyClass = (ChestHybridJointspaceTaskspaceTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.taskspace_trajectory_message_.equals(otherMyClass.taskspace_trajectory_message_)) return false;
      if (!this.jointspace_trajectory_message_.equals(otherMyClass.jointspace_trajectory_message_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ChestHybridJointspaceTaskspaceTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("taskspace_trajectory_message=");
      builder.append(this.taskspace_trajectory_message_);      builder.append(", ");
      builder.append("jointspace_trajectory_message=");
      builder.append(this.jointspace_trajectory_message_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>robotiq-hand-drivers/src/main/java/us/ihmc/robotiq/RobotiqHandTesting.java<|end_filename|>
package us.ihmc.robotiq;

import java.awt.GridBagConstraints;
import java.awt.GridBagLayout;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import javax.swing.JButton;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.JSlider;
import javax.swing.event.ChangeEvent;
import javax.swing.event.ChangeListener;

import controller_msgs.msg.dds.HandDesiredConfigurationMessage;
import controller_msgs.msg.dds.ManualHandControlPacket;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HandConfiguration;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotiq.control.RobotiqHandCommandManager;

public class RobotiqHandTesting
{
	private final JFrame frame = new JFrame();
	private final JPanel panel = new JPanel(new GridBagLayout());
	private final GridBagConstraints c = new GridBagConstraints();
	
	private final SliderWithTwoButtonsPanel finger1Panel = new SliderWithTwoButtonsPanel("Index Finger");
	private final SliderWithTwoButtonsPanel finger2Panel = new SliderWithTwoButtonsPanel("Middle Finger");
	private final SliderWithTwoButtonsPanel finger3Panel = new SliderWithTwoButtonsPanel("Thumb");
	
	private final JPanel buttonPanel = new JPanel(new GridBagLayout());
	private final JButton initializeButton = new JButton("Initialize");
	private final JButton resetButton = new JButton("Reset");
	
	private RobotiqHandCommandManager manager;

	public static void main(String[] args) throws Exception
	{
		
		new RobotiqHandTesting().init();
		
	}
	
	private void init()
	{
		manager = new RobotiqHandCommandManager(RobotSide.RIGHT);

		//		setupSliderListeners();
		
		setupButtonListeners();
		
		setupUI();
	}
	
	private void setupUI()
	{
		frame.add(panel);
		
		c.gridx = 0;
		c.gridy = 0;
		
		panel.add(finger1Panel, c);
		
		c.gridy++;
		panel.add(finger2Panel, c);
		
		c.gridy++;
		panel.add(finger3Panel, c);
		
		c.gridy++;
		panel.add(buttonPanel, c);
		
		c.gridx = 0;
		c.gridy = 0;
		buttonPanel.add(initializeButton, c);
		
		c.gridx++;
		buttonPanel.add(resetButton, c);
		
		frame.pack();
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.setVisible(true);
	}
	
	private void setupSliderListeners()
	{
		finger1Panel.attachSliderActionListener(new ChangeListener()
		{
			@Override
			public void stateChanged(ChangeEvent e)
			{
				JSlider slider = (JSlider) e.getSource();
				System.out.println(slider.getValue());
			}
		});
		
		finger2Panel.attachSliderActionListener(new ChangeListener()
		{
			@Override
			public void stateChanged(ChangeEvent e)
			{
				JSlider slider = (JSlider) e.getSource();
				System.out.println(slider.getValue());
			}
		});
		
		finger3Panel.attachSliderActionListener(new ChangeListener()
		{
			@Override
			public void stateChanged(ChangeEvent e)
			{
				JSlider slider = (JSlider) e.getSource();
				System.out.println(slider.getValue());
			}
		});
	}
	
	private void setupButtonListeners()
	{
		finger1Panel.attachTopButtonActionListener(new ActionListener()
		{
			@Override
			public void actionPerformed(ActionEvent e)
			{
				JSlider slider = finger1Panel.getSlider();
				System.out.println(slider.getValue());
				ManualHandControlPacket packet = HumanoidMessageTools.createManualHandControlPacket(RobotSide.LEFT, finger1Panel.getSlider().getValue(), finger2Panel.getSlider().getValue(), finger3Panel.getSlider().getValue(), 0, 1);
				manager.sendHandCommand(packet);
			}
		});
		
		finger1Panel.attachBottomButtonActionListener(new ResetSliderActionListener(finger1Panel.getSlider()));
		
		finger2Panel.attachTopButtonActionListener(new ActionListener()
		{
			@Override
			public void actionPerformed(ActionEvent e)
			{
				
			}
		});
		
		finger2Panel.attachBottomButtonActionListener(new ResetSliderActionListener(finger2Panel.getSlider()));
		
		finger3Panel.attachTopButtonActionListener(new ActionListener()
		{
			@Override
			public void actionPerformed(ActionEvent e)
			{
				
			}
		});
		
		finger3Panel.attachBottomButtonActionListener(new ResetSliderActionListener(finger3Panel.getSlider()));
		
		initializeButton.addActionListener(new ActionListener()
		{
			@Override
			public void actionPerformed(ActionEvent e)
			{
				manager.sendHandCommand(HumanoidMessageTools.createHandDesiredConfigurationMessage(RobotSide.LEFT, HandConfiguration.CALIBRATE));
			}
		});
		
		resetButton.addActionListener(new ActionListener()
		{
			@Override
			public void actionPerformed(ActionEvent e)
			{
				manager.sendHandCommand(HumanoidMessageTools.createHandDesiredConfigurationMessage(RobotSide.LEFT, HandConfiguration.RESET));
			}
		});
	}
	
	class ResetSliderActionListener implements ActionListener
	{
		private final JSlider slider;
		
		public ResetSliderActionListener(JSlider slider)
		{
			this.slider = slider;
		}
		
		@Override
		public void actionPerformed(ActionEvent e)
		{
			slider.setValue(0);
		}
	}
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/fingers/ValkyrieHandModel.java<|end_filename|>
package us.ihmc.valkyrie.fingers;

import us.ihmc.avatar.handControl.packetsAndConsumers.HandModel;

public class ValkyrieHandModel implements HandModel
{
   @Override
   public ValkyrieHandJointName[] getHandJointNames()
   {
      return ValkyrieHandJointName.values;
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/RemoteIPRegistryServer.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client;

import java.rmi.RemoteException;
import java.rmi.server.UnicastRemoteObject;

import us.ihmc.simulationConstructionSetTools.simulationDispatcher.interfaces.RemoteIPRegistryInterface;

@SuppressWarnings("serial")
public class RemoteIPRegistryServer extends UnicastRemoteObject implements RemoteIPRegistryInterface
{
   private DispatchHostList hostList;
   private String pwd;

   public RemoteIPRegistryServer(DispatchHostList hostList, String pwd) throws RemoteException
   {
      this.pwd = <PASSWORD>;
      this.hostList = hostList;
   }

   @Override
   public void registerMe(String hostName, String pwd) throws RemoteException
   {
      if (!pwd.equals(this.pwd))
      {
         System.out.println("Invalid Password! : " + pwd);

         throw new RemoteException("Invalid Password!");
      }

      hostList.registerMe(hostName);
   }

   @Override
   public String[] getAllRegistered(String pwd) throws RemoteException
   {
      if (!pwd.equals(this.pwd))
         throw new RemoteException("Invalid Password!");

      return hostList.getAllRegistered();
   }

}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/controllers/pidGains/YoPIDSE3Gains.java<|end_filename|>
package us.ihmc.robotics.controllers.pidGains;

/**
 * An extension of the {@link PIDSE3Gains} interface that provides additional access
 * to YoVariables in the implementation.
 */
public interface YoPIDSE3Gains extends PIDSE3Gains
{
   @Override
   public abstract YoPID3DGains getPositionGains();

   @Override
   public abstract YoPID3DGains getOrientationGains();
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/KryoStreamDeSerializer.java<|end_filename|>
package us.ihmc.communication.net;

import java.io.IOException;
import java.io.InputStream;

import com.esotericsoftware.kryo.Kryo;
import com.esotericsoftware.kryo.io.Input;

/**
 * Serialize objects, write length and object to stream and read objects from stream
 * 
 * Not thread safe
 * 
 * @author <NAME>
 *
 */
public class KryoStreamDeSerializer
{

   private final Kryo kryo;

   private final byte[] readBuffer;
   private final Input input;
   
   
   public KryoStreamDeSerializer(int inputBufferSize)
   {
      kryo = new Kryo();
      kryo.setReferences(false);
      kryo.setRegistrationRequired(true);
      
      readBuffer = new byte[inputBufferSize];

      input = new Input();
      
      input.setBuffer(readBuffer);
   }
   
   /**
    * Register class with Kryo te serialize
    * 
    * @param clazz
    */
   public void registerClass(Class<?> clazz)
   {
      kryo.register(clazz);
   }
   
   /**
    * Register classes using a netclasslist
    */
   public void registerClasses(NetClassList netClassList)
   {
      netClassList.registerWithKryo(kryo);
   }

   /**
    * Blocking read for the next object
    * 
    * @param inputStream to read from
    * @return next object
    * @throws IOException
    */
   public Object read(InputStream inputStream) throws IOException
   {
      int length = readInt(inputStream);
      final int readLength = inputStream.read(readBuffer, 0, length);
      if(readLength != length)
      {
         throw new IOException("Cannot read whole object, expected " + length + " bytes, got " + readLength + " bytes");
      }
      input.setBuffer(readBuffer, 0, length);
      return kryo.readClassAndObject(input);
   }

   private int readInt(InputStream inputStream) throws IOException
   {
      final int readLength = inputStream.read(readBuffer, 0, 4);
      if (readLength != 4)
      {
         throw new IOException("Cannot read length of object");
      }
      int length = (readBuffer[0] & 0xFF) << 24 | (readBuffer[1] & 0xFF) << 16 | (readBuffer[2] & 0xFF) << 8 | (readBuffer[3] & 0xFF);

      return length;
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/TurningThenStraightFootstepGenerator.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePose2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.overheadPath.TurnThenStraightOverheadPath;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.robotics.robotSide.SideDependentList;

public class TurningThenStraightFootstepGenerator extends AbstractSimpleParametersFootstepGenerator
{
   private TurnThenStraightOverheadPath footstepPath;
   private FramePoint2D endPoint;
   private double pathOrientation;

   public TurningThenStraightFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FramePoint2D endPoint, PathTypeStepParameters pathType)
   {
      super(feet, soleFrames, pathType);
      this.endPoint = endPoint;
      this.pathOrientation = pathType.getAngle();
   }

   public TurningThenStraightFootstepGenerator(SideDependentList<RigidBodyBasics> feet, SideDependentList<ReferenceFrame> soleFrames, FramePoint2D endPoint, PathTypeStepParameters pathType,
           RobotSide stanceStart)
   {
      super(feet, soleFrames, pathType, stanceStart);
      this.endPoint = endPoint;
      this.pathOrientation = pathType.getAngle();
   }

   protected void initialize(FramePose2D startPose)
   {
      setFootstepPath(new TurnThenStraightOverheadPath(startPose, endPoint, pathOrientation, noTranslationTolerance));
      footstepCounter = new FootstepCounterForTurnThenStraightPaths();
   }

   protected void initializeFootstepCounter(double openingAngle, double closingAngle, double stepLength, boolean hipExtensionFirst, boolean willSetFirstStraightStepToFarSideIfNoTurns, double standardFootWidth)
   {
      double totalAngle = footstepPath.getSignedTurningAmount();
      double totalDistance = footstepPath.getDistance();
      ((FootstepCounterForTurnThenStraightPaths) footstepCounter).initialize(openingAngle, closingAngle, stepLength, totalAngle, totalDistance,
              hipExtensionFirst, isLeftRightPath, willSetFirstStraightStepToFarSideIfNoTurns, initialDeltaFeetX, initialDeltaFeetY, initialDeltaFeetYaw, standardFootWidth);
   }

   protected double getSignedInitialTurnDirection()
   {
      return footstepPath.getSignedTurningAmount();
   }

   public void setFootstepPath(TurnThenStraightOverheadPath footstepPath)
   {
      this.footstepPath = footstepPath;
   }

   protected TurnThenStraightOverheadPath getPath()
   {
      return footstepPath;
   }

   public double getTurningAmount()
   {
      if (footstepPath == null)
         initialize();

      return footstepPath.getSignedTurningAmount();
   }

   public double getDistance()
   {
      if (footstepPath == null)
         initialize();

      return footstepPath.getDistance();
   }
   
   public boolean hasDisplacement()
   {
      double eps = noTranslationTolerance;

      return (getDistance() > eps) || (Math.abs(getTurningAmount()) > eps);
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/parameters/AtlasParameterTunerOffline.java<|end_filename|>
package us.ihmc.atlas.parameters;

import java.nio.file.Path;
import java.nio.file.Paths;

import org.apache.commons.io.FilenameUtils;

import us.ihmc.atlas.AtlasRobotModel;
import us.ihmc.parameterTuner.guiElements.main.ParameterGuiInterface;
import us.ihmc.parameterTuner.guiElements.main.ParameterTuningApplication;
import us.ihmc.parameterTuner.offline.FileInputManager;

public class AtlasParameterTunerOffline extends ParameterTuningApplication
{
   @Override
   protected ParameterGuiInterface createInputManager()
   {
      String relativeFilePath = FilenameUtils.separatorsToSystem(AtlasRobotModel.getParameterResourceName());
      Path filePath = Paths.get("src", "main", "resources", relativeFilePath);
      return new FileInputManager(filePath.toFile());
   }

   public static void main(String[] args)
   {
      launch(args);
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/subscribers/TimeStampedTransformBuffer.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.subscribers;

import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.robotics.kinematics.TimeStampedTransform3D;
import us.ihmc.robotics.kinematics.TransformInterpolationCalculator;

public class TimeStampedTransformBuffer
{
   private final int size;
   private final TimeStampedTransform3D[] buffer;
   private int currentIndex;
   private long oldestTimeStamp = Long.MAX_VALUE;
   private long newestTimestamp;
   private boolean filledBufferAtleastOnce;

   private final TransformInterpolationCalculator transformInterpolationCalculator = new TransformInterpolationCalculator();
   
   public TimeStampedTransformBuffer(int size)
   {
      this.size = size;
      this.buffer = new TimeStampedTransform3D[size];
      for(int i = 0; i < buffer.length; i++)
      {
         this.buffer[i] = new TimeStampedTransform3D();
         this.buffer[i].setTimeStamp(Long.MAX_VALUE);
      }
      currentIndex = 0;
   }

   /**
    * Try to find the transform at the given timestamp (key). Interpolate between two registered timestamps if the one asked for hasn't been registered.
    * @param timestamp
    * @param timeStampedTransform3DToPack
    * @return
    */
   public boolean findTransform(long timestamp, TimeStampedTransform3D timeStampedTransform3DToPack)
   {
      if (!isInRange(timestamp))
         return false;

      for (int i = currentIndex - 1; i >= -size + currentIndex; i--)
      {
         int index = (i < 0) ? size + i : i;

         TimeStampedTransform3D floorData = buffer[index];
         if (floorData.getTimeStamp() == timestamp)
         {
            timeStampedTransform3DToPack.set(floorData);
            return true;
         }
         else if (floorData.getTimeStamp() < timestamp)
         {
            index++;

            if (index >= size)
            {
               index = 0;
            }

            TimeStampedTransform3D ceilingData = buffer[index];

            transformInterpolationCalculator.interpolate(floorData, ceilingData, timeStampedTransform3DToPack, timestamp);
            return true;
         }
      }

      return false;
   }

   public void put(RigidBodyTransform newestTransform, long timeStamp)
   {
      TimeStampedTransform3D recycledTimestampedTransformFromBuffer = buffer[currentIndex];
      recycledTimestampedTransformFromBuffer.setTimeStamp(timeStamp);
      recycledTimestampedTransformFromBuffer.setTransform3D(newestTransform);
      
      newestTimestamp = timeStamp;
      currentIndex++;

      if (currentIndex >= size)
      {
         currentIndex = 0;
         filledBufferAtleastOnce = true;
      }

      if (oldestTimeStamp == Long.MAX_VALUE)
      {
         oldestTimeStamp = newestTimestamp;
      }
      else if(filledBufferAtleastOnce)
      {
         oldestTimeStamp = buffer[currentIndex].getTimeStamp();
      }
   }

   public boolean isInRange(long timestamp)
   {
      return ((timestamp >= oldestTimeStamp) && (timestamp <= newestTimestamp));
   }

   public long getNewestTimestamp()
   {
      return newestTimestamp;
   }

   public long getOldestTimestamp()
   {
      return oldestTimeStamp;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedRequestedSteppingStateMessage.java<|end_filename|>
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 quadruped controller API.
       * This message is used to switch the control scheme between different stepping modes.
       */
public class QuadrupedRequestedSteppingStateMessage extends Packet<QuadrupedRequestedSteppingStateMessage> implements Settable<QuadrupedRequestedSteppingStateMessage>, EpsilonComparable<QuadrupedRequestedSteppingStateMessage>
{
   public static final byte REQUEST_STAND = (byte) 0;
   public static final byte REQUEST_STEP = (byte) 1;
   public static final byte REQUEST_SOLE_WAYPOINT = (byte) 2;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Specifies the which state the controller should transition into.
            */
   public byte quadruped_stepping_requested_event_ = (byte) 255;

   public QuadrupedRequestedSteppingStateMessage()
   {
   }

   public QuadrupedRequestedSteppingStateMessage(QuadrupedRequestedSteppingStateMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedRequestedSteppingStateMessage other)
   {
      sequence_id_ = other.sequence_id_;

      quadruped_stepping_requested_event_ = other.quadruped_stepping_requested_event_;

   }

   /**
            * 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_;
   }

   /**
            * Specifies the which state the controller should transition into.
            */
   public void setQuadrupedSteppingRequestedEvent(byte quadruped_stepping_requested_event)
   {
      quadruped_stepping_requested_event_ = quadruped_stepping_requested_event;
   }
   /**
            * Specifies the which state the controller should transition into.
            */
   public byte getQuadrupedSteppingRequestedEvent()
   {
      return quadruped_stepping_requested_event_;
   }


   public static Supplier<QuadrupedRequestedSteppingStateMessagePubSubType> getPubSubType()
   {
      return QuadrupedRequestedSteppingStateMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedRequestedSteppingStateMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedRequestedSteppingStateMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.quadruped_stepping_requested_event_, other.quadruped_stepping_requested_event_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedRequestedSteppingStateMessage)) return false;

      QuadrupedRequestedSteppingStateMessage otherMyClass = (QuadrupedRequestedSteppingStateMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.quadruped_stepping_requested_event_ != otherMyClass.quadruped_stepping_requested_event_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedRequestedSteppingStateMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("quadruped_stepping_requested_event=");
      builder.append(this.quadruped_stepping_requested_event_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/multisenseBlobExperiments/MultisenseBlobDetectionPointCloudReceiver.java<|end_filename|>
package us.ihmc.atlas.multisenseBlobExperiments;

import controller_msgs.msg.dds.PointCloudWorldPacket;
import sensor_msgs.PointCloud2;
import us.ihmc.communication.packetCommunicator.PacketCommunicator;
import us.ihmc.communication.packets.PacketDestination;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.utilities.ros.subscriber.RosPointCloudSubscriber;

public class MultisenseBlobDetectionPointCloudReceiver extends RosPointCloudSubscriber
{
   private static final boolean DEBUG = false;

   private final PacketCommunicator packetCommunicator;

   public MultisenseBlobDetectionPointCloudReceiver(PacketCommunicator packetCommunicator)
   {
      this.packetCommunicator = packetCommunicator;
   }

   /**
    * received a point cloud from the multisense
    * transform the points using the mocap data
    */
   @Override
   public void onNewMessage(PointCloud2 pointCloud)
   {
      UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud);
      Point3D[] points = pointCloudData.getPoints();

      PointCloudWorldPacket pointCloudWorldPacket = new PointCloudWorldPacket();
      HumanoidMessageTools.setDecayingWorldScan(points, pointCloudWorldPacket);
      pointCloudWorldPacket.setDestination(PacketDestination.BROADCAST.ordinal());

      if(DEBUG)
         System.out.println("received point cloud. cloud size = " + points.length);

      packetCommunicator.send(pointCloudWorldPacket);
   }
}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/depthData/RosPointCloudReceiver.java<|end_filename|>
package us.ihmc.ihmcPerception.depthData;

import java.util.ArrayList;
import java.util.Arrays;

import sensor_msgs.PointCloud2;
import us.ihmc.commons.time.Stopwatch;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.utilities.ros.RosMainNode;
import us.ihmc.utilities.ros.subscriber.RosPointCloudSubscriber;

public class RosPointCloudReceiver extends RosPointCloudSubscriber
{
   private final boolean DEBUG = false;
   private Stopwatch timer;
   {
      if(DEBUG)
         timer = new Stopwatch().start();
   }

   private final String rosTopic;
   private final ReferenceFrame cloudFrame;
   private final PointCloudDataReceiverInterface pointCloudDataReceiver;
   private final ReferenceFrame sensorframe;
   private final PointCloudSource[] pointCloudSource;

   public RosPointCloudReceiver(String rosTopic, RosMainNode rosMainNode, ReferenceFrame cloudFrame, ReferenceFrame sensorframe,
         PointCloudDataReceiverInterface pointCloudDataReceiver, PointCloudSource... pointCloudSource)
   {
      this.rosTopic = rosTopic;
      this.cloudFrame = cloudFrame;
      this.pointCloudDataReceiver = pointCloudDataReceiver;
      this.sensorframe = sensorframe;
      this.pointCloudSource = pointCloudSource;

      rosMainNode.attachSubscriber(rosTopic, this);
   }

   @Override
	public void onNewMessage(PointCloud2 pointCloud) 
	{
      if(DEBUG)
      {
         System.out.println(getClass().getSimpleName() + ": Received point cloud from " + rosTopic + " at rate " + 1.0 / timer.averageLap() + " FPS");
         timer.lap();
      }

      UnpackedPointCloud pointCloudData = unpackPointsAndIntensities(pointCloud);
		Point3D[] points = pointCloudData.getPoints();
		ArrayList<Point3D> pointsAsArrayList = new ArrayList<Point3D>(Arrays.asList(points));
	   long[] timestamps = new long[points.length];
	   long time = pointCloud.getHeader().getStamp().totalNsecs();
	   Arrays.fill(timestamps, time);
		pointCloudDataReceiver.receivedPointCloudData(cloudFrame, sensorframe, timestamps, pointsAsArrayList, pointCloudSource);
	}
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/footstepGenerator/FootstepCounter.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep.footstepGenerator;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.commons.MathTools;

public abstract class FootstepCounter
{
   private static final String DEBUG_SIGNATURE = "FootstepCounter\n\t";
   protected double closingAngle;
   private final boolean DEBUG = false;
   protected boolean hipExtensionFirst;
   protected double openingAngle;
   protected ArrayList<Double> stepSValues = new ArrayList<Double>();
   protected ArrayList<Double> linearToTurningRatios = new ArrayList<Double>();
   protected ArrayList<Integer> subPathIndices = new ArrayList<Integer>();
   protected double stepLength;
   protected int indexOfFirstStraightPathStep = -1;
   protected boolean isLeftRightMotion;

   public List<Double> getSValues2()
   {
      return stepSValues;
   }

   public List<Double> getlinearToTurningRatios()
   {
      return linearToTurningRatios;
   }

   public List<Integer> getSubPathIndices() {
      return subPathIndices;
   }

   public int getFirstStraightPathStepIndex()
   {
      return indexOfFirstStraightPathStep;
   }

   protected void reInitializeFields(double openingAngle, double closingAngle, double length, boolean hipExtensionFirst, boolean isLeftRightMotion)
   {
      stepSValues.clear();
      linearToTurningRatios.clear();
      subPathIndices.clear();
      this.openingAngle = openingAngle;
      this.closingAngle = closingAngle;
      this.stepLength = length;
      this.hipExtensionFirst = hipExtensionFirst;
      this.isLeftRightMotion = isLeftRightMotion;
      indexOfFirstStraightPathStep = -1;
   }

   protected double generateTurnSteps(double totalAngle, double initialHipYawOpen, int currentSubPathIndex)
   {
      double overshootStepFraction;
      if (MathTools.epsilonEquals(0.0, totalAngle, 1e-8))
      {
         overshootStepFraction = 1.0;

         return overshootStepFraction;
      }

      double openingDeltaS = openingAngle / Math.abs(totalAngle);
      double closingDeltaS = closingAngle / Math.abs(totalAngle);
      double initialOffsetDeltaS = Math.abs((initialHipYawOpen / 2) / totalAngle);
      
      boolean hipExtensionSecond = !hipExtensionFirst;
      double firstTurningStepDeltaS = hipExtensionFirst ? openingDeltaS : closingDeltaS;
      double secondTurningStepDeltaS = hipExtensionSecond ? openingDeltaS : closingDeltaS;

      double deltaSForPairOfTurningSteps = closingDeltaS + openingDeltaS;
      double totalS = 1 + initialOffsetDeltaS;
      double pairsOfTurningSteps = totalS / deltaSForPairOfTurningSteps;
      int minimumPairsOfTurningSteps = (int) Math.floor(pairsOfTurningSteps);
      double sRemaining = totalS - (minimumPairsOfTurningSteps) * deltaSForPairOfTurningSteps;

      printIfDebug("pairsOfTurningSteps =" + pairsOfTurningSteps);

      int totalPureTurningSteps;
      if (firstTurningStepDeltaS > sRemaining)
      {
         totalPureTurningSteps = 2 * minimumPairsOfTurningSteps;
         overshootStepFraction = 1 - sRemaining / firstTurningStepDeltaS;
      }
      else
      {
         totalPureTurningSteps = 2 * minimumPairsOfTurningSteps + 1;
         sRemaining = (sRemaining - firstTurningStepDeltaS);
         overshootStepFraction = 1 - sRemaining / secondTurningStepDeltaS;
      }

      printIfDebug("sRemaining = " + sRemaining + "\n\t" + "overshoot = " + overshootStepFraction);


      double currentS = -initialOffsetDeltaS;
      if (initialOffsetDeltaS > firstTurningStepDeltaS)
         printIfDebug("Initial foot yaw offset is greater than the first turning step yaw and will result in a negative path S");

      for (int i = 0; i < totalPureTurningSteps; i++)
      {
         currentS += isEvenStep(i) ? firstTurningStepDeltaS : secondTurningStepDeltaS;
         addSubpathStep(currentS, 0.0, currentSubPathIndex);
      }

      return overshootStepFraction;
   }

   protected boolean generateStraightSegmentSteps(double totalDistance, double overshootStepFraction, double initialFootDeltaX, double initialFootDeltaY,
           int currentSubPathIndex, boolean willSetFirstStraightStepToFarSideIfNoTurns, double standardFootWidth)
   {
      if (MathTools.epsilonEquals(0.0, totalDistance, 1e-7))
      {
         printIfDebug("small straight segment, skipping straight steps");
         return false;
      }

      if (isLeftRightMotion && (stepSValues.size() > 0))
      {
         addSubpathStep(0, 1.0, currentSubPathIndex);
         addSubpathStep(0, 1.0, currentSubPathIndex);    
         // This last step may be deleted higher up, if it is the same side as the next step... Just need to know direction of travel...
         // indexOfFirstStraightPathStep will help identify this step

         overshootStepFraction = 1.0;
      }

      indexOfFirstStraightPathStep = stepSValues.size();

      double initialFeetOffsetAdjustment = 0;
      boolean leftRightMotionFarFootStep = false;
      if (indexOfFirstStraightPathStep == 0)
      {
         if (isLeftRightMotion)
         {
            if (willSetFirstStraightStepToFarSideIfNoTurns)
            {
               initialFeetOffsetAdjustment = (initialFootDeltaY - standardFootWidth) / 2.0;    // far foot adjustment (will require extra step or one less...)
               leftRightMotionFarFootStep = true;
            }
            else
            {
               initialFeetOffsetAdjustment = (standardFootWidth - initialFootDeltaY) / 2.0;    // near foot adjustment (correct)
               leftRightMotionFarFootStep = false;
            }
         }
         else
         {
            if (willSetFirstStraightStepToFarSideIfNoTurns)
               initialFeetOffsetAdjustment = Math.abs(initialFootDeltaX) / 2.0;    // far foot adjustment
            else
               initialFeetOffsetAdjustment = -Math.abs(initialFootDeltaX) / 2.0;    // near foot adjustment (potential for negative pathS)
         }
      }

      double estimatedNumberOfStraightSteps = Math.max(0, (totalDistance - initialFeetOffsetAdjustment)) / stepLength;
      int straightStepsBesidesOvershoot = (int) Math.max(0, Math.ceil(estimatedNumberOfStraightSteps - overshootStepFraction));
      double actualStepLength = (totalDistance - initialFeetOffsetAdjustment) / (straightStepsBesidesOvershoot + overshootStepFraction);
      double actualDeltaSPerStep = actualStepLength / totalDistance;
      double currentStraightSValue = initialFeetOffsetAdjustment / totalDistance + overshootStepFraction * actualDeltaSPerStep;

      if (leftRightMotionFarFootStep)
      {
         currentStraightSValue-=actualDeltaSPerStep;
         straightStepsBesidesOvershoot ++;
      }

      addSubpathStep(currentStraightSValue, overshootStepFraction, currentSubPathIndex);
      if (isLeftRightMotion && !leftRightMotionFarFootStep)
         addSubpathStep(currentStraightSValue, 1.0, currentSubPathIndex);

      for (int i = 0; i < straightStepsBesidesOvershoot; i++)
      {
         currentStraightSValue += actualDeltaSPerStep;
         addSubpathStep(currentStraightSValue, 1.0, currentSubPathIndex);
         if (isLeftRightMotion)
            addSubpathStep(currentStraightSValue, 1.0, currentSubPathIndex);
      }

      printIfDebug("currentStraightSValue after finishing straight steps = " + currentStraightSValue);
      
      return true;
   }

   protected void addSubpathStep(double subPathS, double linearToTurningRatio, int pathSegmentIndex)
   {
      stepSValues.add(subPathS);
      linearToTurningRatios.add(linearToTurningRatio);
      subPathIndices.add(pathSegmentIndex);
   }

   protected void removeSubpathStep(int indexToRemove)
   {
      stepSValues.remove(indexToRemove);
      linearToTurningRatios.remove(indexToRemove);
      subPathIndices.remove(indexToRemove);
   }

   protected abstract int getNumberOfSubPaths();

   protected boolean isEvenStep(int i)
   {
      return i % 2 == 0;
   }

   protected void printIfDebug(String message)
   {
      if (DEBUG)
      {
         System.out.println(DEBUG_SIGNATURE + message);
      }
   }

   ArrayList<Double> getStepSValues()
   {
      return new ArrayList<Double>(this.stepSValues);
   }

   void setStepSValues(ArrayList<Double> stepSValues)
   {
      this.stepSValues.clear();
      this.stepSValues.addAll(stepSValues);
   }

   protected void generateFinalSquareUpSteps(boolean lastStepsAreStraightSteps) {
      removeFinalStraightStepsToBeSquared(lastStepsAreStraightSteps);
   
      generateFinalSquareUpStep();
      generateFinalSquareUpStep();
   }

   private void removeFinalStraightStepsToBeSquared(boolean lastStepsAreStraightSteps) {
      int numSteps = stepSValues.size();
      if (lastStepsAreStraightSteps && (numSteps > 0))
      {
         // Remove last straight step since it can go directly to the correct orientation with the final square up steps
         int lastStep = numSteps - 1;
         removeSubpathStep(lastStep);
   
         // Need to remove two steps if left/right path and no turn steps taken
         if (isLeftRightMotion)
         {
            lastStep = numSteps - 2;
            removeSubpathStep(lastStep);
         }
      }
   }

   private void generateFinalSquareUpStep() {
      addSubpathStep(1.0, 1.0, getNumberOfSubPaths()-1); 
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/geometry/ConcaveHullDecomposition.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.geometry;

import static us.ihmc.commons.lists.ListWrappingIndexTools.*;
import static us.ihmc.robotEnvironmentAwareness.geometry.ConcaveHullTools.*;

import java.util.ArrayList;
import java.util.List;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.log.LogTools;

public class ConcaveHullDecomposition
{
   private static final boolean EXPORT_BAD_POLYGONS_TO_FILE = false;
   private static final String BAD_POLYGONS_FILE_NAME = "badPolygon";
   private static int badPolygonsFileIndex = 0;

   /**
    * Inspired from the SL-decomposition in the paper 
    * <a href="https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=2&cad=rja&uact=8&ved=0ahUKEwjZlOab96XPAhXBQD4KHcXeB4MQFggsMAE&url=https%3A%2F%2Fparasol.tamu.edu%2Fpublications%2Fdownload.php%3Ffile_id%3D390&usg=AFQjCNF3wXvuCxXNREhu4CW-oNyd1caa0A&sig2=X-zxaHykED7EuqkYhkfUgg">
    *  Approximate Convex Decomposition of Polygons</a>.
    *  @param concaveHullCollection [input] the collection of concave hulls to be decomposed into convex polygons.
    *  @param depthThreshold [input] the algorithm determines whether the polygon is to split or not by looking at the maximum depth of concave pockets in the concave hull.
    *  When a pocket is deeper than {@code depthThreshold} the concave hull will be split in two.
    *  Otherwise, the pocket vertices will be removed.
    *  @param convexPolygonsToPack [output] the convex polygons approximating the concave hull.
    */
   public static void recursiveApproximateDecomposition(ConcaveHullCollection concaveHullCollection, double depthThreshold, List<ConvexPolygon2D> convexPolygonsToPack)
   {
      for (ConcaveHull concaveHull : concaveHullCollection)
         recursiveApproximateDecomposition(concaveHull.getConcaveHullVertices(), depthThreshold, convexPolygonsToPack);
   }

   /**
    * Inspired from the SL-decomposition in the paper 
    * <a href="https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=2&cad=rja&uact=8&ved=0ahUKEwjZlOab96XPAhXBQD4KHcXeB4MQFggsMAE&url=https%3A%2F%2Fparasol.tamu.edu%2Fpublications%2Fdownload.php%3Ffile_id%3D390&usg=AFQjCNF3wXvuCxXNREhu4CW-oNyd1caa0A&sig2=X-zxaHykED7EuqkYhkfUgg">
    *  Approximate Convex Decomposition of Polygons</a>.
    *  @param concaveHull [input] the concave hull to be decomposed into convex polygons.
    *  @param depthThreshold [input] the algorithm determines whether the polygon is to split or not by looking at the maximum depth of concave pockets in the concave hull.
    *  When a pocket is deeper than {@code depthThreshold} the concave hull will be split in two.
    *  Otherwise, the pocket vertices will be removed.
    *  @param convexPolygonsToPack [output] the convex polygons approximating the concave hull.
    */
   public static void recursiveApproximateDecomposition(ConcaveHull concaveHull, double depthThreshold, List<ConvexPolygon2D> convexPolygonsToPack)
   {
      recursiveApproximateDecomposition(concaveHull.getConcaveHullVertices(), depthThreshold, convexPolygonsToPack);
   }

   /**
    * Inspired from the SL-decomposition in the paper 
    * <a href="https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=2&cad=rja&uact=8&ved=0ahUKEwjZlOab96XPAhXBQD4KHcXeB4MQFggsMAE&url=https%3A%2F%2Fparasol.tamu.edu%2Fpublications%2Fdownload.php%3Ffile_id%3D390&usg=AFQjCNF3wXvuCxXNREhu4CW-oNyd1caa0A&sig2=X-zxaHykED7EuqkYhkfUgg">
    *  Approximate Convex Decomposition of Polygons</a>.
    *  @param concaveHullVertices [input] the concave hull to be decomposed into convex polygons.
    *  @param depthThreshold [input] the algorithm determines whether the polygon is to split or not by looking at the maximum depth of concave pockets in the concave hull.
    *  When a pocket is deeper than {@code depthThreshold} the concave hull will be split in two.
    *  Otherwise, the pocket vertices will be removed.
    *  @param convexPolygonsToPack [output] the convex polygons approximating the concave hull.
    */
   public static void recursiveApproximateDecomposition(List<? extends Point2DReadOnly> concaveHullVertices, double depthThreshold, List<ConvexPolygon2D> convexPolygonsToPack)
   {
      recursiveApproximateDecompositionInternal(new ArrayList<>(concaveHullVertices), depthThreshold, convexPolygonsToPack);
   }

   private static void recursiveApproximateDecompositionInternal(List<Point2DReadOnly> concaveHullVertices, double depthThreshold, List<ConvexPolygon2D> convexPolygonsToPack)
   {
      
      ConcaveHullPocket pocket = null;
      boolean hasFoundDeepPocket = false;

      while (!hasFoundDeepPocket)
      {
         if (concaveHullVertices.isEmpty())
         {
            LogTools.warn("The concave hull is empty");
            return;
         }

         // The concave hull is actually convex, end of recursion
         if (ConcaveHullTools.isHullConvex(concaveHullVertices))
         {
            convexPolygonsToPack.add(new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(concaveHullVertices)));
            return;
         }

         pocket = ConcaveHullTools.findFirstConcaveHullPocket(concaveHullVertices);

         if (pocket == null)
         {
            LogTools.error("Did not find any pocket.");
            if (EXPORT_BAD_POLYGONS_TO_FILE)
               ConcaveHullTools.exportVertexListToFile(concaveHullVertices, BAD_POLYGONS_FILE_NAME + (badPolygonsFileIndex++));
            return;
         }

         int startBridgeIndex = pocket.getStartBridgeIndex();
         int endBridgeIndex = pocket.getEndBridgeIndex();

         // The pocket is negligible, remove the vertices.
         if (pocket.getMaxDepth() < depthThreshold)
            removeAllExclusive(startBridgeIndex, endBridgeIndex, concaveHullVertices);
         else
            hasFoundDeepPocket = true;
      }

      int deepestVertexIndex = pocket.getDeepestVertexIndex();
      Vector2D cutDirection = new Vector2D();
      Point2DReadOnly endBridgeVertex = pocket.getEndBridgeVertex();
      Point2DReadOnly startBridgeVertex = pocket.getStartBridgeVertex();
      cutDirection.sub(endBridgeVertex, startBridgeVertex);
      cutDirection.normalize();
      cutDirection.set(cutDirection.getY(), -cutDirection.getX()); // Rotate 90 degrees to the right (inside polygon)

      Point2DReadOnly deepestVertex = pocket.getDeepestVertex();


      Point2D otherVertexForCutting = new Point2D();
      int startBridgeIndex = pocket.getStartBridgeIndex();
      int endBridgeIndex = pocket.getEndBridgeIndex();

      int otherVertexIndexForCutting = findClosestIntersectionWithRay(deepestVertex, cutDirection, endBridgeIndex, startBridgeIndex, concaveHullVertices, otherVertexForCutting);
      otherVertexIndexForCutting = next(otherVertexIndexForCutting, concaveHullVertices);
      
      if (otherVertexIndexForCutting == -1)
      {
         LogTools.error("Something went wrong finding the other vertex for cutting. Pocket vertex: " + deepestVertex + ", bridge: start: "
               + startBridgeVertex + ", end: " + endBridgeVertex);
         if (EXPORT_BAD_POLYGONS_TO_FILE)
            ConcaveHullTools.exportVertexListToFile(concaveHullVertices, BAD_POLYGONS_FILE_NAME + (badPolygonsFileIndex++));
         return;
      }

      // It is easier to add a new vertex rather than using an existing one, it limits the number of edge cases.
      concaveHullVertices.add(otherVertexIndexForCutting, otherVertexForCutting);
      if (otherVertexIndexForCutting < deepestVertexIndex)
         deepestVertexIndex++;

      // decompose the two new polygons.
      int p1StartIndex = deepestVertexIndex;
      int p1EndIndex = otherVertexIndexForCutting;
      int p2StartIndex = otherVertexIndexForCutting;
      int p2EndIndex = deepestVertexIndex;

      List<Point2DReadOnly> p1 = subListInclusive(p1StartIndex, p1EndIndex, concaveHullVertices);
      List<Point2DReadOnly> p2 = subListInclusive(p2StartIndex, p2EndIndex, concaveHullVertices);

      if (p1.size() == concaveHullVertices.size() || p2.size() == concaveHullVertices.size())
      {
         if (EXPORT_BAD_POLYGONS_TO_FILE)
            ConcaveHullTools.exportVertexListToFile(concaveHullVertices, BAD_POLYGONS_FILE_NAME + (badPolygonsFileIndex++));
         LogTools.error("Something went wrong splitting the polygon");
         return;
      }

      recursiveApproximateDecomposition(p1, depthThreshold, convexPolygonsToPack);
      recursiveApproximateDecomposition(p2, depthThreshold, convexPolygonsToPack);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/UIConnectedPacket.java<|end_filename|>
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;

/**
       * TODO: This message is not really used, the feature it provides needs to be fixed or this message needs to be deleted.
       */
public class UIConnectedPacket extends Packet<UIConnectedPacket> implements Settable<UIConnectedPacket>, EpsilonComparable<UIConnectedPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;

   public UIConnectedPacket()
   {
   }

   public UIConnectedPacket(UIConnectedPacket other)
   {
      this();
      set(other);
   }

   public void set(UIConnectedPacket 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<UIConnectedPacketPubSubType> getPubSubType()
   {
      return UIConnectedPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return UIConnectedPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(UIConnectedPacket 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 UIConnectedPacket)) return false;

      UIConnectedPacket otherMyClass = (UIConnectedPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("UIConnectedPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HeightQuadTreeToolboxRequestMessage.java<|end_filename|>
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 height quad tree module
       */
public class HeightQuadTreeToolboxRequestMessage extends Packet<HeightQuadTreeToolboxRequestMessage> implements Settable<HeightQuadTreeToolboxRequestMessage>, EpsilonComparable<HeightQuadTreeToolboxRequestMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean request_clear_quad_tree_;
   public boolean request_quad_tree_update_;

   public HeightQuadTreeToolboxRequestMessage()
   {
   }

   public HeightQuadTreeToolboxRequestMessage(HeightQuadTreeToolboxRequestMessage other)
   {
      this();
      set(other);
   }

   public void set(HeightQuadTreeToolboxRequestMessage other)
   {
      sequence_id_ = other.sequence_id_;

      request_clear_quad_tree_ = other.request_clear_quad_tree_;

      request_quad_tree_update_ = other.request_quad_tree_update_;

   }

   /**
            * 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 void setRequestClearQuadTree(boolean request_clear_quad_tree)
   {
      request_clear_quad_tree_ = request_clear_quad_tree;
   }
   public boolean getRequestClearQuadTree()
   {
      return request_clear_quad_tree_;
   }

   public void setRequestQuadTreeUpdate(boolean request_quad_tree_update)
   {
      request_quad_tree_update_ = request_quad_tree_update;
   }
   public boolean getRequestQuadTreeUpdate()
   {
      return request_quad_tree_update_;
   }


   public static Supplier<HeightQuadTreeToolboxRequestMessagePubSubType> getPubSubType()
   {
      return HeightQuadTreeToolboxRequestMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HeightQuadTreeToolboxRequestMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HeightQuadTreeToolboxRequestMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.request_clear_quad_tree_, other.request_clear_quad_tree_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.request_quad_tree_update_, other.request_quad_tree_update_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HeightQuadTreeToolboxRequestMessage)) return false;

      HeightQuadTreeToolboxRequestMessage otherMyClass = (HeightQuadTreeToolboxRequestMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.request_clear_quad_tree_ != otherMyClass.request_clear_quad_tree_) return false;

      if(this.request_quad_tree_update_ != otherMyClass.request_quad_tree_update_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HeightQuadTreeToolboxRequestMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("request_clear_quad_tree=");
      builder.append(this.request_clear_quad_tree_);      builder.append(", ");
      builder.append("request_quad_tree_update=");
      builder.append(this.request_quad_tree_update_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/CenterOfMassAccelerationCalculator.java<|end_filename|>
package us.ihmc.robotics.screwTheory;

import java.util.stream.Stream;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

public class CenterOfMassAccelerationCalculator
{
   private final FramePoint3D comLocation = new FramePoint3D(ReferenceFrame.getWorldFrame());
   private final FrameVector3D linkLinearMomentumDot = new FrameVector3D(ReferenceFrame.getWorldFrame());
   private final SpatialAccelerationCalculator spatialAccelerationCalculator;
   private final RigidBodyBasics[] rigidBodies;
   private final RigidBodyBasics base;

   public CenterOfMassAccelerationCalculator(RigidBodyBasics rootBody, SpatialAccelerationCalculator spatialAccelerationCalculator)
   {
      this(rootBody, rootBody.subtreeArray(), spatialAccelerationCalculator);
   }

   public CenterOfMassAccelerationCalculator(RigidBodyBasics base, RigidBodyBasics[] rigidBodies, SpatialAccelerationCalculator spatialAccelerationCalculator)
   {
      this.spatialAccelerationCalculator = spatialAccelerationCalculator;
      this.rigidBodies = Stream.of(rigidBodies).filter(body -> body.getInertia() != null).toArray(RigidBodyBasics[]::new);
      this.base = base;
   }
   
   public void getCoMAcceleration(FrameVector3D comAccelerationToPack)
   {
      boolean firstIteration = true;
      double totalMass = 0.0;

      for (RigidBodyBasics rigidBody : rigidBodies)
      {
         double mass = rigidBody.getInertia().getMass();
         rigidBody.getCenterOfMass(comLocation);

         linkLinearMomentumDot.setIncludingFrame(spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(base, rigidBody, comLocation));
         linkLinearMomentumDot.scale(mass);

         if (firstIteration)
            comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot);
         else
            comAccelerationToPack.add(linkLinearMomentumDot);

         totalMass += mass;
         firstIteration = false;
      }

      comAccelerationToPack.scale(1.0 / totalMass);
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/linearAlgebra/DampedSVDNullspaceCalculatorTest.java<|end_filename|>
package us.ihmc.robotics.linearAlgebra;

public class DampedSVDNullspaceCalculatorTest extends DampedNullspaceCalculatorTest
{
   @Override
   public DampedNullspaceCalculator getDampedNullspaceProjectorCalculator()
   {
      return new DampedSVDNullspaceCalculator(10, 0.0);
   }

   @Override
   public NullspaceCalculator getNullspaceProjectorCalculator()
   {
      return getDampedNullspaceProjectorCalculator();
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/walkingpath/footstep/SkeletonPathFootStep.java<|end_filename|>
package us.ihmc.manipulation.planning.walkingpath.footstep;

import java.awt.geom.Point2D;

import us.ihmc.robotics.robotSide.RobotSide;

public class SkeletonPathFootStep
{
   private RobotSide robotSide;
   private Point2D location;
   private double yawAngle;
   private int indexOfSegment;

   public SkeletonPathFootStep()
   {
      this.robotSide = RobotSide.LEFT;
      this.location = new Point2D.Double();
      this.yawAngle = 0.0;
   }

   public SkeletonPathFootStep(RobotSide robotSide, Point2D location, double yawAngle)
   {
      this.robotSide = robotSide;
      this.location = location;
      this.yawAngle = yawAngle;
   }

   public void setRobotSide(RobotSide robotSide)
   {
      this.robotSide = robotSide;
   }

   public void setLocation(Point2D location)
   {
      this.location = location;
   }

   public void setYawAngle(double yawAngle)
   {
      this.yawAngle = yawAngle;
   }

   public void setIndexOfSegment(int index)
   {
      this.indexOfSegment = index;
   }

   public RobotSide getRobotSide()
   {
      return robotSide;
   }

   public Point2D getLocation()
   {
      return location;
   }

   public double getYawAngle()
   {
      return yawAngle;
   }

   public int getIndexOfSegment()
   {
      return this.indexOfSegment;
   }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/posePlayback/PlaybackPose.java<|end_filename|>
package us.ihmc.avatar.posePlayback;

import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.Set;

import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
import us.ihmc.robotModels.FullRobotModel;
import us.ihmc.simulationconstructionset.OneDegreeOfFreedomJoint;
import us.ihmc.simulationconstructionset.OneDegreeOfFreedomJointHolder;

public class PlaybackPose
{
   private final LinkedHashMap<OneDoFJointBasics, Double> playbackPoseMap;
   private double playBackDelayBeforePose = 1.0;
   private double playBackDuration = 1.0;

   public PlaybackPose(FullRobotModel fullRobotModel, OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder)
   {
      playbackPoseMap = new LinkedHashMap<OneDoFJointBasics, Double>();
      OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints();

      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
      {
         String jointName = oneDoFJoint.getName();
         OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName);

         double jointAngle = oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue();

         playbackPoseMap.put(oneDoFJoint, jointAngle);
      }
   }

   public PlaybackPose(FullRobotModel fullRobotModel, OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder, double playBackDelayBeforePose,
                       double playbackDuration)
   {
      this(fullRobotModel, oneDegreeOfFreedomJointHolder);
      this.playBackDelayBeforePose = playBackDelayBeforePose;
      this.playBackDuration = playbackDuration;
   }

   public PlaybackPose(LinkedHashMap<OneDoFJointBasics, Double> playbackPoseMap)
   {
      if (playbackPoseMap == null) throw new RuntimeException("playbackPoseMap == null");
      this.playbackPoseMap = playbackPoseMap;
   }


   public PlaybackPose(OneDoFJointBasics[] oneDoFJoints, double[] jointAngles, double delay, double duration)
   {
      int length = oneDoFJoints.length;

      if (jointAngles.length != length)
         throw new RuntimeException("jointAngles.length != length");

      playbackPoseMap = new LinkedHashMap<OneDoFJointBasics, Double>();

      for (int i = 0; i < oneDoFJoints.length; i++)
      {
         OneDoFJointBasics oneDoFJoint = oneDoFJoints[i];
         playbackPoseMap.put(oneDoFJoint, jointAngles[i]);
      }

      this.playBackDelayBeforePose = delay;
      this.playBackDuration = duration;
   }

   public PlaybackPose(OneDoFJointBasics[] joints, double playBackDelayBeforePose, double playbackDuration)
   {
      playbackPoseMap = new LinkedHashMap<OneDoFJointBasics, Double>();

      for (OneDoFJointBasics oneDoFJoint : joints)
      {
         playbackPoseMap.put(oneDoFJoint, oneDoFJoint.getQ());
      }

      this.playBackDelayBeforePose = playBackDelayBeforePose;
      this.playBackDuration = playbackDuration;
   }

   public PlaybackPose(LinkedHashMap<OneDoFJointBasics, Double> playbackPoseMap, double playBackDelayBeforePose, double playbackDuration)
   {
      if (playbackPoseMap == null) throw new RuntimeException("playbackPoseMap == null");

      this.playbackPoseMap = playbackPoseMap;
      this.playBackDelayBeforePose = playBackDelayBeforePose;
      this.playBackDuration = playbackDuration;
   }

   public void setRobotAtPose(OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder)
   {
      Set<OneDoFJointBasics> oneDoFJoints = playbackPoseMap.keySet();

      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
      {
         String jointName = oneDoFJoint.getName();
         OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName);

         Double value = playbackPoseMap.get(oneDoFJoint);
         oneDegreeOfFreedomJoint.getQYoVariable().set(value);
      }
   }

   public static PlaybackPose morph(PlaybackPose pose1, PlaybackPose pose2, double morphPercentage)
   {
      if (morphPercentage < 0.0)
         morphPercentage = 0.0;
      if (morphPercentage > 1.0)
         morphPercentage = 1.0;


      Set<OneDoFJointBasics> oneDoFJoints = pose1.playbackPoseMap.keySet();
      LinkedHashMap<OneDoFJointBasics, Double> playbackPoseMap = new LinkedHashMap<OneDoFJointBasics, Double>();

      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
      {
         double angleOne = pose1.getJointAngle(oneDoFJoint);
         double angleTwo = pose2.getJointAngle(oneDoFJoint);

         double morphedAngle = (1.0 - morphPercentage) * angleOne + morphPercentage * angleTwo;

         playbackPoseMap.put(oneDoFJoint, morphedAngle);
      }

      return new PlaybackPose(playbackPoseMap);
   }

   public Double getJointAngle(OneDoFJointBasics oneDoFJoint)
   {
      if (!playbackPoseMap.containsKey(oneDoFJoint)) return null;
      return playbackPoseMap.get(oneDoFJoint);
   }

   public double getPlayBackDelayBeforePose()
   {
      return playBackDelayBeforePose;
   }

   public double getPlayBackDuration()
   {
      return playBackDuration;
   }

   public void setPlaybackDelayBeforePose(double playbackTransitionDelayBeforePose)
   {
      this.playBackDelayBeforePose = playbackTransitionDelayBeforePose;
   }

   public void setPlayBackDuration(double playBackDuration)
   {
      this.playBackDuration = playBackDuration;
   }

   public String toString()
   {
      String ret = "{";

      Set<OneDoFJointBasics> oneDoFJoints = playbackPoseMap.keySet();

      boolean firstOne = true;
      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
      {
         if (!firstOne)
         {
            ret = ret + ", ";
         }

         ret = ret + getJointAngle(oneDoFJoint);
         firstOne = false;
      }

      ret = ret + "}";

      return ret;
   }

   public boolean epsilonEquals(PlaybackPose pose, double jointEpsilon, double timeEpsilon)
   {
      if (this.playbackPoseMap.size() != pose.playbackPoseMap.size())
         return false;

      if (Math.abs(this.playBackDelayBeforePose - pose.getPlayBackDelayBeforePose()) > timeEpsilon)
         return false;

      if (Math.abs(this.playBackDuration - pose.getPlayBackDuration()) > timeEpsilon)
         return false;

      for (OneDoFJointBasics oneDoFJoint : playbackPoseMap.keySet())
      {
         Double value = pose.playbackPoseMap.get(oneDoFJoint);
         if (value == null)
            return false;

         Double valueTwo = playbackPoseMap.get(oneDoFJoint);
         if (Math.abs(valueTwo - value) > jointEpsilon)
         {
            return false;
         }
      }

      return true;
   }

// public void switchSideDependentValues(FullRobotModel fullRobotModel)
// {
//    double[] newJointAngles = new double[ROSAtlasJointMap.numberOfJoints];
//
//    for (int index = 0; index < ROSAtlasJointMap.numberOfJoints; index++)
//    {
//       int oppositeSideIndex = ROSAtlasJointMapCorrelation.oppositeSideIndex[index];
//       double relativeSign = ROSAtlasJointMapCorrelation.symmetricSignChange[index] ? -1.0 : 1.0;
//       newJointAngles[oppositeSideIndex] = relativeSign * jointAngles[index];
//    }
//
//    for (int i = 0; i < jointAngles.length; i++)
//    {
//       jointAngles[i] = newJointAngles[i];
//       //         System.out.println(i + " \t" + jointAngles[i]);
//    }
//
//    //finger joints
//    double temp = 0;
//    for (int i = 0; i < ROSSandiaJointMap.numberOfJointsPerHand; i++)
//    {
//       temp = fingerJointAngles.get(RobotSide.LEFT)[i];
//       fingerJointAngles.get(RobotSide.LEFT)[i] = fingerJointAngles.get(RobotSide.RIGHT)[i];
//       fingerJointAngles.get(RobotSide.RIGHT)[i] = temp;
//    }
// }

   public PlaybackPose copy()
   {
      LinkedHashMap<OneDoFJointBasics, Double> playbackPoseMapCopy = new LinkedHashMap<OneDoFJointBasics, Double>();
      Set<OneDoFJointBasics> oneDoFJoints = this.playbackPoseMap.keySet();

      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
      {
         playbackPoseMapCopy.put(oneDoFJoint, playbackPoseMap.get(oneDoFJoint));
      }

      return new PlaybackPose(playbackPoseMapCopy, playBackDelayBeforePose, playBackDuration);
   }

   public double[] getJointAngles()
   {
      Set<OneDoFJointBasics> oneDoFJoints = this.playbackPoseMap.keySet();

      int size = oneDoFJoints.size();
      int index = 0;
      double[] ret = new double[size];

      for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
      {
         ret[index] = playbackPoseMap.get(oneDoFJoint);
         index++;
      }

      return ret;
   }

   public void getOneDoFJoints(ArrayList<OneDoFJointBasics> oneDoFJointsToPack)
   {
      oneDoFJointsToPack.addAll(playbackPoseMap.keySet());
   }

   public void getJointAngles(ArrayList<OneDoFJointBasics> oneDoFJoints, double[] jointAnglesToPack)
   {
      for (int i = 0; i < oneDoFJoints.size(); i++)
      {
         jointAnglesToPack[i] = playbackPoseMap.get(oneDoFJoints.get(i));
      }
   }


}


<|start_filename|>atlas/src/main/java/us/ihmc/atlas/AtlasPosePlaybackSCSBridge.java<|end_filename|>
package us.ihmc.atlas;

import java.io.IOException;

import com.martiansoftware.jsap.FlaggedOption;
import com.martiansoftware.jsap.JSAP;
import com.martiansoftware.jsap.JSAPResult;

import us.ihmc.avatar.drcRobot.DRCRobotModel;
import us.ihmc.avatar.drcRobot.RobotTarget;
import us.ihmc.avatar.posePlayback.PosePlaybackSCSBridge;
import us.ihmc.robotModels.FullHumanoidRobotModel;
import us.ihmc.simulationConstructionSetTools.util.HumanoidFloatingRootJointRobot;

public class AtlasPosePlaybackSCSBridge
{
   public static void main(String[] args) throws IOException
   {
      // Flag to set robot model
      JSAP jsap = new JSAP();
      FlaggedOption robotModel = new FlaggedOption("robotModel").setLongFlag("model").setShortFlag('m').setRequired(true).setStringParser(JSAP.STRING_PARSER);
      robotModel.setHelp("Robot models: " + AtlasRobotModelFactory.robotModelsToString());

      DRCRobotModel model;
      try
      {
         jsap.registerParameter(robotModel);

         JSAPResult config = jsap.parse(args);

         if (config.success())
         {
            model = AtlasRobotModelFactory.createDRCRobotModel(config.getString("robotModel"), RobotTarget.SCS, false);
         }
         else
         {
            System.out.println("Enter a robot model.");

            return;
         }
      }
      catch (Exception e)
      {
         System.out.println("Robot model not found");
         e.printStackTrace();

         return;
      }

      HumanoidFloatingRootJointRobot sdfRobot = model.createHumanoidFloatingRootJointRobot(false);
      FullHumanoidRobotModel fullRobotModel = model.createFullRobotModel();
      FullHumanoidRobotModel fullRobotModelForSlider = model.createFullRobotModel();

      new PosePlaybackSCSBridge(sdfRobot, fullRobotModel, fullRobotModelForSlider, model.getControllerDT());
   }
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/newtonsCradle/StackOfBlocksRobot.java<|end_filename|>
package us.ihmc.exampleSimulations.newtonsCradle;

import java.util.ArrayList;
import java.util.Random;

import us.ihmc.commons.RandomNumbers;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.robotics.robotDescription.CollisionMeshDescription;
import us.ihmc.simulationconstructionset.FloatingJoint;
import us.ihmc.simulationconstructionset.Link;
import us.ihmc.simulationconstructionset.Robot;

public class StackOfBlocksRobot
{
   private final ArrayList<Robot> robots = new ArrayList<Robot>();

   public StackOfBlocksRobot(int numberOfBlocks)
   {
      Random random = new Random(1886L);
      createFallingObjects(numberOfBlocks, random);
   }

   private void createFallingObjects(int numberOfObjects, Random random)
   {
      double objectHeight = 0.1;

      for (int i = 0; i < numberOfObjects; i++)
      {
         Robot robot = new Robot("StackOfBlocksRobot" + i);

         Vector3D offset = new Vector3D(0.0, 0.0, 0.0);
         FloatingJoint floatingJoint = new FloatingJoint("object" + i, "object" + i, offset, robot);
         Link link = createBox(objectHeight, random, i, robot);

         floatingJoint.setLink(link);
         robot.addRootJoint(floatingJoint);

         double x = 0.0;
         double y = 0.0;
         double z = (objectHeight * 1.05) * (i + 1.0);

         double yaw = 0.0;
         double pitch = RandomNumbers.nextDouble(random, -Math.PI/90.0, Math.PI/90.0);
         double roll = RandomNumbers.nextDouble(random, -Math.PI/90.0, Math.PI/90.0);

         floatingJoint.setPosition(x, y, z);
         floatingJoint.setYawPitchRoll(yaw, pitch, roll);

         this.robots.add(robot);
      }
   }

   private Link createBox(double objectHeight, Random random, int i, Robot robot)
   {
      double objectLength = 0.1;
      double objectWidth = 0.08;
      double objectMass = 0.2;

      Link link = new Link("object" + i);
      link.setMassAndRadiiOfGyration(objectMass, objectLength / 2.0, objectWidth / 2.0, objectHeight / 2.0);
      link.setComOffset(0.0, 0.0, 0.0);

      Graphics3DObject linkGraphics = new Graphics3DObject();
      linkGraphics.translate(0.0, 0.0, -objectHeight / 2.0);
      AppearanceDefinition randomColor = YoAppearance.randomColor(random);
      linkGraphics.addCube(objectLength, objectWidth, objectHeight, randomColor);
      link.setLinkGraphics(linkGraphics);


      CollisionMeshDescription collisionMeshDescription = new CollisionMeshDescription();
      collisionMeshDescription.addCubeReferencedAtCenter(objectLength, objectWidth, objectHeight);
      collisionMeshDescription.setCollisionGroup(0xff);
      collisionMeshDescription.setCollisionMask(0xff);
      link.addCollisionMesh(collisionMeshDescription);
      return link;
   }

   public ArrayList<Robot> getRobots()
   {
      return robots;
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/kryo/PPSTimestampOffsetProvider.java<|end_filename|>
package us.ihmc.humanoidRobotics.kryo;

import controller_msgs.msg.dds.RobotConfigurationData;
import us.ihmc.communication.net.PacketConsumer;

public interface PPSTimestampOffsetProvider extends PacketConsumer<RobotConfigurationData>
{
   public long getCurrentTimestampOffset();

   public long adjustTimeStampToRobotClock(long timeStamp);
   
   public boolean offsetIsDetermined();

   public long adjustRobotTimeStampToRosClock(long timeStamp);
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/footstep/SimpleFootstepMask.java<|end_filename|>
package us.ihmc.humanoidRobotics.footstep;

import java.util.ArrayList;

import us.ihmc.euclid.referenceFrame.FrameConvexPolygon2D;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVertex2DSupplier;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotics.contactable.ContactablePlaneBody;
import us.ihmc.robotics.geometry.InclusionFunction;

public class SimpleFootstepMask implements InclusionFunction<Point3D>
{
   private final static boolean DEBUG = false;
   private ReferenceFrame yawFrame2d;
   private double safetyBuffer;
   private FrameConvexPolygon2D footPolygon;

   public SimpleFootstepMask(ReferenceFrame yawFrame2D, ContactablePlaneBody foot, double footKernelMaskSafetyBuffer)
   {
      this.yawFrame2d = yawFrame2D;
      this.safetyBuffer = footKernelMaskSafetyBuffer;
      ArrayList<FramePoint2D> contactPoints = new ArrayList<FramePoint2D>();
      for (FramePoint2D point : foot.getContactPoints2d())
      {
         contactPoints.add(new FramePoint2D(yawFrame2D, inflate(point.getX()), inflate(point.getY())));
      }

      footPolygon = new FrameConvexPolygon2D(FrameVertex2DSupplier.asFrameVertex2DSupplier(contactPoints));
      if (DEBUG)
         System.out.println("SimpleFootstepMask: footPolygon is \n" + footPolygon + " \nand yawFrame2d = \n"
                            + yawFrame2d.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame()));
   }

   private double inflate(double x)
   {
      return x + Math.signum(x) * safetyBuffer;
   }

   public boolean isIncluded(Point3D testInput)
   {
      FramePoint3D test = new FramePoint3D(ReferenceFrame.getWorldFrame(),testInput);
      if (DEBUG)
         System.out.printf("SimpleFootstepMask: testing point input is <%.2f,%.2f> in frame %s", test.getX(), test.getY(), test.getReferenceFrame());
      test.changeFrame(yawFrame2d);
      boolean isPointInside = footPolygon.isPointInside(new FramePoint2D(test));
      if (DEBUG)
         System.out.printf("SimpleFootstepMask: testing point <%.2f,%.2f> in frame %s. Point is in is %s.  ", test.getX(), test.getY(), test.getReferenceFrame(),isPointInside ? "true":"false");

      return isPointInside;
   }

}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/taskExecutor/PelvisTrajectoryTask.java<|end_filename|>
package us.ihmc.humanoidBehaviors.taskExecutor;

import controller_msgs.msg.dds.PelvisTrajectoryMessage;
import us.ihmc.humanoidBehaviors.behaviors.primitives.PelvisTrajectoryBehavior;
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;

public class PelvisTrajectoryTask extends BehaviorAction
{
   private final PelvisTrajectoryMessage pelvisTrajectoryMessage;
   private final PelvisTrajectoryBehavior pelvisTrajectoryBehavior;

   public PelvisTrajectoryTask(PelvisTrajectoryMessage pelvisTrajectoryMessage, PelvisTrajectoryBehavior pelvisTrajectoryBehavior)
   {
      super(pelvisTrajectoryBehavior);
      this.pelvisTrajectoryBehavior = pelvisTrajectoryBehavior;
      this.pelvisTrajectoryMessage = pelvisTrajectoryMessage;
   }

   @Override
   protected void setBehaviorInput()
   {
      pelvisTrajectoryBehavior.setInput(pelvisTrajectoryMessage);
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/net/ConnectionStateListener.java<|end_filename|>
package us.ihmc.communication.net;

public interface ConnectionStateListener
{
   public abstract void connected();
   
   public abstract void disconnected();
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/inputDevices/joystick/JoystickComponentFilter.java<|end_filename|>
package us.ihmc.tools.inputDevices.joystick;

import net.java.games.input.Component.Identifier;

public interface JoystickComponentFilter
{
   public double apply(double value);
   
   public Identifier getIdentifier();
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/main/ParameterGuiInterface.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.main;

import java.util.List;

import javafx.scene.Node;
import us.ihmc.parameterTuner.guiElements.GuiParameter;
import us.ihmc.parameterTuner.guiElements.GuiRegistry;

public interface ParameterGuiInterface
{
   /**
    * This will be called periodically by the GUI to check if the registry structure needs reloading.
    * If that is the case the GUI will call {@link #getRegistriesCopy()} and update all parameters.
    */
   public boolean pollReloadAll();

   /**
    * This will be called by the GUI to load the registry structure if a reload is requested through
    * {@link #pollReloadAll()}. The returned registry will be modified by the GUI.
    */
   public List<GuiRegistry> getRegistriesCopy();

   /**
    * This will be called periodically by the GUI if the user modifies any parameter. The list will
    * contain copies of all parameters that were changed by the user. The GUI will not hold on to a
    * reference of the changed parameters so they are safe to modify.
    */
   public void submitChangedParameters(List<GuiParameter> changedParameters);

   /**
    * This will be called periodically by the GUI to get a list with parameters that were updated by
    * other entities such as a different tuning GUI or a YoVariableServer. Note, that the GUI will
    * only update the value of the parameters. Description and suggested bounds will not be updated.
    */
   public List<GuiParameter> pollUpdatedParameters();

   /**
    * This can be used to provide the GUI with a interface for controlling the input manager. If the
    * value is null no node will be added. For example, in the case of a tuning GUI implementation
    * that uses files, this node could contain 'open' and 'save' buttons that will be added to the main
    * window.
    */
   public Node getInputManagerNode();

   /**
    * This will be called once by the GUI to inform the input manager that the program was shut down.
    * This will allow the input manager to close any YoVariableServers and clean up.
    */
   public void shutdown();

   /**
    * Allows the GUI to modify the root registry. This is used when the user would like to save an XML
    * file that uses a non-root registry as a new root. For example if the registry structure seen in
    * the GUI is "scs" - "atlas" - "controller" but the user of the GUI would like to save an XML file
    * for the controller.
    */
   public void changeRootRegistries(List<String> pollRootRegistryNames);
}


<|start_filename|>websitedocs/website/node_modules/docusaurus/lib/start-server.js<|end_filename|>
#!/usr/bin/env node

/**
 * Copyright (c) 2017-present, Facebook, Inc.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

require('babel-register')({
  babelrc: false,
  only: [__dirname, process.cwd() + '/core'],
  plugins: [
    require('./server/translate-plugin.js'),
    'transform-class-properties',
    'transform-object-rest-spread',
  ],
  presets: ['react', 'env'],
});

// For verifying port usage
const tcpPortUsed = require('tcp-port-used');

// initial check that required files are present
const chalk = require('chalk');
const fs = require('fs');
const openBrowser = require('react-dev-utils/openBrowser');
const CWD = process.cwd();
const env = require('./server/env.js');

if (!fs.existsSync(CWD + '/siteConfig.js')) {
  console.error(
    chalk.red('Error: No siteConfig.js file found in website folder!')
  );
  process.exit(1);
}

if (env.versioning.enabled && env.versioning.missingVersionsPage) {
  env.versioning.printMissingVersionsPageError();
  process.exit(1);
}

const program = require('commander');

program
  .option('--port <number>', 'Specify port number')
  .option('--no-watch', 'Toggle live reload file watching')
  .parse(process.argv);

let port = parseInt(program.port, 10) || process.env.PORT || 3000;
let numAttempts = 0;
const MAX_ATTEMPTS = 10;
checkPort();

function checkPort() {
  tcpPortUsed
    .check(port, 'localhost')
    .then(function(inUse) {
      if (inUse && numAttempts >= MAX_ATTEMPTS) {
        console.log(
          'Reached max attempts, exiting. Please open up some ports or ' +
            'increase the number of attempts and try again.'
        );
        process.exit(1);
      } else if (inUse) {
        console.error(chalk.red('Port ' + port + ' is in use'));
        // Try again but with port + 1
        port += 1;
        numAttempts += 1;
        checkPort();
      } else {
        // start local server on specified port
        const server = require('./server/server.js');
        server(port, program.opts());
        const {baseUrl} = require(CWD + '/siteConfig.js');
        const host = `http://localhost:${port}${baseUrl}`;
        console.log('Docusaurus server started on port %d', port);
        openBrowser(host);
      }
    })
    .catch(function(ex) {
      setTimeout(function() {
        throw ex;
      }, 0);
    });
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/dataobjects/HandJointName.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.dataobjects;

import us.ihmc.robotics.partNames.FingerName;
import us.ihmc.robotics.robotSide.RobotSide;

public interface HandJointName
{
   /**
    * Retrieves the name of the finger to which this joint belongs.
    * 
    * @param robotSide refers to which hand this joint belongs.
    * @return the finger name.
    */
   public FingerName getFinger(RobotSide robotSide);

   /**
    * Provides a side-dependent index for this joint that contained in [0, N[, where N is the number
    * of hand joints.
    * 
    * @param robotSide refers to which hand this joint belongs.
    * @return the index associated with this joint.
    */
   public int getIndex(RobotSide robotSide);

   /**
    * Get the {@code HandJointName} for all the finger joints for one hand.
    * 
    * @return the array containing all the joint names.
    */
   public HandJointName[] getValues();

   /**
    * Retrieves the joint name as a {@code String} that corresponds to the joint name as defined in
    * the robot definition, i.e. the SDF/URDF model or the {@code RobotDescription}.
    * 
    * @param robotSide refers to which hand this joint belongs.
    * @return the {@code String} of this joint name.
    */
   public String getJointName(RobotSide robotSide);
}


<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/joystick/GraspingJavaFXTopics.java<|end_filename|>
package us.ihmc.valkyrie.joystick;

import us.ihmc.avatar.joystickBasedJavaFXController.XBoxOneJavaFXController;
import us.ihmc.messager.MessagerAPIFactory;
import us.ihmc.messager.MessagerAPIFactory.Category;
import us.ihmc.messager.MessagerAPIFactory.CategoryTheme;
import us.ihmc.messager.MessagerAPIFactory.MessagerAPI;
import us.ihmc.messager.MessagerAPIFactory.Topic;
import us.ihmc.messager.MessagerAPIFactory.TypedTopicTheme;

public class GraspingJavaFXTopics
{
   private static final MessagerAPIFactory apiFactory = new MessagerAPIFactory();

   private static final Category Root = apiFactory.createRootCategory(apiFactory.createCategoryTheme("Grasping"));

   private static final CategoryTheme FingerControl = apiFactory.createCategoryTheme("FingerControl");

   private static final CategoryTheme Left = apiFactory.createCategoryTheme("Left");
   private static final CategoryTheme Right = apiFactory.createCategoryTheme("Right");
   private static final CategoryTheme Thumb = apiFactory.createCategoryTheme("Thumb");
   private static final CategoryTheme Index = apiFactory.createCategoryTheme("Index");
   private static final CategoryTheme Middle = apiFactory.createCategoryTheme("Middle");
   private static final CategoryTheme Pinky = apiFactory.createCategoryTheme("Pinky");

   private static final TypedTopicTheme<Double> Roll = apiFactory.createTypedTopicTheme("Roll");
   private static final TypedTopicTheme<Double> Pitch = apiFactory.createTypedTopicTheme("Pitch");
   private static final TypedTopicTheme<Double> Pitch2 = apiFactory.createTypedTopicTheme("Pitch2");
   private static final TypedTopicTheme<Boolean> SendMessage = apiFactory.createTypedTopicTheme("SendMessage");

   private static final TypedTopicTheme<Integer> Selected = apiFactory.createTypedTopicTheme("Selected");

   public static final Topic<Double> LeftThumbRoll = Root.child(FingerControl).child(Left).child(Thumb).topic(Roll);
   public static final Topic<Double> LeftThumb = Root.child(FingerControl).child(Left).child(Thumb).topic(Pitch);
   public static final Topic<Double> LeftThumb2 = Root.child(FingerControl).child(Left).child(Thumb).topic(Pitch2);
   public static final Topic<Double> LeftIndex = Root.child(FingerControl).child(Left).child(Index).topic(Pitch);
   public static final Topic<Double> LeftMiddle = Root.child(FingerControl).child(Left).child(Middle).topic(Pitch);
   public static final Topic<Double> LeftPinky = Root.child(FingerControl).child(Left).child(Pinky).topic(Pitch);
   public static final Topic<Double> RightThumbRoll = Root.child(FingerControl).child(Right).child(Thumb).topic(Roll);
   public static final Topic<Double> RightThumb = Root.child(FingerControl).child(Right).child(Thumb).topic(Pitch);
   public static final Topic<Double> RightThumb2 = Root.child(FingerControl).child(Right).child(Thumb).topic(Pitch2);
   public static final Topic<Double> RightIndex = Root.child(FingerControl).child(Right).child(Index).topic(Pitch);
   public static final Topic<Double> RightMiddle = Root.child(FingerControl).child(Right).child(Middle).topic(Pitch);
   public static final Topic<Double> RightPinky = Root.child(FingerControl).child(Right).child(Pinky).topic(Pitch);
   public static final Topic<Boolean> LeftSendMessage = Root.child(FingerControl).child(Left).topic(SendMessage);
   public static final Topic<Boolean> RightSendMessage = Root.child(FingerControl).child(Right).topic(SendMessage);

   static
   {
      apiFactory.includeMessagerAPIs(XBoxOneJavaFXController.XBoxOneControllerAPI);
   }

   public static final MessagerAPI API = apiFactory.getAPIAndCloseFactory();
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SimulatedLidarScanPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "SimulatedLidarScanPacket" defined in "SimulatedLidarScanPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from SimulatedLidarScanPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit SimulatedLidarScanPacket_.idl instead.
*
*/
public class SimulatedLidarScanPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.SimulatedLidarScanPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::SimulatedLidarScanPacket_";

   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.SimulatedLidarScanPacket 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.SimulatedLidarScanPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);current_alignment += (100 * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += controller_msgs.msg.dds.LidarScanParametersMessagePubSubType.getMaxCdrSerializedSize(current_alignment);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.SimulatedLidarScanPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.SimulatedLidarScanPacket data, int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);
      current_alignment += (data.getRanges().size() * 4) + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);


      current_alignment += controller_msgs.msg.dds.LidarScanParametersMessagePubSubType.getCdrSerializedSize(data.getLidarScanParameters(), current_alignment);


      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.SimulatedLidarScanPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      if(data.getRanges().size() <= 100)
      cdr.write_type_e(data.getRanges());else
          throw new RuntimeException("ranges field exceeds the maximum length");

      cdr.write_type_2(data.getSensorId());

      controller_msgs.msg.dds.LidarScanParametersMessagePubSubType.write(data.getLidarScanParameters(), cdr);
   }

   public static void read(controller_msgs.msg.dds.SimulatedLidarScanPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      cdr.read_type_e(data.getRanges());	
      data.setSensorId(cdr.read_type_2());
      	
      controller_msgs.msg.dds.LidarScanParametersMessagePubSubType.read(data.getLidarScanParameters(), cdr);	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.SimulatedLidarScanPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_e("ranges", data.getRanges());
      ser.write_type_2("sensor_id", data.getSensorId());
      ser.write_type_a("lidar_scan_parameters", new controller_msgs.msg.dds.LidarScanParametersMessagePubSubType(), data.getLidarScanParameters());

   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.SimulatedLidarScanPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      ser.read_type_e("ranges", data.getRanges());
      data.setSensorId(ser.read_type_2("sensor_id"));
      ser.read_type_a("lidar_scan_parameters", new controller_msgs.msg.dds.LidarScanParametersMessagePubSubType(), data.getLidarScanParameters());

   }

   public static void staticCopy(controller_msgs.msg.dds.SimulatedLidarScanPacket src, controller_msgs.msg.dds.SimulatedLidarScanPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.SimulatedLidarScanPacket createData()
   {
      return new controller_msgs.msg.dds.SimulatedLidarScanPacket();
   }
   @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.SimulatedLidarScanPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.SimulatedLidarScanPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.SimulatedLidarScanPacket src, controller_msgs.msg.dds.SimulatedLidarScanPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public SimulatedLidarScanPacketPubSubType newInstance()
   {
      return new SimulatedLidarScanPacketPubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/lists/FrameTuple2dArrayList.java<|end_filename|>
package us.ihmc.robotics.lists;

import java.util.List;

import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.euclid.referenceFrame.FramePoint2D;
import us.ihmc.euclid.referenceFrame.FrameVector2D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DBasics;
import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;

public class FrameTuple2dArrayList<T extends FrameTuple2DBasics> extends RecyclingArrayList<T>
{
   private FrameTuple2dArrayList(Class<T> clazz)
   {
      super(clazz);
   }

   private FrameTuple2dArrayList(int initialSize, Class<T> clazz)
   {
      super(initialSize, clazz);
   }

   public void setOrCreate(int i, FrameTuple2DReadOnly frameTuple2d)
   {
      getAndGrowIfNeeded(i).setIncludingFrame(frameTuple2d);
   }

   public void set(int i, FrameTuple2DReadOnly frameTuple2d)
   {
      get(i).setIncludingFrame(frameTuple2d);
   }

   private void unsafeSet(int i, FrameTuple2DReadOnly frameTuple2d)
   {
      unsafeGet(i).setIncludingFrame(frameTuple2d);
   }

   private void unsafeSet(int i, ReferenceFrame referenceFrame, Tuple2DReadOnly tuple2d)
   {
      unsafeGet(i).setIncludingFrame(referenceFrame, tuple2d);
   }

   public void copyFromListAndTrimSize(FrameTuple2dArrayList<?> otherList)
   {
      for (int i = 0; i < otherList.size(); i++)
      {
         getAndGrowIfNeeded(i).setIncludingFrame(otherList.get(i));
      }

      while(size() > otherList.size())
      {
         remove(size() - 1);
      }
   }

   public void copyFromListAndTrimSize(List<? extends FrameTuple2DReadOnly> otherList)
   {
      for (int i = 0; i < otherList.size(); i++)
      {
         getAndGrowIfNeeded(i).setIncludingFrame(otherList.get(i));
      }

      while(size() > otherList.size())
      {
         remove(size() - 1);
      }
   }

   public void copyFromPoint2dListAndTrimSize(ReferenceFrame referenceFrame, List<? extends Tuple2DReadOnly> otherList)
   {
      for (int i = 0; i < otherList.size(); i++)
      {
         getAndGrowIfNeeded(i).setIncludingFrame(referenceFrame, otherList.get(i));
      }

      while(size() > otherList.size())
      {
         remove(size() - 1);
      }
   }

   public static FrameTuple2dArrayList<FramePoint2D> createFramePoint2dArrayList()
   {
      return new FrameTuple2dArrayList<>(FramePoint2D.class);
   }

   public static FrameTuple2dArrayList<FrameVector2D> createFrameVector2dArrayList()
   {
      return new FrameTuple2dArrayList<>(FrameVector2D.class);
   }

   public static FrameTuple2dArrayList<FramePoint2D> createFramePoint2dArrayList(int initialSize)
   {
      return new FrameTuple2dArrayList<>(initialSize, FramePoint2D.class);
   }

   public static FrameTuple2dArrayList<FrameVector2D> createFrameVector2dArrayList(int initialSize)
   {
      return new FrameTuple2dArrayList<>(initialSize, FrameVector2D.class);
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/walkingpath/rrtplanner/RRT2DPlannerWalkingPath.java<|end_filename|>
package us.ihmc.manipulation.planning.walkingpath.rrtplanner;

import us.ihmc.commons.PrintTools;
import us.ihmc.manipulation.planning.rrt.RRTNode;
import us.ihmc.manipulation.planning.rrt.RRTPlanner;
import us.ihmc.manipulation.planning.walkingpath.footstep.SkeletonPathFootStepPlanner;

public class RRT2DPlannerWalkingPath extends RRTPlanner
{
   SkeletonPathFootStepPlanner footStepPlanner;

   public RRT2DPlannerWalkingPath(RRTNode root, RRTNode goal, double stepLength)
   {
      super(root, goal, stepLength);
   }

   public boolean expandTreeGoal(RRTNode nodeOne, RRTNode nodeTwo)
   {
      if (getRRTTree().expandTree() == true)
      {
         for (int i = 0; i < getRRTTree().getNewNode().getDimensionOfNodeData(); i++)
         {
            nodeOne.setNodeData(i, getRRTTree().getNewNode().getNodeData(i));
            nodeTwo.setNodeData(i, getRRTTree().getNearNode().getNodeData(i));
         }

         if (getRRTTree().getNewNode().getDistance(getGoalNode()) < getRRTTree().getStepLength())
         {
            getRRTTree().getNewNode().addChildNode(getGoalNode());
            getRRTTree().updatePathNode(getGoalNode());
            setOptimalPath(getRRTTree().getPathNode());

            PrintTools.info("path size is " + getOptimalPath().size());
            return true;
         }
         return false;
      }
      return false;
   }

   public void createFootStepPlanner(double stepLength, double stepWidth)
   {
      double[] pathX = new double[getOptimalPath().size()];
      double[] pathY = new double[getOptimalPath().size()];

      for (int i = 0; i < getOptimalPath().size(); i++)
      {
         pathX[i] = getOptimalPath().get(i).getNodeData(0);
         pathY[i] = getOptimalPath().get(i).getNodeData(1);
      }
      footStepPlanner = new SkeletonPathFootStepPlanner(pathX, pathY, stepLength, stepWidth);
   }

   public SkeletonPathFootStepPlanner getFootStepPlanner()
   {
      return footStepPlanner;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/Point2dInConvexPolygon2d.java<|end_filename|>
package us.ihmc.robotics.geometry;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.Line2D;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple2D.Vector2D;
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;

/**
 * A Point2d in ConvexPolygon coordinate is defined by  (eccentricity, angle) similar to the polar coordinate system
 * However, eccentricity defines the relative distance to origin w.r.t. maximum possible distance on the same epipolar line within the polygon.
 * Essentially, eccentricity = 1 means the point is on the polygon boundary.
 * @author tingfan
 *
 */
public class Point2dInConvexPolygon2d extends Point2D
{
   private static final long serialVersionUID = 5818978949209007789L;
   protected  ConvexPolygon2D polygon;
   private final Point2D origin = new Point2D(0,0);


   public Point2dInConvexPolygon2d(ConvexPolygon2D polygon, double x, double y)
   {
      super(x,y);
      this.polygon = polygon;
   }

   public boolean isPointInsidePolygon()
   {
      return polygon.isPointInside(this);
   }


   public void setEccentricity(double r)
   {
      r=Math.max(r, 1e-5);
      double e = getEccentricity();
      scale(r/e);
   }

   public void setAngle(double angle)
   {
      Point2DBasics point = findEdgePoint(angle);
      point.scale(getEccentricity());
      set(point);
   }


      public double getAngle()
   {
      return Math.atan2(getY(),getX());
   }

   public double getEccentricity()
   {
      Point2DBasics edgePoint = findEdgePoint(getX(), getY());
      return Math.max(1e-3, distance(origin) / edgePoint.distance(origin));
   }

   private Point2DBasics findEdgePoint(double angle)
   {
      return findEdgePoint(Math.cos(angle), Math.sin(angle));
   }

   private Point2DBasics findEdgePoint(double x, double y)
   {
      if (x==0 && y==0)
         x=1; //as eccentricity=0
      Line2D ray = new Line2D(new Point2D(0,0), new Vector2D(x,y));
      Point2DBasics[] edgePoints = polygon.intersectionWithRay(ray);
      if(edgePoints.length!=1)
         throw new RuntimeException("intersecting points should be 1, but we get" + edgePoints.length);
      return edgePoints[0];
   }

}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/walkingpath/footstep/SkeletonPath.java<|end_filename|>
package us.ihmc.manipulation.planning.walkingpath.footstep;

import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.ArrayList;

public class SkeletonPath
{
   private ArrayList<Line2D> pathSegments = new ArrayList<Line2D>();
   private Point2D finalPoint;

   public SkeletonPath()
   {

   }

   public SkeletonPath(double[] skeletonPathX, double[] skeletonPathY)
   {
      for (int i = 0; i < skeletonPathX.length - 1; i++)
      {
         Line2D.Double aSegment = new Line2D.Double(skeletonPathX[i], skeletonPathY[i], skeletonPathX[i + 1], skeletonPathY[i + 1]);
         pathSegments.add(aSegment);
      }
   }

   public SkeletonPath(double shiftY, SkeletonPath refSegments)
   {
      Line2D aLine;
      for (int i = 0; i < refSegments.get().size(); i++)
      {
         aLine = getShiftedSegment(shiftY, refSegments.get().get(i));
         pathSegments.add(aLine);
      }
   }

   public ArrayList<Line2D> get()
   {
      return pathSegments;
   }

   public Line2D getSegment(int index)
   {
      return pathSegments.get(index);
   }

   private Line2D getShiftedSegment(double delY, Line2D refLine)
   {
      Line2D retLine;

      double diffX = refLine.getX2() - refLine.getX1();
      double diffY = refLine.getY2() - refLine.getY1();

      double normDiff = Math.sqrt(diffX * diffX + diffY * diffY);

      double shiftX, shiftY;

      shiftX = -diffY / normDiff * delY;
      shiftY = diffX / normDiff * delY;

      retLine = new Line2D.Double(refLine.getX1() + shiftX, refLine.getY1() + shiftY, refLine.getX2() + shiftX, refLine.getY2() + shiftY);

      return retLine;
   }

   public int getIndexOfClosestSegment(Point2D aPoint)
   {
      int indexOfClosestSegment = 0;

      double minMatric = Double.MAX_VALUE;
      double curMatric = minMatric;

      for (int i = 0; i < pathSegments.size(); i++)
      {
         curMatric = pathSegments.get(i).ptSegDist(aPoint);
         if (curMatric < minMatric)
         {
            minMatric = curMatric;
            indexOfClosestSegment = i;
         }
      }

      return indexOfClosestSegment;
   }

   private Point2D getLocationOfClosestPointOnSegment(int indexOfSegment, Point2D aPoint)
   {
      Point2D ret = new Point2D.Double();

      Line2D aSegment = pathSegments.get(indexOfSegment);

      double paramA = aSegment.getY2() - aSegment.getY1();
      double paramB = -aSegment.getX2() + aSegment.getX1();
      double paramC = -aSegment.getX1() * aSegment.getY2() + aSegment.getX2() * aSegment.getY1();

      ret.setLocation((paramB * (paramB * aPoint.getX() - paramA * aPoint.getY()) - paramA * paramC) / (paramA * paramA + paramB * paramB),
                      (paramA * (-paramB * aPoint.getX() + paramA * aPoint.getY()) - paramB * paramC) / (paramA * paramA + paramB * paramB));

      if (aSegment.ptSegDist(ret) > 0.0001)
      {
         if (aSegment.getP1().distance(ret) > aSegment.getP2().distance(ret))
         {
            ret = aSegment.getP2();
         }
         else
         {
            ret = aSegment.getP1();
         }
      }

      return ret;
   }

   public Point2D getLocationOfClosestPointOnSegment(Point2D aPoint)
   {
      int indexOfSegment = getIndexOfClosestSegment(aPoint);

      return getLocationOfClosestPointOnSegment(indexOfSegment, aPoint);
   }

   public double getYawAngleOfClosestPointOnSegment(Point2D aPoint)
   {
      int indexOfSegment = getIndexOfClosestSegment(aPoint);

      return getYawAngleOfSegment(indexOfSegment);
   }

   public double getYawAngleOfSegment(int indexOfSegment)
   {
      double diffX = pathSegments.get(indexOfSegment).getX2() - pathSegments.get(indexOfSegment).getX1();
      double diffY = pathSegments.get(indexOfSegment).getY2() - pathSegments.get(indexOfSegment).getY1();

      double norm = Math.sqrt(diffX * diffX + diffY * diffY);

      double alpha;

      if (diffX >= 0)
      {
         alpha = Math.asin(diffY / norm);
      }
      else
      {
         if (diffY >= 0)
         {
            alpha = -Math.asin(diffY / norm) + Math.PI;
         }
         else
         {
            alpha = -Math.asin(diffY / norm) - Math.PI;
         }
      }

      return alpha;
   }

   public Point2D getFinalPoint()
   {
      finalPoint = new Point2D.Double();
      finalPoint.setLocation(pathSegments.get(pathSegments.size() - 1).getP2());
      return finalPoint;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/ManualHandControlPacket.java<|end_filename|>
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;

/**
       * Atlas specific message
       */
public class ManualHandControlPacket extends Packet<ManualHandControlPacket> implements Settable<ManualHandControlPacket>, EpsilonComparable<ManualHandControlPacket>
{
   public static final byte ROBOT_SIDE_LEFT = (byte) 0;
   public static final byte ROBOT_SIDE_RIGHT = (byte) 1;
   public static final int VELOCITY = 0;
   public static final int POSITION = 1;
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public byte robot_side_ = (byte) 255;
   public double index_;
   public double middle_;
   public double thumb_;
   public double spread_;
   public int control_type_;

   public ManualHandControlPacket()
   {
   }

   public ManualHandControlPacket(ManualHandControlPacket other)
   {
      this();
      set(other);
   }

   public void set(ManualHandControlPacket other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      index_ = other.index_;

      middle_ = other.middle_;

      thumb_ = other.thumb_;

      spread_ = other.spread_;

      control_type_ = other.control_type_;

   }

   /**
            * 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 void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   public byte getRobotSide()
   {
      return robot_side_;
   }

   public void setIndex(double index)
   {
      index_ = index;
   }
   public double getIndex()
   {
      return index_;
   }

   public void setMiddle(double middle)
   {
      middle_ = middle;
   }
   public double getMiddle()
   {
      return middle_;
   }

   public void setThumb(double thumb)
   {
      thumb_ = thumb;
   }
   public double getThumb()
   {
      return thumb_;
   }

   public void setSpread(double spread)
   {
      spread_ = spread;
   }
   public double getSpread()
   {
      return spread_;
   }

   public void setControlType(int control_type)
   {
      control_type_ = control_type;
   }
   public int getControlType()
   {
      return control_type_;
   }


   public static Supplier<ManualHandControlPacketPubSubType> getPubSubType()
   {
      return ManualHandControlPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return ManualHandControlPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(ManualHandControlPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.index_, other.index_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.middle_, other.middle_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.thumb_, other.thumb_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.spread_, other.spread_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.control_type_, other.control_type_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof ManualHandControlPacket)) return false;

      ManualHandControlPacket otherMyClass = (ManualHandControlPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if(this.index_ != otherMyClass.index_) return false;

      if(this.middle_ != otherMyClass.middle_) return false;

      if(this.thumb_ != otherMyClass.thumb_) return false;

      if(this.spread_ != otherMyClass.spread_) return false;

      if(this.control_type_ != otherMyClass.control_type_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("ManualHandControlPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("index=");
      builder.append(this.index_);      builder.append(", ");
      builder.append("middle=");
      builder.append(this.middle_);      builder.append(", ");
      builder.append("thumb=");
      builder.append(this.thumb_);      builder.append(", ");
      builder.append("spread=");
      builder.append(this.spread_);      builder.append(", ");
      builder.append("control_type=");
      builder.append(this.control_type_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/DoNothingControllerStateFactory.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.DoNothingControllerState;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;

public class DoNothingControllerStateFactory implements HighLevelControllerStateFactory
{
   private DoNothingControllerState doNothingControllerState;

   @Override
   public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper)
   {
      if (doNothingControllerState == null)
      {
         doNothingControllerState = new DoNothingControllerState(controllerFactoryHelper.getHighLevelHumanoidControllerToolbox().getControlledOneDoFJoints(),
                                                                 controllerFactoryHelper.getHighLevelControllerParameters());
      }

      return doNothingControllerState;
   }

   @Override
   public HighLevelControllerName getStateEnum()
   {
      return HighLevelControllerName.DO_NOTHING_BEHAVIOR;
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/ROSMessageFileCreator.java<|end_filename|>
package us.ihmc.utilities.ros;

import org.apache.commons.lang3.text.WordUtils;

import us.ihmc.commons.PrintTools;
import us.ihmc.communication.ros.generators.*;
import us.ihmc.utilities.ros.msgToPacket.converter.GenericROSTranslationTools;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.lang.reflect.Field;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.Set;

public class ROSMessageFileCreator
{
   private static final int WRAP_LENGTH = 100;
   boolean overwriteSubMessages;

   public ROSMessageFileCreator(boolean overwriteSubMessages)
   {
      this.overwriteSubMessages = overwriteSubMessages;
   }

   public String createNewRosMessageFromGenerator(RosCustomGenerator generator, boolean overwrite) throws Exception
   {
      String messageName = generator.getMessageName();
      Path msgDirectoryPath = Paths.get("..", "ihmc-ros-tools", "ROSMessagesAndServices", generator.getRosPackage(), "msg");
      if(!Files.exists(msgDirectoryPath))
      {
         Files.createDirectories(msgDirectoryPath);
      }

      File messageFile = msgDirectoryPath.resolve(messageName + ".msg").toFile();

      if (overwrite ||  !messageFile.exists())
      {
         StringBuilder fileContents = new StringBuilder();
         try(FileWriter writer = new FileWriter(messageFile))
         {
            addTypeDocumentationToFileContents(generator.getTypeDocumentation(), messageName, fileContents);

            for (RosFieldDefinition rosFieldDefinition : generator.getFields())
            {
               cleanupAndLineWrapDocumentation(fileContents, rosFieldDefinition.getDocumentation());
               fileContents.append(rosFieldDefinition.getType()).append(" ").append(rosFieldDefinition.getFieldName()); //.append("\n\n");
               if(rosFieldDefinition.isConstant())
               {
                  fileContents.append("=").append(rosFieldDefinition.getConstantValue());
               }

               fileContents.append("\n\n");
            }

            writer.append(fileContents.toString());
         }
         catch (IOException e)
         {
            e.printStackTrace();
         }
      }
      return messageName;
   }

   @SuppressWarnings("unchecked")
   public String createNewRosMessage(Class clazz, boolean overwrite) throws Exception
   {
      RosMessagePacket rosMessageAnnotation = (RosMessagePacket) clazz.getAnnotation(RosMessagePacket.class);
      String messageName = clazz.getSimpleName();

      Path msgDirectoryPath = Paths.get("..", "ihmc-ros-tools", "ROSMessagesAndServices", rosMessageAnnotation.rosPackage(), "msg");
      if(!Files.exists(msgDirectoryPath))
      {
         Files.createDirectories(msgDirectoryPath);
      }

      messageName = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(messageName);

      File messageFile = msgDirectoryPath.resolve(messageName + ".msg").toFile();

      if (overwrite ||  !messageFile.exists())
      {
         messageFile.delete();

         try(FileWriter writer = new FileWriter(messageFile))
         {
            messageFile.createNewFile();
//            PrintTools.info("Created empty msg file: " + messageFile.getCanonicalPath());

            StringBuilder fileContents = new StringBuilder();

            addTypeDocumentationToFileContents(rosMessageAnnotation.documentation(), messageName, fileContents);

            fileContents.append("\n");

            ArrayList<Field> exportedFields = new ArrayList<>();
            Set<Class<? extends Enum>> exportedEnumClasses = new HashSet<>();

            gatherExportedFieldsAndEnums(clazz, exportedFields, exportedEnumClasses);

            addFieldsToFileContents(fileContents, exportedFields);

            fileContents.append("\n");

            addEnumDocumentationToFileContents(fileContents, exportedEnumClasses);

            writer.write(fileContents.toString());
         }
         catch (IOException e)
         {
            e.printStackTrace();
         }
      }

      return messageName;
   }

   private void gatherExportedFieldsAndEnums(Class clazz, ArrayList<Field> exportedFields, Set<Class<? extends Enum>> exportedEnumClasses)
   {
      for (Field field : clazz.getFields())
      {
         if(field.isAnnotationPresent(RosExportedField.class))
         {
            if(Enum.class.isAssignableFrom(field.getType()))
            {
               exportedEnumClasses.add((Class<? extends Enum>) field.getType());
            }

            exportedFields.add(field);
         }
      }
   }

   private void addFieldsToFileContents(StringBuilder fileContents, Collection<Field> exportedFields) throws IOException
   {
      for (Field exportedField : exportedFields)
      {
         cleanupAndLineWrapDocumentation(fileContents, exportedField.getAnnotation(RosExportedField.class).documentation());
         String rosTypeForJavaType = GenericROSTranslationTools.getRosTypeForJavaType(exportedField, exportedField.getType());
         System.out.println("Received ros type of " + rosTypeForJavaType + " for field " + exportedField.getName() + " declared in " + exportedField
               .getDeclaringClass().getSimpleName());
         fileContents.append(rosTypeForJavaType).append(" ").append(camelCaseToLowerCaseWithUnderscores(exportedField.getName())).append("\n\n");
      }
   }

   @SuppressWarnings("unchecked")
   private void addEnumDocumentationToFileContents(StringBuilder fileContents, Set<Class<? extends Enum>> exportedEnumClasses) throws IOException
   {
      if(!exportedEnumClasses.isEmpty())
      {
         fileContents.append("# This message utilizes \"enums\". Enum value information for this message follows.").append("\n\n");
      }

      for (Class<? extends Enum> exportedEnumClass : exportedEnumClasses)
      {

         fileContents.append("# \"").append(camelCaseToLowerCaseWithUnderscores(exportedEnumClass.getSimpleName())).append("\" enum values:").append("\n");

         ArrayList<Field> documentedFields = getExplicitlyDocumentedEnumConstants(exportedEnumClass);

         if(!documentedFields.isEmpty())
         {
            addExplicitlyDocumentedEnumToFileContents(fileContents, exportedEnumClass, documentedFields);
         }
         else if(GenericROSTranslationTools.hasDocumentation(exportedEnumClass))
         {
            addImplicitlyDocumentedEnumToFileContents(fileContents, exportedEnumClass);
         }
         else
         {
            PrintTools.error(ROSMessageFileCreator.class, "Could not find any documentation for the enum values of " + exportedEnumClass.getCanonicalName()
            + " even though it is ROS Exported Field. Excluding from generated files. Please add documentation to the enum values. If this is a low-level/implicitly documented enum, consult ROSMessageConversionTools.java's getDocumentation() method.");
         }

         fileContents.append("\n");
      }
   }

   private ArrayList<Field> getExplicitlyDocumentedEnumConstants(Class<? extends Enum> exportedEnumClass)
   {
      ArrayList<Field> documentedFields = new ArrayList<>();
      for (Field field : exportedEnumClass.getFields())
      {
         if(field.isEnumConstant() && field.isAnnotationPresent(RosEnumValueDocumentation.class))
         {
            documentedFields.add(field);
         }
      }
      return documentedFields;
   }

   private void addImplicitlyDocumentedEnumToFileContents(StringBuilder fileContents, Class<? extends Enum> exportedEnumClass)
   {
      Enum[] enumConstants = exportedEnumClass.getEnumConstants();
      for (Enum enumConstant : enumConstants)
      {
         fileContents.append("uint8 ").append(enumConstant.name()).append("=");
         String enumDocumentation = GenericROSTranslationTools.getDocumentation(exportedEnumClass, enumConstant);
         fileContents.append(enumConstant.ordinal()).append(" ");
         fileContents.append("# ").append(enumDocumentation).append("\n");
      }
   }

   private void addExplicitlyDocumentedEnumToFileContents(StringBuilder fileContents, Class<? extends Enum> exportedEnumClass,
         ArrayList<Field> documentedFields)
   {
      for (Field documentedField : documentedFields)
      {
         fileContents.append("uint8 ").append(documentedField.getName()).append("=");
         for (Enum anEnum : exportedEnumClass.getEnumConstants())
         {
            if(anEnum.name().equals(documentedField.getName()))
            {
               RosEnumValueDocumentation annotation = documentedField.getAnnotation(RosEnumValueDocumentation.class);
               fileContents.append(anEnum.ordinal()).append(" ");
               fileContents.append("# ").append(annotation.documentation()).append("\n");
               break;
            }
         }
      }
   }

   private void addTypeDocumentationToFileContents(String typeDocumentation, String messageName, StringBuilder fileContents)
   {
      fileContents.append("## ").append(messageName).append("\n");

      cleanupAndLineWrapDocumentation(fileContents, typeDocumentation);
   }

   private void cleanupAndLineWrapDocumentation(StringBuilder fileContents, String documentation)
   {
      String[] documentationLines = WordUtils.wrap(documentation.replace("\n", " ").replace("\r", " ").replace("\r\n", " "), WRAP_LENGTH).split(System.lineSeparator());

      for (String line : documentationLines)
      {
         fileContents.append("# ").append(line).append("\n");
      }
   }

   private String camelCaseToLowerCaseWithUnderscores(String in)
   {
      String regex = "([a-z0-9])([A-Z]+)";
      String replacement = "$1_$2";

      String inWithUnderscores = in.replaceAll(regex, replacement);
      String ret = inWithUnderscores.toLowerCase();

      return ret;
   }
}


<|start_filename|>ihmc-ros-tools/src/main/java/us/ihmc/utilities/ros/publisher/RosPoint2dPublisher.java<|end_filename|>
package us.ihmc.utilities.ros.publisher;

import ihmc_msgs.Point2dRosMessage;
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;

/**
 *
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class RosPoint2dPublisher extends RosTopicPublisher<Point2dRosMessage>
{
   public RosPoint2dPublisher(boolean latched)
   {
      super(Point2dRosMessage._TYPE, latched);
   }

   public void publish(float x, float y)
   {
      Point2dRosMessage message = getMessage();
      message.setX(x);
      message.setY(y);
      publish(message);
   }

   public void publish(Point2DBasics point2d)
   {
      publish((float) point2d.getX(), (float) point2d.getY());
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/ground/steppingStones/SteppingStone.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.ground.steppingStones;

import java.util.ArrayList;
import java.util.Random;

import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.geometry.interfaces.Vertex2DSupplier;
import us.ihmc.euclid.tools.EuclidCoreRandomTools;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.graphicsDescription.Graphics3DObject;
import us.ihmc.graphicsDescription.appearance.AppearanceDefinition;
import us.ihmc.graphicsDescription.appearance.YoAppearance;
import us.ihmc.robotics.geometry.ConvexPolygonTools;

public class SteppingStone
{
   private final String name;

   private final double baseZ;
   private final double height;
   private final ConvexPolygon2D convexPolygon2d;
   private final ConvexPolygon2D shrunkenPolygon = new ConvexPolygon2D();

   private final ConvexPolygon2D polygonToShrink;

   public SteppingStone(String name, double baseZ, double height, ConvexPolygon2D convexPolygon2d, ConvexPolygon2D footPolygon)
   {
      this.name = name;
      this.baseZ = baseZ;
      this.height = height;
      this.convexPolygon2d = new ConvexPolygon2D(convexPolygon2d);
      polygonToShrink = new ConvexPolygon2D(footPolygon);
      new ConvexPolygonTools().shrinkInto(polygonToShrink, new Point2D(0.0, 0.0), convexPolygon2d, shrunkenPolygon);
   }

   public SteppingStone(String name, double baseZ, double height, ArrayList<Point2D> points, ConvexPolygon2D footPolygon)
   {
      this.name = name;
      this.baseZ = baseZ;
      this.height = height;
      convexPolygon2d = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points));
      polygonToShrink = new ConvexPolygon2D(footPolygon);
      new ConvexPolygonTools().shrinkInto(polygonToShrink, new Point2D(0.0, 0.0), convexPolygon2d, shrunkenPolygon);
   }

   public String getName()
   {
      return name;
   }

   public double getHeight()
   {
      return height;
   }

   public ConvexPolygon2D getConvexPolygon2d()
   {
      return convexPolygon2d;
   }

   public ConvexPolygon2D getShrunkenConvexPolygon2d()
   {
      return shrunkenPolygon;
   }


   public Graphics3DObject createLinkGraphics(AppearanceDefinition yoAppearance)
   {
      Graphics3DObject ret = new Graphics3DObject();
      double polygonExtrusionHeight = height-baseZ;
      ret.translate(new Vector3D(0.0, 0.0, baseZ));
      ret.addExtrudedPolygon(convexPolygon2d, polygonExtrusionHeight, yoAppearance);
            
      if (this.shrunkenPolygon != null)
      {
         //      ret.translate(new Vector3d(0.0, 0.0, baseZ));
         ret.addExtrudedPolygon(shrunkenPolygon, polygonExtrusionHeight + 0.001, YoAppearance.DarkGreen());
      }
      
      return ret;
   }


   public boolean intersectsLocation(double x, double y)
   {
      if (convexPolygon2d.isPointInside(x, y))
         return true;


      return false;
   }

   public static SteppingStone generateRandomCicularStone(String name, Random random, double xCenter, double yCenter, double baseZ, double height,
           double radius, ConvexPolygon2D shrunkenPolygon)
   {
      ArrayList<Point2D> points = generateRandomCircularPoints(xCenter, yCenter, radius, 40);
      ConvexPolygon2D polygon2d = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(points));
      SteppingStone steppingStone = new SteppingStone(name, baseZ, height, polygon2d, shrunkenPolygon);

      return steppingStone;
   }

   public static SteppingStone createRectangularStone(String name, double xMin, double xMax, double yMin, double yMax, double baseZ, double height,
           ConvexPolygon2D shrunkenPolygon)
   {
      ArrayList<Point2D> points = generateRectangularPoints(xMin, xMax, yMin, yMax);
      SteppingStone steppingStone = new SteppingStone(name, baseZ, height, points, shrunkenPolygon);

      return steppingStone;
   }


   public static SteppingStone generateRandomPolygonalStone(String name, Random random, double xCenter, double yCenter, double baseZ, double height,
           double radius, int numSides, ConvexPolygon2D shrunkenPolygon)
   {
      ArrayList<Point2D> points = generateRandomPolygonalPoints(random, xCenter, yCenter, radius, numSides);
      SteppingStone steppingStone = new SteppingStone(name, baseZ, height, points, shrunkenPolygon);

      return steppingStone;
   }

   public static SteppingStone generateRegularPolygonalStone(String name, double xCenter, double yCenter, double baseZ, double height, double radius,
           int numSides, ConvexPolygon2D shrunkenPolygon)
   {
      ArrayList<Point2D> points = generateRegularPolygonalPoints(xCenter, yCenter, radius, numSides);
      SteppingStone steppingStone = new SteppingStone(name, baseZ, height, points, shrunkenPolygon);

      return steppingStone;
   }

   private static ArrayList<Point2D> generateRegularPolygonalPoints(double xCenter, double yCenter, double radius, int numberOfPoints)
   {
      ArrayList<Point2D> points = new ArrayList<Point2D>();
      double alpha = (2.0 * Math.PI) / (numberOfPoints);
      Point2D zeroPoint = new Point2D(xCenter, yCenter);
      Point2D firstPoint = new Point2D(xCenter + radius, yCenter);
      points.add(firstPoint);

      for (int i = 1; i < numberOfPoints; i++)
      {
         double x = Math.cos(alpha) * (firstPoint.getX() - zeroPoint.getX()) - Math.sin(alpha) * (firstPoint.getY() - zeroPoint.getY()) + zeroPoint.getX();
         double y = Math.sin(alpha) * (firstPoint.getX() - zeroPoint.getX()) + Math.cos(alpha) * (firstPoint.getY() - zeroPoint.getY()) + zeroPoint.getY();
         Point2D nextPoint = new Point2D(x, y);
         points.add(nextPoint);
         firstPoint = nextPoint;
      }

      return points;
   }

   private static ArrayList<Point2D> generateRandomPolygonalPoints(Random random, double xCenter, double yCenter, double radius, int numberOfPoints)
   {
      ArrayList<Point2D> points = new ArrayList<Point2D>();
      double alpha = (2.0 * Math.PI) / (numberOfPoints);
      Point2D zeroPoint = new Point2D(xCenter, yCenter);
      Point2D firstPoint = new Point2D(xCenter + radius, yCenter);
      points.add(firstPoint);
      double minAngle = 0.0, maxAngle = alpha;

      for (int i = 1; i < numberOfPoints; i++)
      {
         double randonAngle = randomDoubleInRange(random, minAngle, maxAngle);
         double x = Math.cos(randonAngle) * (firstPoint.getX() - zeroPoint.getX()) - Math.sin(randonAngle) * (firstPoint.getY() - zeroPoint.getY())
                    + zeroPoint.getX();
         double y = Math.sin(randonAngle) * (firstPoint.getX() - zeroPoint.getX()) + Math.cos(randonAngle) * (firstPoint.getY() - zeroPoint.getY())
                    + zeroPoint.getY();
         Point2D nextPoint = new Point2D(x, y);
         points.add(nextPoint);
         minAngle = maxAngle;
         maxAngle = maxAngle + alpha;
      }

      return points;
   }

   private static ArrayList<Point2D> generateRandomCircularPoints(double xCenter, double yCenter, double radius, int numberOfPoints)
   {
      ArrayList<Point2D> points = new ArrayList<Point2D>();

      Random random = new Random(1972L);

      Point2D zeroFramePoint = new Point2D(xCenter, yCenter);

      double xMin = xCenter - radius;
      double xMax = xCenter + radius;
      double yMin = yCenter - radius;
      double yMax = yCenter + radius;

      for (int i = 0; i < numberOfPoints; i++)
      {
         Point2D randomPoint = EuclidCoreRandomTools.nextPoint2D(random, xMin, xMax, yMin, yMax);

         if (randomPoint.distance(zeroFramePoint) > radius)
            continue;

         points.add(randomPoint);
      }

      return points;
   }


   private static ArrayList<Point2D> generateRectangularPoints(double xMin, double xMax, double yMin, double yMax)
   {
      ArrayList<Point2D> points = new ArrayList<Point2D>();
      Point2D randomPoint = new Point2D(xMin, yMin);
      points.add(randomPoint);
      randomPoint = new Point2D(xMin, yMax);
      points.add(randomPoint);
      randomPoint = new Point2D(xMax, yMax);
      points.add(randomPoint);
      randomPoint = new Point2D(xMax, yMin);
      points.add(randomPoint);

      return points;
   }

   private static double randomDoubleInRange(Random random, double min, double max)
   {
      return min + random.nextDouble() * (max - min);
   }

}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/LocalVideoPacket.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets;

import java.awt.image.BufferedImage;

import controller_msgs.msg.dds.IntrinsicParametersMessage;
import us.ihmc.communication.packets.Packet;

public class LocalVideoPacket extends Packet<LocalVideoPacket>
{
   public long timeStamp;
   public BufferedImage image;
   public IntrinsicParametersMessage intrinsicParameters;

   public LocalVideoPacket()
   {

   }

   @Override
   public void set(LocalVideoPacket other)
   {
      timeStamp = other.timeStamp;
      image = other.image;
      intrinsicParameters =  other.intrinsicParameters;
      setPacketInformation(other);
   }

   public long getTimeStamp()
   {
      return timeStamp;
   }

   public BufferedImage getImage()
   {
      return image;
   }

   public IntrinsicParametersMessage getIntrinsicParameters()
   {
      return intrinsicParameters;
   }

   public void setIntrinsicParameters(IntrinsicParametersMessage intrinsicParameters)
   {
      this.intrinsicParameters = intrinsicParameters;
   }

   @Override
   public boolean epsilonEquals(LocalVideoPacket other, double epsilon)
   {
      boolean ret = timeStamp == other.timeStamp;
      ret &= image.equals(other.image);
      ret &= intrinsicParameters.equals(other.intrinsicParameters);
      return ret;
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/StopAllTrajectoryMessage.java<|end_filename|>
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.
       * Stop the execution of any trajectory being executed.
       */
public class StopAllTrajectoryMessage extends Packet<StopAllTrajectoryMessage> implements Settable<StopAllTrajectoryMessage>, EpsilonComparable<StopAllTrajectoryMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;

   public StopAllTrajectoryMessage()
   {
   }

   public StopAllTrajectoryMessage(StopAllTrajectoryMessage other)
   {
      this();
      set(other);
   }

   public void set(StopAllTrajectoryMessage 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<StopAllTrajectoryMessagePubSubType> getPubSubType()
   {
      return StopAllTrajectoryMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return StopAllTrajectoryMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(StopAllTrajectoryMessage 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 StopAllTrajectoryMessage)) return false;

      StopAllTrajectoryMessage otherMyClass = (StopAllTrajectoryMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("StopAllTrajectoryMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>websitedocs/website/i18n/en.json<|end_filename|>
{
  "_comment": "This file is auto-generated by write-translations.js",
  "localized-strings": {
    "next": "Next",
    "previous": "Previous",
    "tagline": "Open Robotics Software Documentation",
    "00-intro-control": "Introduction",
    "00-pre-reqs-eclipse": "Prerequisites",
    "Prerequisites": "Prerequisites",
    "00-pre-reqs-intellij": "Prerequisites",
    "000-summary-control": "Summary",
    "01-controllercore": "Controller Core",
    "01-project-eclipse": "Create your First GradleProject with a build.gradle File",
    "Create your First GradleProject": "Create your First GradleProject",
    "01-project-intellij": "Create your First GradleProject with a build.gradle File",
    "02-setup-gradle-project-eclipse": "Import GradleProject",
    "Import GradleProject": "Import GradleProject",
    "02-setup-gradle-project-intellij": "Import GradleProject",
    "02-yovariables": "Yo Variables",
    "03-new-class-eclipse": "Create The ValkyrieDemo Class",
    "Create the ValkyrieDemo Class": "Create the ValkyrieDemo Class",
    "03-new-class-intellij": "Create The ValkyrieDemo Class",
    "04-run-valkyrie-eclipse": "ValkyrieDemo Run Configuration",
    "ValkyrieDemo Run Configuration": "ValkyrieDemo Run Configuration",
    "04-run-valkyrie-intellij": "ValkyrieDemo Run Configuration",
    "05-run-valkyrie-scs-eclipse": "Run ValkyrieDemo",
    "Run ValkyrieDemo": "Run ValkyrieDemo",
    "05-run-valkyrie-scs": "Run ValkyrieDemo",
    "installgradle": "Installing Gradle",
    "installjava": "Installing Java",
    "open-software-home": "IHMC Open Robotics Software Documentatation",
    "quickstarthome": "Quick Start",
    "IHMC Home": "IHMC Home",
    "All IHMC Docs": "All IHMC Docs",
    "About IHMC": "About IHMC",
    "Quick Start (Eclipse)": "Quick Start (Eclipse)",
    "Quick Start (IntelliJ)": "Quick Start (IntelliJ)",
    "IHMC Humanoid Controller": "IHMC Humanoid Controller"
  },
  "pages-strings": {
    "Help Translate|recruit community translators for your project": "Help Translate",
    "Edit this Doc|recruitment message asking to edit the doc source": "Edit",
    "Translate this Doc|recruitment message asking to translate the docs": "Translate"
  }
}


<|start_filename|>ihmc-quadruped-robotics/src/main/java/us/ihmc/quadrupedRobotics/estimator/stateEstimator/QuadrupedSensorReaderWrapper.java<|end_filename|>
package us.ihmc.quadrupedRobotics.estimator.stateEstimator;

import us.ihmc.sensorProcessing.simulatedSensors.SensorReader;

public interface QuadrupedSensorReaderWrapper extends SensorReader
{
   void setSensorReader(SensorReader sensorReader);
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/environments/CinderBlockFieldWithFiducialEnvironment.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.environments;

import java.util.List;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.simulationConstructionSetTools.util.ground.CombinedTerrainObject3D;
import us.ihmc.simulationconstructionset.ExternalForcePoint;
import us.ihmc.simulationconstructionset.Robot;
import us.ihmc.simulationconstructionset.util.ground.TerrainObject3D;

public class CinderBlockFieldWithFiducialEnvironment implements CommonAvatarEnvironmentInterface
{
   private final CombinedTerrainObject3D combinedTerrainObject3D = new CombinedTerrainObject3D(getClass().getSimpleName());
   private static final boolean SHORT_FIELD = false;

   public enum FiducialType
   {
      FIDUCIAL_50,
      VALVE
   }

   public CinderBlockFieldWithFiducialEnvironment(FiducialType fiducialType)
   {
      combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.setUpGround("FlatGround"));

      if(SHORT_FIELD)
         combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.setUpShortCinderBlockField("CinderBlockField", 0.0, 1.0));
      else
         combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.setUpCinderBlockFieldActual("CinderBlockField", 0.0, 0.0));

      switch (fiducialType)
      {
      case FIDUCIAL_50: combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.addFiducial(new Vector3D(12.0, 0.0, 1.7), 0.0, Fiducial.FIDUCIAL50)); break;
      case VALVE: combinedTerrainObject3D.addTerrainObject(DefaultCommonAvatarEnvironment.addValveTextureBox(new Vector3D(10.0, 0.0, 1.7), 0.0)); break;
      }
   }

   @Override
   public TerrainObject3D getTerrainObject3D()
   {
      return combinedTerrainObject3D;
   }

   @Override
   public List<? extends Robot> getEnvironmentRobots()
   {
      return null;
   }

   @Override
   public void createAndSetContactControllerToARobot()
   {
   }

   @Override
   public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints)
   {
   }

   @Override
   public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener)
   {
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/ConvexPolygonConstructorFromInteriorOfRays.java<|end_filename|>
package us.ihmc.robotics.geometry;

import java.util.ArrayList;
import java.util.List;

import org.apache.commons.lang3.mutable.MutableBoolean;

import us.ihmc.euclid.geometry.Line2D;
import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DBasics;
import us.ihmc.euclid.tuple2D.Point2D;
import us.ihmc.commons.lists.RecyclingArrayList;

public class ConvexPolygonConstructorFromInteriorOfRays
{
   private static final int POINT_POOL_INITIAL_SIZE = 10;
   private final ArrayList<Point2D> pointPool = new ArrayList<Point2D>();
   
   private final ArrayList<Point2D> intersectionArrayList = new ArrayList<Point2D>();
   private final ArrayList<Point2D> intersectionPoints = new ArrayList<Point2D>();
   private final RecyclingArrayList<MutableBoolean> removeRay = new RecyclingArrayList<MutableBoolean>(MutableBoolean.class);
   
   public ConvexPolygonConstructorFromInteriorOfRays()
   {
      for(int i = 0; i < POINT_POOL_INITIAL_SIZE; i++)
      {
         pointPool.add(new Point2D());
      }
   }
   
   public boolean constructFromInteriorOfRays(List<Line2D> rays, ConvexPolygon2DBasics polygonToPack)
   {      
      removeRay.clear();
      
      intersectionPoints.clear();
      for (int i=0; i<rays.size(); i++)
      {
         intersectionPoints.add(null);
         removeRay.getAndGrowIfNeeded(i).setValue(false);
      }

      for (int rayIndex = 0; rayIndex < rays.size(); rayIndex++)
      {
         if (removeRay.get(rayIndex).booleanValue())
         {
            continue;    // This ray was removed already. Don't consider it.
         }

         Line2D ray = rays.get(rayIndex);
         if (ray == null)
            continue;

         // Check previous non-null rays. For each one If its point is outside this ray, mark the ray as removed and delete its point.
         int previousIndexToCheck = rayIndex;

         while (true)
         {
            previousIndexToCheck--;
            if (previousIndexToCheck < 0)
               previousIndexToCheck = rays.size() - 1;

            if (previousIndexToCheck == rayIndex)
            {
               throw new RuntimeException("Should never get here!!!");
            }

            if (removeRay.get(previousIndexToCheck).booleanValue())
               continue;    // That ray was removed already. Check a previous ray.

            Point2D previousPointToCheck = intersectionPoints.get(previousIndexToCheck);

            // If the point is not to be in the polygon, as determined by the current ray under consideration, remove it and try the previous one.
            if ((previousPointToCheck != null) && (ray.isPointOnLeftSideOfLine(previousPointToCheck)))
            {
               removeRay.get(previousIndexToCheck).setValue(true);
               removeIntersectionPoint(intersectionPoints, previousIndexToCheck);
//               intersectionPoints[previousIndexToCheck] = null;

               continue;
            }

            // Found a previous Ray that may be ok. Use it to find this rays potential intersection point:
            Line2D previousRayToCheck = rays.get(previousIndexToCheck);

            Point2D intersection = getAndRemovePointFromPool();
            if (intersection == null) throw new RuntimeException("intersection == null!");
            
            boolean foundIntersection = ray.intersectionWith(previousRayToCheck, intersection);
            
            if (!foundIntersection)
            {
               returnPointsToPool(intersectionPoints);
               return false;    // If two lines don't intersect, then it must be null interior, when using this as intended...
            }
            intersectionPoints.set(rayIndex, intersection);

            // Now keep looking back to make sure that this point is valid:
            int evenMorePreviousIndexToCheck = previousIndexToCheck;
            while (true)
            {
               evenMorePreviousIndexToCheck--;
               if (evenMorePreviousIndexToCheck < 0)
                  evenMorePreviousIndexToCheck = rays.size() - 1;

               if (evenMorePreviousIndexToCheck == rayIndex)
               {
                  returnPointsToPool(intersectionPoints);
                  return false;    // If loop around, must be null interior.

                  // throw new RuntimeException("Should never get here!!!");
               }

               if (removeRay.get(evenMorePreviousIndexToCheck).booleanValue())
                  continue;    // That ray was removed already. Check a previous ray.
               Line2D evenMorePreviousRayToCheck = rays.get(evenMorePreviousIndexToCheck);

               // If the intersection point is not valid, then need to remove the previousRayToCheck and continue looking for more to remove...
               if (evenMorePreviousRayToCheck.isPointOnLeftSideOfLine(intersection))
               {
                  removeRay.get(previousIndexToCheck).setValue(true);
                  removeIntersectionPoint(intersectionPoints, previousIndexToCheck);
//                  intersectionPoints[previousIndexToCheck] = null;

                  previousIndexToCheck = evenMorePreviousIndexToCheck;
                  
                  intersection = getAndRemovePointFromPool();
                  foundIntersection = ray.intersectionWith(evenMorePreviousRayToCheck, intersection);
                  if (!foundIntersection)
                  {
                     returnPointsToPool(intersectionPoints);
                     return false;    // If two rays don't intersect, then it must be null interior, when using this as intended...
                  }

                  intersectionPoints.set(rayIndex, intersection);

               }
               else
               {
                  break;
               }
            }

            break;
         }
      }

      intersectionArrayList.clear();

      for (int i=0; i<intersectionPoints.size(); i++)
      {
         Point2D point = intersectionPoints.get(i);
         
         if (point != null)
            intersectionArrayList.add(point);
      }

      if (intersectionArrayList.size() < 3)
      {
         returnPointsToPool(intersectionPoints);
         return false;
      }

      polygonToPack.clear();
      for (int i = 0; i < intersectionArrayList.size(); i++)
         polygonToPack.addVertex(intersectionArrayList.get(i));
      polygonToPack.update();

      returnPointsToPool(intersectionPoints);
      return true;
   }

   private Point2D getAndRemovePointFromPool()
   {
      if (pointPool.isEmpty())
      {
         pointPool.add(new Point2D());
      }
      
      return pointPool.remove(pointPool.size()-1);
   }
   
   private void removeIntersectionPoint(ArrayList<Point2D> intersectionPoints, int index)
   {
      Point2D pointToRemove = intersectionPoints.get(index);
      
      if (pointToRemove != null)
      {
         pointPool.add(pointToRemove);
         intersectionPoints.set(index, null);
      }
      
   }
   
   private void returnPointsToPool(ArrayList<Point2D> intersectionPoints)
   {
      for (int i=0; i<intersectionPoints.size(); i++)
      {
         Point2D point = intersectionPoints.get(i);
         if (point != null) pointPool.add(point);
      }
      
      intersectionPoints.clear();
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/DoubleSlider.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

public class DoubleSlider extends NumericSlider<Double>
{
   private static final int ticks = 100;

   @Override
   public double toDouble(Double number)
   {
      return number;
   }

   @Override
   public Double toNumber(double doubleValue)
   {
      return doubleValue;
   }

   @Override
   public int getPrecision()
   {
      double range = getMax() - getMin();
      double step = range / ticks;
      double precision = Math.log10(step);
      return (int) -Math.round(precision);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WholeBodyTrajectoryToolboxOutputStatus.java<|end_filename|>
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;

/**
       * General purpose message normally used to report the solution of a whole-body trajectory planner.
       * Main usage is the IHMC WholeBodyTrajectoryToolbox.
       */
public class WholeBodyTrajectoryToolboxOutputStatus extends Packet<WholeBodyTrajectoryToolboxOutputStatus> implements Settable<WholeBodyTrajectoryToolboxOutputStatus>, EpsilonComparable<WholeBodyTrajectoryToolboxOutputStatus>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * 0: not completed.
            * 1: fail to find initial guess.
            * 2: fail to complete expanding tree.
            * 3: fail to optimize path.
            * 4: solution is available.
            */
   public int planning_result_;
   public us.ihmc.idl.IDLSequence.Double  trajectory_times_;
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.KinematicsToolboxOutputStatus>  robot_configurations_;

   public WholeBodyTrajectoryToolboxOutputStatus()
   {
      trajectory_times_ = new us.ihmc.idl.IDLSequence.Double (50, "type_6");

      robot_configurations_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.KinematicsToolboxOutputStatus> (50, new controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType());

   }

   public WholeBodyTrajectoryToolboxOutputStatus(WholeBodyTrajectoryToolboxOutputStatus other)
   {
      this();
      set(other);
   }

   public void set(WholeBodyTrajectoryToolboxOutputStatus other)
   {
      sequence_id_ = other.sequence_id_;

      planning_result_ = other.planning_result_;

      trajectory_times_.set(other.trajectory_times_);
      robot_configurations_.set(other.robot_configurations_);
   }

   /**
            * 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_;
   }

   /**
            * 0: not completed.
            * 1: fail to find initial guess.
            * 2: fail to complete expanding tree.
            * 3: fail to optimize path.
            * 4: solution is available.
            */
   public void setPlanningResult(int planning_result)
   {
      planning_result_ = planning_result;
   }
   /**
            * 0: not completed.
            * 1: fail to find initial guess.
            * 2: fail to complete expanding tree.
            * 3: fail to optimize path.
            * 4: solution is available.
            */
   public int getPlanningResult()
   {
      return planning_result_;
   }


   public us.ihmc.idl.IDLSequence.Double  getTrajectoryTimes()
   {
      return trajectory_times_;
   }


   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.KinematicsToolboxOutputStatus>  getRobotConfigurations()
   {
      return robot_configurations_;
   }


   public static Supplier<WholeBodyTrajectoryToolboxOutputStatusPubSubType> getPubSubType()
   {
      return WholeBodyTrajectoryToolboxOutputStatusPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WholeBodyTrajectoryToolboxOutputStatusPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WholeBodyTrajectoryToolboxOutputStatus other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.planning_result_, other.planning_result_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsDoubleSequence(this.trajectory_times_, other.trajectory_times_, epsilon)) return false;

      if (this.robot_configurations_.size() != other.robot_configurations_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.robot_configurations_.size(); i++)
         {  if (!this.robot_configurations_.get(i).epsilonEquals(other.robot_configurations_.get(i), epsilon)) return false; }
      }


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WholeBodyTrajectoryToolboxOutputStatus)) return false;

      WholeBodyTrajectoryToolboxOutputStatus otherMyClass = (WholeBodyTrajectoryToolboxOutputStatus) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.planning_result_ != otherMyClass.planning_result_) return false;

      if (!this.trajectory_times_.equals(otherMyClass.trajectory_times_)) return false;
      if (!this.robot_configurations_.equals(otherMyClass.robot_configurations_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WholeBodyTrajectoryToolboxOutputStatus {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("planning_result=");
      builder.append(this.planning_result_);      builder.append(", ");
      builder.append("trajectory_times=");
      builder.append(this.trajectory_times_);      builder.append(", ");
      builder.append("robot_configurations=");
      builder.append(this.robot_configurations_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/visibilityGraphs/interfaces/NavigableRegionFilter.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.interfaces;

import java.util.List;

import us.ihmc.robotics.geometry.PlanarRegion;

public interface NavigableRegionFilter
{
   boolean isPlanarRegionNavigable(PlanarRegion query, List<PlanarRegion> allOtherRegions);
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/handControl/packetsAndConsumers/ManualHandControlProvider.java<|end_filename|>
package us.ihmc.avatar.handControl.packetsAndConsumers;

import java.util.concurrent.ConcurrentLinkedQueue;

import controller_msgs.msg.dds.ManualHandControlPacket;
import us.ihmc.pubsub.subscriber.Subscriber;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.ros2.NewMessageListener;

public class ManualHandControlProvider implements NewMessageListener<ManualHandControlPacket>
{

   private final ConcurrentLinkedQueue<ManualHandControlPacket> packetQueue = new ConcurrentLinkedQueue<ManualHandControlPacket>();
   private RobotSide robotSide;

   public ManualHandControlProvider(RobotSide robotSide)
   {
      this.robotSide = robotSide;
   }

   @Override
   public void onNewDataMessage(Subscriber<ManualHandControlPacket> subscriber)
   {
      packetQueue.add(subscriber.takeNextData());
   }

   public ManualHandControlPacket pullPacket()
   {
      return packetQueue.poll();
   }

   public boolean isNewPacketAvailable()
   {
      return !packetQueue.isEmpty();
   }

   public RobotSide getSide()
   {
      return this.robotSide;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/UIOcTree.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui;

import java.util.Map;
import java.util.Map.Entry;

import us.ihmc.jOctoMap.key.OcTreeKey;
import us.ihmc.jOctoMap.ocTree.baseImplementation.AbstractOcTreeBase;
import us.ihmc.robotEnvironmentAwareness.communication.packets.NormalOcTreeMessage;

public class UIOcTree extends AbstractOcTreeBase<UIOcTreeNode>
{
   public UIOcTree(NormalOcTreeMessage normalOcTreeMessage)
   {
      this(normalOcTreeMessage, null);
   }

   public UIOcTree(NormalOcTreeMessage normalOcTreeMessage, Map<OcTreeKey, Integer> nodeKeyToRegionIdMap)
   {
      super(normalOcTreeMessage.resolution, normalOcTreeMessage.treeDepth);
      if (normalOcTreeMessage.root != null)
         root = new UIOcTreeNode(normalOcTreeMessage.root, resolution, treeDepth);
      else
         root = null;

      if (nodeKeyToRegionIdMap != null)
         nodeKeyToRegionIdMap.entrySet().stream().forEach(this::setNodeRegionId);

      if (root != null)
         updateInnerRegionIdRecursive(root, 0);
   }

   private void updateInnerRegionIdRecursive(UIOcTreeNode node, int depth)
   {
      // only recurse and update for inner nodes:
      if (node.hasAtLeastOneChild())
      {
         // return early for last level:
         if (depth < treeDepth - 1)
         {
            for (int i = 0; i < 8; i++)
            {
               UIOcTreeNode childNode = node.getChild(i);
               if (childNode != null)
                  updateInnerRegionIdRecursive(childNode, depth + 1);
            }
         }
         node.setRegionIdFromChildren();
      }
   }

   private void setNodeRegionId(Entry<OcTreeKey, Integer> nodeKeyToRegionIdEntry)
   {
      UIOcTreeNode node = search(nodeKeyToRegionIdEntry.getKey());
      if (node != null)
         node.setRegionId(nodeKeyToRegionIdEntry.getValue());
   }

   @Override
   protected Class<UIOcTreeNode> getNodeClass()
   {
      return UIOcTreeNode.class;
   }
}


<|start_filename|>ihmc-path-planning/src/visualizers/java/us/ihmc/pathPlanning/visibilityGraphs/ui/properties/Point3DProperty.java<|end_filename|>
package us.ihmc.pathPlanning.visibilityGraphs.ui.properties;

import javafx.beans.property.Property;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.robotEnvironmentAwareness.ui.properties.ParametersProperty;

public class Point3DProperty extends ParametersProperty<Point3D>
{
   private final DoubleField x = new DoubleField(Point3D::getX, (p, v) -> p.setX(v));
   private final DoubleField y = new DoubleField(Point3D::getY, (p, v) -> p.setY(v));
   private final DoubleField z = new DoubleField(Point3D::getZ, (p, v) -> p.setZ(v));

   public Point3DProperty(Object bean, String name)
   {
      this(bean, name, new Point3D());
   }

   public Point3DProperty(Object bean, String name, Point3D initialValue)
   {
      super(bean, name, initialValue);
   }

   public void bindBidirectionalX(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, x);
   }
   
   public void bindBidirectionalY(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, y);
   }
   
   public void bindBidirectionalZ(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, z);
   }

   @Override
   protected Point3D getValueCopy(Point3D valueToCopy)
   {
      return new Point3D(valueToCopy);
   }

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WholeBodyTrajectoryToolboxConfigurationMessage.java<|end_filename|>
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;

/**
       * Message used to configure the IHMC whole-body trajetory planner.
       * Main usage is the IHMC WholeBodyTrajectoryToolbox.
       */
public class WholeBodyTrajectoryToolboxConfigurationMessage extends Packet<WholeBodyTrajectoryToolboxConfigurationMessage> implements Settable<WholeBodyTrajectoryToolboxConfigurationMessage>, EpsilonComparable<WholeBodyTrajectoryToolboxConfigurationMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public int number_of_initial_guesses_ = -1;
   public int maximum_expansion_size_ = -1;
   public controller_msgs.msg.dds.KinematicsToolboxOutputStatus initial_configuration_;

   public WholeBodyTrajectoryToolboxConfigurationMessage()
   {
      initial_configuration_ = new controller_msgs.msg.dds.KinematicsToolboxOutputStatus();
   }

   public WholeBodyTrajectoryToolboxConfigurationMessage(WholeBodyTrajectoryToolboxConfigurationMessage other)
   {
      this();
      set(other);
   }

   public void set(WholeBodyTrajectoryToolboxConfigurationMessage other)
   {
      sequence_id_ = other.sequence_id_;

      number_of_initial_guesses_ = other.number_of_initial_guesses_;

      maximum_expansion_size_ = other.maximum_expansion_size_;

      controller_msgs.msg.dds.KinematicsToolboxOutputStatusPubSubType.staticCopy(other.initial_configuration_, initial_configuration_);
   }

   /**
            * 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 void setNumberOfInitialGuesses(int number_of_initial_guesses)
   {
      number_of_initial_guesses_ = number_of_initial_guesses;
   }
   public int getNumberOfInitialGuesses()
   {
      return number_of_initial_guesses_;
   }

   public void setMaximumExpansionSize(int maximum_expansion_size)
   {
      maximum_expansion_size_ = maximum_expansion_size;
   }
   public int getMaximumExpansionSize()
   {
      return maximum_expansion_size_;
   }


   public controller_msgs.msg.dds.KinematicsToolboxOutputStatus getInitialConfiguration()
   {
      return initial_configuration_;
   }


   public static Supplier<WholeBodyTrajectoryToolboxConfigurationMessagePubSubType> getPubSubType()
   {
      return WholeBodyTrajectoryToolboxConfigurationMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WholeBodyTrajectoryToolboxConfigurationMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WholeBodyTrajectoryToolboxConfigurationMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.number_of_initial_guesses_, other.number_of_initial_guesses_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.maximum_expansion_size_, other.maximum_expansion_size_, epsilon)) return false;

      if (!this.initial_configuration_.epsilonEquals(other.initial_configuration_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WholeBodyTrajectoryToolboxConfigurationMessage)) return false;

      WholeBodyTrajectoryToolboxConfigurationMessage otherMyClass = (WholeBodyTrajectoryToolboxConfigurationMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.number_of_initial_guesses_ != otherMyClass.number_of_initial_guesses_) return false;

      if(this.maximum_expansion_size_ != otherMyClass.maximum_expansion_size_) return false;

      if (!this.initial_configuration_.equals(otherMyClass.initial_configuration_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WholeBodyTrajectoryToolboxConfigurationMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("number_of_initial_guesses=");
      builder.append(this.number_of_initial_guesses_);      builder.append(", ");
      builder.append("maximum_expansion_size=");
      builder.append(this.maximum_expansion_size_);      builder.append(", ");
      builder.append("initial_configuration=");
      builder.append(this.initial_configuration_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SCSListenerPacket.java<|end_filename|>
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 IHMC Simulation Construction Set.
       * Notifies the user when the simulation has stopped.
       */
public class SCSListenerPacket extends Packet<SCSListenerPacket> implements Settable<SCSListenerPacket>, EpsilonComparable<SCSListenerPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public boolean is_stopped_ = true;

   public SCSListenerPacket()
   {
   }

   public SCSListenerPacket(SCSListenerPacket other)
   {
      this();
      set(other);
   }

   public void set(SCSListenerPacket other)
   {
      sequence_id_ = other.sequence_id_;

      is_stopped_ = other.is_stopped_;

   }

   /**
            * 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 void setIsStopped(boolean is_stopped)
   {
      is_stopped_ = is_stopped;
   }
   public boolean getIsStopped()
   {
      return is_stopped_;
   }


   public static Supplier<SCSListenerPacketPubSubType> getPubSubType()
   {
      return SCSListenerPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SCSListenerPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SCSListenerPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_stopped_, other.is_stopped_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SCSListenerPacket)) return false;

      SCSListenerPacket otherMyClass = (SCSListenerPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.is_stopped_ != otherMyClass.is_stopped_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SCSListenerPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("is_stopped=");
      builder.append(this.is_stopped_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/SimulatedLidarScanPacket.java<|end_filename|>
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;

public class SimulatedLidarScanPacket extends Packet<SimulatedLidarScanPacket> implements Settable<SimulatedLidarScanPacket>, EpsilonComparable<SimulatedLidarScanPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.idl.IDLSequence.Float  ranges_;
   public int sensor_id_;
   public controller_msgs.msg.dds.LidarScanParametersMessage lidar_scan_parameters_;

   public SimulatedLidarScanPacket()
   {
      ranges_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5");

      lidar_scan_parameters_ = new controller_msgs.msg.dds.LidarScanParametersMessage();
   }

   public SimulatedLidarScanPacket(SimulatedLidarScanPacket other)
   {
      this();
      set(other);
   }

   public void set(SimulatedLidarScanPacket other)
   {
      sequence_id_ = other.sequence_id_;

      ranges_.set(other.ranges_);
      sensor_id_ = other.sensor_id_;

      controller_msgs.msg.dds.LidarScanParametersMessagePubSubType.staticCopy(other.lidar_scan_parameters_, lidar_scan_parameters_);
   }

   /**
            * 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 us.ihmc.idl.IDLSequence.Float  getRanges()
   {
      return ranges_;
   }

   public void setSensorId(int sensor_id)
   {
      sensor_id_ = sensor_id;
   }
   public int getSensorId()
   {
      return sensor_id_;
   }


   public controller_msgs.msg.dds.LidarScanParametersMessage getLidarScanParameters()
   {
      return lidar_scan_parameters_;
   }


   public static Supplier<SimulatedLidarScanPacketPubSubType> getPubSubType()
   {
      return SimulatedLidarScanPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return SimulatedLidarScanPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(SimulatedLidarScanPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsFloatSequence(this.ranges_, other.ranges_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sensor_id_, other.sensor_id_, epsilon)) return false;

      if (!this.lidar_scan_parameters_.epsilonEquals(other.lidar_scan_parameters_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof SimulatedLidarScanPacket)) return false;

      SimulatedLidarScanPacket otherMyClass = (SimulatedLidarScanPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.ranges_.equals(otherMyClass.ranges_)) return false;
      if(this.sensor_id_ != otherMyClass.sensor_id_) return false;

      if (!this.lidar_scan_parameters_.equals(otherMyClass.lidar_scan_parameters_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("SimulatedLidarScanPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("ranges=");
      builder.append(this.ranges_);      builder.append(", ");
      builder.append("sensor_id=");
      builder.append(this.sensor_id_);      builder.append(", ");
      builder.append("lidar_scan_parameters=");
      builder.append(this.lidar_scan_parameters_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/walking/FootstepDataListCorruptor.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.walking;

import java.util.Random;

import controller_msgs.msg.dds.FootstepDataListMessage;
import controller_msgs.msg.dds.FootstepDataMessage;
import us.ihmc.commons.RandomNumbers;
import us.ihmc.euclid.axisAngle.AxisAngle;
import us.ihmc.euclid.tuple3D.Point3D;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.humanoidRobotics.communication.packets.HumanoidMessageTools;
import us.ihmc.robotics.random.RandomGeometry;

public class FootstepDataListCorruptor
{
   private final Random random = new Random(1778L);
   private final Vector3D minLocationCorruption = new Vector3D();
   private final Vector3D maxLocationCorruption = new Vector3D();
   private final double maxRotationCorruption;
   
   public FootstepDataListCorruptor(Vector3D minLocationCorruption, Vector3D maxLocationCorruption, double maxRotationCorruption)
   {
      this.minLocationCorruption.set(minLocationCorruption);
      this.maxLocationCorruption.set(maxLocationCorruption);
      this.maxRotationCorruption = maxRotationCorruption;
   }
   
   public FootstepDataListMessage corruptDataList(FootstepDataListMessage footstepDataList)
   {
      FootstepDataListMessage ret = HumanoidMessageTools.createFootstepDataListMessage(footstepDataList.getDefaultSwingDuration(), footstepDataList.getDefaultTransferDuration());
      
      for (int i = 0; i < footstepDataList.getFootstepDataList().size(); i++)
      {
         ret.getFootstepDataList().add().set(corruptFootstepData(footstepDataList.getFootstepDataList().get(i)));
      }
      
      return ret;
   }
   
   public FootstepDataMessage corruptFootstepData(FootstepDataMessage footstepData)
   {
      FootstepDataMessage ret = new FootstepDataMessage(footstepData);
      
      Point3D location = new Point3D(ret.getLocation());
      Quaternion orientation = new Quaternion(ret.getOrientation());
      
      corruptOrientation(orientation);
      ret.getOrientation().set(orientation);
      
      corruptLocationVector(location);
      ret.getLocation().set(location);
      return ret;
   }
   
   private void corruptOrientation(Quaternion orientation)
   {
      Vector3D axis = RandomGeometry.nextVector3D(random);
      double angle = RandomNumbers.nextDouble(random, -maxRotationCorruption, maxRotationCorruption);
      
      AxisAngle axisAngle4d = new AxisAngle();
      axisAngle4d.set(axis, angle);
      
      Quaternion corruption = new Quaternion();
      corruption.set(axisAngle4d);
            
      orientation.multiply(corruption);
   }

   private void corruptLocationVector(Point3D location)
   {
      Vector3D randomVector = RandomGeometry.nextVector3D(random, minLocationCorruption, maxLocationCorruption);
      location.add(randomVector);
   }
   
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedBodyOrientationMessage.java<|end_filename|>
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 commands the controller to move in taskspace the body to the desired orientation while going through the specified trajectory points.
       * A Hermite based curve (third order) is used to interpolate the orientations.
       * This message allows controlling the body orientation without interfering with position that will still be controlled to maintain the current desired capture point position.
       * To execute a normal trajectory to reach a desired body orientation, set only one trajectory point with zero velocity and its time to be equal to the desired trajectory time.
       */
public class QuadrupedBodyOrientationMessage extends Packet<QuadrupedBodyOrientationMessage> implements Settable<QuadrupedBodyOrientationMessage>, EpsilonComparable<QuadrupedBodyOrientationMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean is_expressed_in_absolute_time_ = true;
   /**
            * Indicates if the given trajectory should be considered an "absolute" orientation or an "offset" orientation
            */
   public boolean is_an_offset_orientation_ = true;
   /**
            * The orientation trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage so3_trajectory_;

   public QuadrupedBodyOrientationMessage()
   {
      so3_trajectory_ = new controller_msgs.msg.dds.SO3TrajectoryMessage();
   }

   public QuadrupedBodyOrientationMessage(QuadrupedBodyOrientationMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedBodyOrientationMessage other)
   {
      sequence_id_ = other.sequence_id_;

      is_expressed_in_absolute_time_ = other.is_expressed_in_absolute_time_;

      is_an_offset_orientation_ = other.is_an_offset_orientation_;

      controller_msgs.msg.dds.SO3TrajectoryMessagePubSubType.staticCopy(other.so3_trajectory_, so3_trajectory_);
   }

   /**
            * 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_;
   }

   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public void setIsExpressedInAbsoluteTime(boolean is_expressed_in_absolute_time)
   {
      is_expressed_in_absolute_time_ = is_expressed_in_absolute_time;
   }
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean getIsExpressedInAbsoluteTime()
   {
      return is_expressed_in_absolute_time_;
   }

   /**
            * Indicates if the given trajectory should be considered an "absolute" orientation or an "offset" orientation
            */
   public void setIsAnOffsetOrientation(boolean is_an_offset_orientation)
   {
      is_an_offset_orientation_ = is_an_offset_orientation;
   }
   /**
            * Indicates if the given trajectory should be considered an "absolute" orientation or an "offset" orientation
            */
   public boolean getIsAnOffsetOrientation()
   {
      return is_an_offset_orientation_;
   }


   /**
            * The orientation trajectory information.
            */
   public controller_msgs.msg.dds.SO3TrajectoryMessage getSo3Trajectory()
   {
      return so3_trajectory_;
   }


   public static Supplier<QuadrupedBodyOrientationMessagePubSubType> getPubSubType()
   {
      return QuadrupedBodyOrientationMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedBodyOrientationMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedBodyOrientationMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_expressed_in_absolute_time_, other.is_expressed_in_absolute_time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_an_offset_orientation_, other.is_an_offset_orientation_, epsilon)) return false;

      if (!this.so3_trajectory_.epsilonEquals(other.so3_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedBodyOrientationMessage)) return false;

      QuadrupedBodyOrientationMessage otherMyClass = (QuadrupedBodyOrientationMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.is_expressed_in_absolute_time_ != otherMyClass.is_expressed_in_absolute_time_) return false;

      if(this.is_an_offset_orientation_ != otherMyClass.is_an_offset_orientation_) return false;

      if (!this.so3_trajectory_.equals(otherMyClass.so3_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedBodyOrientationMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("is_expressed_in_absolute_time=");
      builder.append(this.is_expressed_in_absolute_time_);      builder.append(", ");
      builder.append("is_an_offset_orientation=");
      builder.append(this.is_an_offset_orientation_);      builder.append(", ");
      builder.append("so3_trajectory=");
      builder.append(this.so3_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-simulation-toolkit/src/main/java/us/ihmc/sensorProcessing/simulatedSensors/Vector3DProvider.java<|end_filename|>
package us.ihmc.sensorProcessing.simulatedSensors;

import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;

public interface Vector3DProvider
{
   Vector3DReadOnly getValue();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WalkingControllerFailureStatusMessage.java<|end_filename|>
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.
       * The controller will send this message when detecting a fall.
       */
public class WalkingControllerFailureStatusMessage extends Packet<WalkingControllerFailureStatusMessage> implements Settable<WalkingControllerFailureStatusMessage>, EpsilonComparable<WalkingControllerFailureStatusMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * Specifies the estimated falling direction in 2D
            */
   public us.ihmc.euclid.tuple3D.Vector3D falling_direction_;

   public WalkingControllerFailureStatusMessage()
   {
      falling_direction_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public WalkingControllerFailureStatusMessage(WalkingControllerFailureStatusMessage other)
   {
      this();
      set(other);
   }

   public void set(WalkingControllerFailureStatusMessage other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.falling_direction_, falling_direction_);
   }

   /**
            * 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_;
   }


   /**
            * Specifies the estimated falling direction in 2D
            */
   public us.ihmc.euclid.tuple3D.Vector3D getFallingDirection()
   {
      return falling_direction_;
   }


   public static Supplier<WalkingControllerFailureStatusMessagePubSubType> getPubSubType()
   {
      return WalkingControllerFailureStatusMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WalkingControllerFailureStatusMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WalkingControllerFailureStatusMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.falling_direction_.epsilonEquals(other.falling_direction_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WalkingControllerFailureStatusMessage)) return false;

      WalkingControllerFailureStatusMessage otherMyClass = (WalkingControllerFailureStatusMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.falling_direction_.equals(otherMyClass.falling_direction_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WalkingControllerFailureStatusMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("falling_direction=");
      builder.append(this.falling_direction_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/simulationDispatcher/client/gui/AliveDispatchHostPanel.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.simulationDispatcher.client.gui;

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;

import us.ihmc.simulationConstructionSetTools.simulationDispatcher.client.DispatchHost;
import us.ihmc.simulationConstructionSetTools.simulationDispatcher.client.DispatchHostList;

public class AliveDispatchHostPanel extends DispatchHostPanel
{
   private static final long serialVersionUID = -3781923043775451428L;
   private static final int NAME_WIDTH = 12;
   private static final int DESCRIPTION_WIDTH = 12;
   private static final int NUMBER_WIDTH = 5;

   private static final int DESCRIPTION_START = NAME_WIDTH;
   private static final int ELAPSED_TIME_START = DESCRIPTION_START + DESCRIPTION_WIDTH;
   private static final int NUMBER_SIMS_START = ELAPSED_TIME_START + NUMBER_WIDTH;

   private static final int APPROX_TIME_START = NUMBER_SIMS_START + NUMBER_WIDTH;
   private static final int LAST_TIME_START = APPROX_TIME_START + NUMBER_WIDTH;

   private static final int TOTAL_WIDTH = (LAST_TIME_START + NUMBER_WIDTH) * FONT_SIZE;

   protected Dimension sz = new Dimension(TOTAL_WIDTH, FONT_SIZE);


   public AliveDispatchHostPanel(DispatchHostList dispatchHostList)
   {
      super(dispatchHostList);
      setPanelSize(1);
   }


   @Override
   public void hostsChanged()
   {
      dispatchHosts.clear();

      DispatchHost[] aliveHosts = dispatchHostList.getAllAliveHosts();
      for (int i = 0; i < aliveHosts.length; i++)
      {
         dispatchHosts.add(aliveHosts[i]);
      }

      // this.repaint();

      if (selectedHost == null)
         selected = -1;
      else if (dispatchHosts.contains(selectedHost))
      {
         selected = dispatchHosts.indexOf(selectedHost);
      }
      else
      {
         selectedHost = null;
         selected = -1;
      }

      setPanelSize(aliveHosts.length);
   }


   protected void setPanelSize(int nHosts)
   {
      sz.setSize(TOTAL_WIDTH, FONT_SIZE * nHosts + 22);
      this.setPreferredSize(sz);
      this.updateUI();
   }

   @Override
   public void paintComponent(Graphics g)
   {
      super.paintComponent(g);

      g.setColor(Color.black);

      g.drawString("Host Name", 2, 10);
      g.drawString("Description", 12 * (DESCRIPTION_START), 10);
      g.drawString("Elapsed", 12 * (ELAPSED_TIME_START), 10);
      g.drawString("Number", 12 * (NUMBER_SIMS_START), 10);
      g.drawString("Estimate", 12 * (APPROX_TIME_START), 10);
      g.drawString("Last", 12 * (LAST_TIME_START), 10);

      for (int i = 0; i < dispatchHosts.size(); i++)
      {
         DispatchHost dispatchHost = (DispatchHost) dispatchHosts.get(i);

         String hostName = dispatchHost.getHostName();
         String hostProcess = dispatchHost.getHostProcess();

         String description = null;
         if (dispatchHost.getDispatchedSim() != null)
            description = dispatchHost.getDispatchedSim().getDescription();
         String elapsedTime = String.valueOf(((int) dispatchHost.getTimeSinceLastSimulationStarted()));

         String numberSims = String.valueOf(dispatchHost.getNumberOfSimulationsRun());
         String approxTime = String.valueOf(dispatchHost.getApporxTimePerSimulation());
         String lastTime = String.valueOf(dispatchHost.getlastSimulationTime());

         if (i == selected)
            g.setColor(Color.magenta);
         else if (!dispatchHost.isAlive())
            g.setColor(Color.red);
         else if (dispatchHost.isRunningSim())
            g.setColor(Color.green);
         else
            g.setColor(Color.blue);

         g.drawString(hostName + ": " + hostProcess, 2, i * FONT_SIZE + 22);
         if (description != null)
            g.drawString(description, 12 * DESCRIPTION_START, i * FONT_SIZE + 22);
         g.drawString(elapsedTime, 12 * ELAPSED_TIME_START, i * FONT_SIZE + 22);
         g.drawString(numberSims, 12 * NUMBER_SIMS_START, i * FONT_SIZE + 22);
         g.drawString(approxTime, 12 * APPROX_TIME_START, i * FONT_SIZE + 22);
         g.drawString(lastTime, 12 * LAST_TIME_START, i * FONT_SIZE + 22);

      }
   }

}


<|start_filename|>ihmc-perception/src/main/java/us/ihmc/ihmcPerception/depthData/PointCloudDataReceiverInterface.java<|end_filename|>
package us.ihmc.ihmcPerception.depthData;

import java.util.ArrayList;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.tuple3D.Point3D;

public interface PointCloudDataReceiverInterface
{
   void receivedPointCloudData(ReferenceFrame scanFrame, ReferenceFrame lidarFrame, long[] timestamps, ArrayList<Point3D> points, PointCloudSource... sources);

}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/BlackFlyParameterPacketPubSubType.java<|end_filename|>
package controller_msgs.msg.dds;

/**
* 
* Topic data type of the struct "BlackFlyParameterPacket" defined in "BlackFlyParameterPacket_.idl". Use this class to provide the TopicDataType to a Participant. 
*
* This file was automatically generated from BlackFlyParameterPacket_.idl by us.ihmc.idl.generator.IDLGenerator. 
* Do not update this file directly, edit BlackFlyParameterPacket_.idl instead.
*
*/
public class BlackFlyParameterPacketPubSubType implements us.ihmc.pubsub.TopicDataType<controller_msgs.msg.dds.BlackFlyParameterPacket>
{
   public static final java.lang.String name = "controller_msgs::msg::dds_::BlackFlyParameterPacket_";

   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.BlackFlyParameterPacket 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.BlackFlyParameterPacket data) throws java.io.IOException
   {
      deserializeCDR.deserialize(serializedPayload);
      read(data, deserializeCDR);
      deserializeCDR.finishDeserialize();
   }

   public static int getMaxCdrSerializedSize()
   {
      return getMaxCdrSerializedSize(0);
   }

   public static int getMaxCdrSerializedSize(int current_alignment)
   {
      int initial_alignment = current_alignment;

      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);

      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 += 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      return current_alignment - initial_alignment;
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.BlackFlyParameterPacket data)
   {
      return getCdrSerializedSize(data, 0);
   }

   public final static int getCdrSerializedSize(controller_msgs.msg.dds.BlackFlyParameterPacket data, int current_alignment)
   {
      int initial_alignment = 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);


      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 += 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 += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);



      return current_alignment - initial_alignment;
   }

   public static void write(controller_msgs.msg.dds.BlackFlyParameterPacket data, us.ihmc.idl.CDR cdr)
   {
      cdr.write_type_4(data.getSequenceId());

      cdr.write_type_7(data.getAutoExposure());

      cdr.write_type_7(data.getAutoGain());

      cdr.write_type_7(data.getAutoShutter());

      cdr.write_type_6(data.getExposure());

      cdr.write_type_6(data.getFrameRate());

      cdr.write_type_7(data.getFromUi());

      cdr.write_type_6(data.getGain());

      cdr.write_type_6(data.getShutter());

      cdr.write_type_9(data.getRobotSide());

   }

   public static void read(controller_msgs.msg.dds.BlackFlyParameterPacket data, us.ihmc.idl.CDR cdr)
   {
      data.setSequenceId(cdr.read_type_4());
      	
      data.setAutoExposure(cdr.read_type_7());
      	
      data.setAutoGain(cdr.read_type_7());
      	
      data.setAutoShutter(cdr.read_type_7());
      	
      data.setExposure(cdr.read_type_6());
      	
      data.setFrameRate(cdr.read_type_6());
      	
      data.setFromUi(cdr.read_type_7());
      	
      data.setGain(cdr.read_type_6());
      	
      data.setShutter(cdr.read_type_6());
      	
      data.setRobotSide(cdr.read_type_9());
      	

   }

   @Override
   public final void serialize(controller_msgs.msg.dds.BlackFlyParameterPacket data, us.ihmc.idl.InterchangeSerializer ser)
   {
      ser.write_type_4("sequence_id", data.getSequenceId());
      ser.write_type_7("auto_exposure", data.getAutoExposure());
      ser.write_type_7("auto_gain", data.getAutoGain());
      ser.write_type_7("auto_shutter", data.getAutoShutter());
      ser.write_type_6("exposure", data.getExposure());
      ser.write_type_6("frame_rate", data.getFrameRate());
      ser.write_type_7("from_ui", data.getFromUi());
      ser.write_type_6("gain", data.getGain());
      ser.write_type_6("shutter", data.getShutter());
      ser.write_type_9("robot_side", data.getRobotSide());
   }

   @Override
   public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_msgs.msg.dds.BlackFlyParameterPacket data)
   {
      data.setSequenceId(ser.read_type_4("sequence_id"));
      data.setAutoExposure(ser.read_type_7("auto_exposure"));
      data.setAutoGain(ser.read_type_7("auto_gain"));
      data.setAutoShutter(ser.read_type_7("auto_shutter"));
      data.setExposure(ser.read_type_6("exposure"));
      data.setFrameRate(ser.read_type_6("frame_rate"));
      data.setFromUi(ser.read_type_7("from_ui"));
      data.setGain(ser.read_type_6("gain"));
      data.setShutter(ser.read_type_6("shutter"));
      data.setRobotSide(ser.read_type_9("robot_side"));
   }

   public static void staticCopy(controller_msgs.msg.dds.BlackFlyParameterPacket src, controller_msgs.msg.dds.BlackFlyParameterPacket dest)
   {
      dest.set(src);
   }

   @Override
   public controller_msgs.msg.dds.BlackFlyParameterPacket createData()
   {
      return new controller_msgs.msg.dds.BlackFlyParameterPacket();
   }
   @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.BlackFlyParameterPacket data, us.ihmc.idl.CDR cdr)
   {
      write(data, cdr);
   }

   public void deserialize(controller_msgs.msg.dds.BlackFlyParameterPacket data, us.ihmc.idl.CDR cdr)
   {
      read(data, cdr);
   }
   
   public void copy(controller_msgs.msg.dds.BlackFlyParameterPacket src, controller_msgs.msg.dds.BlackFlyParameterPacket dest)
   {
      staticCopy(src, dest);
   }

   @Override
   public BlackFlyParameterPacketPubSubType newInstance()
   {
      return new BlackFlyParameterPacketPubSubType();
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/NeuralNetwork.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork;

import java.util.ArrayList;

import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.ActivationFunction;
import us.ihmc.robotics.functionApproximation.NeuralNetwork.activationFunction.PassThrough;
import us.ihmc.robotics.functionApproximation.NeuralNetwork.importing.NeuralNetworkConfiguration;

/**
 * Simple garbage free production neural network with no training ability. Train your Neural Network using an external library like TensorFlow and import the parameters. 
 * Does not currently support feedback. 
 */
public class NeuralNetwork
{
   private final ArrayList<Layer> layers = new ArrayList<>();
   private String[] inputOrder;
   private String[] outputOrder;
   
   public NeuralNetwork()
   {
   }

   /**
    * Construct a neural network using a NeuralNetworkConfiguration, this is useful if you want to save and load your NN parameters to a YAML file
    * @param config the neural network configuration
    */
   public NeuralNetwork(NeuralNetworkConfiguration config)
   {
      int[] numberOfNeuronsPerLayer = config.getNumberOfNeuronsPerLayer();
      createInputLayer(numberOfNeuronsPerLayer[0]);
      
      ActivationFunction[] activationFunctions = config.getActivationFunctions();
      double[][] bias = config.getBias();
      double[][][] weights = config.getWeights();
      
      //we already created the input layer so start with an index of one
      for(int i = 1; i < numberOfNeuronsPerLayer.length; i++)
      {
         createLayer(numberOfNeuronsPerLayer[i], bias[i], weights[i], activationFunctions[i]);
      }
      inputOrder = config.getInputVariableNames();
      outputOrder = config.getOutputVariableNames();
   }

   /**
    * Creates the initial layer with zero bias and a PassThrough Activation function (the output = the input)
    * @param numberOfNeurons the number of input variables
    */
   public void createInputLayer(int numberOfNeurons)
   {
      if (layers.size() > 0)
      {
         throw new IllegalArgumentException("Neural Network already contains a layer, make sure you create your input layer first! (or fix this class to be more modular)");
      }
      PassThrough passThrough = new PassThrough();
      Layer layer = new Layer();
      for(int i = 0; i < numberOfNeurons; i++)
      {
         layer.addNeuron(new Neuron(passThrough, 0.0));
      }
      layers.add(layer);
   }
   
   /**
    * Creates a hidden layer and connects every node of this layer to every node of the previous layer
    * @param numberOfNeurons the number of neurons in this neural network
    * @param bias the constant added to the output of the neuron
    * @param weights the weight used to scale the inputs. 
    *    [this layer neuron 0][previous layer neuron 0 weight, previous layer neuron 1 weight, etc]
    *    [this layer neuron 1][previous layer neuron 0 weight, previous layer neuron 1 weight, etc]
    * @param activationFunction the "transfer function" used by the neuron to compute the output
    */
   public void createLayer(int numberOfNeurons, double[] bias, double[][] weights, ActivationFunction activationFunction)
   {
      if (layers.size() < 1)
      {
         throw new IllegalArgumentException("Neural Network does not contain an input layer, make sure you create your input layer first! (or fix this class to be more modular)");
      }
      
      Layer previousLayer = layers.get(layers.size() - 1);
      ArrayList<Neuron> previousLayerNeurons = previousLayer.getNeurons();
      
      Layer layer = new Layer();
      
      for(int currentLayerNeuronIndex = 0; currentLayerNeuronIndex < numberOfNeurons; currentLayerNeuronIndex++)
      {
         Neuron neuron = new Neuron(activationFunction, bias[currentLayerNeuronIndex]);
         for(int previousLayerNeuronIndex = 0 ; previousLayerNeuronIndex < previousLayerNeurons.size(); previousLayerNeuronIndex++)
         {
            double weight = weights[currentLayerNeuronIndex][previousLayerNeuronIndex];
            Neuron inputNeuron = previousLayerNeurons.get(previousLayerNeuronIndex);
            neuron.addInputNeuron(inputNeuron, weight);
         }
         layer.addNeuron(neuron);
      }
      layers.add(layer);
   }
   
   /**
    * The input of the neural network. input.length should equal layer zero's number of neurons
    * @param input, index 0 is neuron 0, index 1 is neuron 1, etc
    */
   public void setInput(double[] input)
   {
      Layer inputLayer = layers.get(0);
      ArrayList<Neuron> inputNeurons = inputLayer.getNeurons();
      if (input.length != inputNeurons.size())
      {
         throw new IllegalArgumentException("input array does not equal NN input size");
      }
      
      for(int i = 0; i < inputNeurons.size(); i++)
      {
         Neuron neuron = inputNeurons.get(i);
         // Input variables are stored as bias in the input layer
         neuron.setBias(input[i]);
      }
   }

   /**
    * computes the output of the NN based on the current inputs
    * @param output the results stored in the last layer, output.length should equal the number of neurons in the last layer
    */
   public void compute(double[] output)
   {
      Layer outputLayer = layers.get(layers.size() - 1);
      ArrayList<Neuron> outputNeurons = outputLayer.getNeurons();

      if (output.length != outputNeurons.size())
      {
         throw new IllegalArgumentException("output array does not equal NN output size");
      }

      for (int i = 0; i < layers.size(); i++)
      {
         layers.get(i).compute();
      }

      for (int i = 0; i < outputNeurons.size(); i++)
      {
         output[i] = outputNeurons.get(i).getOutput();
      }
   }

   public String[] getInputOrder()
   {
      return inputOrder;
   }

   public String[] getOutputOrder()
   {
      return outputOrder;
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/packets/ObjectValidityChecker.java<|end_filename|>
package us.ihmc.communication.packets;

import static java.lang.Double.isInfinite;
import static java.lang.Double.isNaN;

import us.ihmc.euclid.axisAngle.AxisAngle;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.euclid.tuple4D.Quaternion;
import us.ihmc.euclid.tuple4D.interfaces.Tuple4DBasics;

public abstract class ObjectValidityChecker
{

   public enum ObjectErrorType
   {
      NULL, CONTAINS_NaN, CONTAINS_INFINITY, INVALID_TIME_VALUE, WRONG_ARRAY_SIZE, NOT_NORMALIZED;

      private int expectedArraySize, actualArraySize;

      public void setExpectedArraySize(int expectedArraySize)
      {
         this.expectedArraySize = expectedArraySize;
      }

      public void setActualArraySize(int actualArraySize)
      {
         this.actualArraySize = actualArraySize;
      }

      public String getMessage()
      {
         switch (this)
         {
         case CONTAINS_INFINITY:
            return "contains infinite value";
         case CONTAINS_NaN:
            return "contains NaN value";
         case INVALID_TIME_VALUE:
            return "is invalid time value";
         case NULL:
            return "is null";
         case WRONG_ARRAY_SIZE:
            return "has a different size than expected. Expected size: " + expectedArraySize + ", actual size: " + actualArraySize;
         case NOT_NORMALIZED:
            return "Indicates that something isn't normalized, e.g. a non-unit Quaternion.";
         default:
            throw new RuntimeException("Enum value not handled: " + this);
         }
      }
   }

   /**
    * Checks the validity of a {@link Tuple2DBasics}
    * @param tuple2dToCheck
    * @return null if the tuple2d is valid, or the error message.
    */
   public static ObjectErrorType validateTuple2d(Tuple2DBasics tuple2dToCheck)
   {
      if (tuple2dToCheck == null)
         return ObjectErrorType.NULL;
      if (isNaN(tuple2dToCheck.getX()) || isNaN(tuple2dToCheck.getY()))
         return ObjectErrorType.CONTAINS_NaN;
      if (isInfinite(tuple2dToCheck.getX()) || isInfinite(tuple2dToCheck.getY()))
         return ObjectErrorType.CONTAINS_INFINITY;

      return null;
   }

   /**
    * Checks the validity of a {@link Tuple3DBasics}
    * @param tuple3dToCheck
    * @return null if the tuple3d is valid, or the error message.
    */
   public static ObjectErrorType validateTuple3d(Tuple3DBasics tuple3dToCheck)
   {
      if (tuple3dToCheck == null)
         return ObjectErrorType.NULL;
      if (isNaN(tuple3dToCheck.getX()) || isNaN(tuple3dToCheck.getY()) || isNaN(tuple3dToCheck.getZ()))
         return ObjectErrorType.CONTAINS_NaN;
      if (isInfinite(tuple3dToCheck.getX()) || isInfinite(tuple3dToCheck.getY()) || isInfinite(tuple3dToCheck.getZ()))
         return ObjectErrorType.CONTAINS_INFINITY;

      return null;
   }

   /**
    * Checks the validity of a {@link Tuple4DBasics}
    * @param tuple4dToCheck
    * @return null if the tuple4d is valid, or the error message.
    */
   public static ObjectErrorType validateTuple4d(Tuple4DBasics tuple4dToCheck)
   {
      if (tuple4dToCheck == null)
         return ObjectErrorType.NULL;
      if (isNaN(tuple4dToCheck.getX()) || isNaN(tuple4dToCheck.getY()) || isNaN(tuple4dToCheck.getZ()) || isNaN(tuple4dToCheck.getS()))
         return ObjectErrorType.CONTAINS_NaN;
      if (isInfinite(tuple4dToCheck.getX()) || isInfinite(tuple4dToCheck.getY()) || isInfinite(tuple4dToCheck.getZ()) || isInfinite(tuple4dToCheck.getS()))
         return ObjectErrorType.CONTAINS_INFINITY;

      return null;
   }

   public static ObjectErrorType validateQuat4d(Quaternion quat4dToCheck)
   {
      ObjectErrorType objectErrorType = validateTuple4d(quat4dToCheck);
      if(objectErrorType != null)
      {
         return objectErrorType;
      }
      else
      {
         boolean quaternionNormalized = quat4dToCheck.isUnitary(1.0e-5);
         if(!quaternionNormalized)
         {
            return ObjectErrorType.NOT_NORMALIZED;
         }
         else
         {
            return null;
         }
      }
   }

   /**
    * Checks the validity of a {@link AxisAngle}
    * @param axisAngleToCheck
    * @return null if the axisAngle4d is valid, or the error message.
    */
   public static ObjectErrorType validateAxisAngle4d(AxisAngle axisAngleToCheck)
   {
      if (axisAngleToCheck == null)
         return ObjectErrorType.NULL;
      if (isNaN(axisAngleToCheck.getX()) || isNaN(axisAngleToCheck.getY()) || isNaN(axisAngleToCheck.getZ()) || isNaN(axisAngleToCheck.getAngle()))
         return ObjectErrorType.CONTAINS_NaN;
      if (isInfinite(axisAngleToCheck.getX()) || isInfinite(axisAngleToCheck.getY()) || isInfinite(axisAngleToCheck.getZ())
            || isInfinite(axisAngleToCheck.getAngle()))
         return ObjectErrorType.CONTAINS_INFINITY;

      return null;
   }

   /**
    * Checks the validity of a {@link double}
    * @param doubleToCheck
    * @return null if the double is valid, or the error message.
    */
   public static ObjectErrorType validateDouble(double doubleToCheck)
   {
      if (isNaN(doubleToCheck))
         return ObjectErrorType.CONTAINS_NaN;
      if (isInfinite(doubleToCheck))
         return ObjectErrorType.CONTAINS_INFINITY;

      return null;
   }

   /**
    * Checks the validity of a trajectoryTime
    * @param trajectoryTimeToCheck
    * @return null if the trajectoryTime is valid, or the error message.
    */
   public static ObjectErrorType validateTrajectoryTime(double trajectoryTimeToCheck)
   {
      if (isNaN(trajectoryTimeToCheck))
         return ObjectErrorType.CONTAINS_NaN;
      if (isInfinite(trajectoryTimeToCheck))
         return ObjectErrorType.CONTAINS_INFINITY;
      if (trajectoryTimeToCheck < 0.0)
         return ObjectErrorType.INVALID_TIME_VALUE;

      return null;
   }

   /**
    * Checks the validity of an array of double
    * @param doubleArrayToCheck
    * @param expectedSize
    * @return null if the array is of the expected size and if each element of the array is valid, or return the error message.
    */
   public static ObjectErrorType validateArrayOfDouble(double[] doubleArrayToCheck, int expectedSize)
   {
      if (doubleArrayToCheck.length != expectedSize)
      {
         ObjectErrorType wrongArraySize = ObjectErrorType.WRONG_ARRAY_SIZE;
         wrongArraySize.setActualArraySize(doubleArrayToCheck.length);
         wrongArraySize.setExpectedArraySize(expectedSize);
         return wrongArraySize;
      }
      for (int arrayIndex = 0; arrayIndex < doubleArrayToCheck.length; arrayIndex++)
      {
         ObjectErrorType validateDouble = validateDouble(doubleArrayToCheck[arrayIndex]);
         if (validateDouble != null)
            return validateDouble;
      }

      return null;
   }

   /**
    * Checks the validity of an enum
    * @param enumToCheck
    * @return null if the enum is valid, or the error message.
    */
   public static ObjectErrorType validateEnum(Enum<?> enumToCheck)
   {
      if (enumToCheck == null)
         return ObjectErrorType.NULL;

      return null;
   }
}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/factories/HighLevelControllerStateFactory.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.factories;

import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelControllerFactoryHelper;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;

public interface HighLevelControllerStateFactory
{
   HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper);

   HighLevelControllerName getStateEnum();

   default boolean isTransitionToControllerRequested()
   {
      return false;
   }
}


<|start_filename|>robot-environment-awareness/src/main/java/us/ihmc/robotEnvironmentAwareness/ui/properties/BoundingBoxParametersProperty.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui.properties;

import javafx.beans.property.Property;
import us.ihmc.robotEnvironmentAwareness.communication.packets.BoundingBoxParametersMessage;

public class BoundingBoxParametersProperty extends ParametersProperty<BoundingBoxParametersMessage>
{
   private final FloatField minX = new FloatField(BoundingBoxParametersMessage::getMinX, (p, v) -> p.setMinX(v));
   private final FloatField minY = new FloatField(BoundingBoxParametersMessage::getMinY, (p, v) -> p.setMinY(v));
   private final FloatField minZ = new FloatField(BoundingBoxParametersMessage::getMinZ, (p, v) -> p.setMinZ(v));
   private final FloatField maxX = new FloatField(BoundingBoxParametersMessage::getMaxX, (p, v) -> p.setMaxX(v));
   private final FloatField maxY = new FloatField(BoundingBoxParametersMessage::getMaxY, (p, v) -> p.setMaxY(v));
   private final FloatField maxZ = new FloatField(BoundingBoxParametersMessage::getMaxZ, (p, v) -> p.setMaxZ(v));

   public BoundingBoxParametersProperty(Object bean, String name)
   {
      super(bean, name, new BoundingBoxParametersMessage());
   }

   public void binBidirectionalMinX(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minX);
   }

   public void binBidirectionalMinY(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minY);
   }

   public void binBidirectionalMinZ(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, minZ);
   }
   
   public void binBidirectionalMaxX(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxX);
   }
   
   public void binBidirectionalMaxY(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxY);
   }
   
   public void binBidirectionalMaxZ(Property<? extends Number> property)
   {
      bindFieldBidirectionalToNumberProperty(property, maxZ);
   }

   @Override
   protected BoundingBoxParametersMessage getValueCopy(BoundingBoxParametersMessage valueToCopy)
   {
      return new BoundingBoxParametersMessage(valueToCopy);
   }
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/EnumDependentInputMapping.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public interface EnumDependentInputMapping<T extends Enum<T>>
{
   public T getEnum();
}


<|start_filename|>example-simulations/src/main/java/us/ihmc/exampleSimulations/controllerCore/ControllerCoreModeChangedListener.java<|end_filename|>
package us.ihmc.exampleSimulations.controllerCore;

import us.ihmc.commonWalkingControlModules.controllerCore.WholeBodyControllerCoreMode;

public interface ControllerCoreModeChangedListener
{
   void controllerCoreModeHasChanged(WholeBodyControllerCoreMode newMode);
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/packetCommunicator/BlackoutPacketConsumer.java<|end_filename|>
package us.ihmc.communication.packetCommunicator;

import us.ihmc.communication.blackoutGenerators.CommunicationBlackoutSimulator;
import us.ihmc.communication.net.ObjectConsumer;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.communication.packets.Packet;

public class BlackoutPacketConsumer<P extends Packet<P>> implements ObjectConsumer<P>
{
   private final PacketConsumer<P> packetConsumer;
   private CommunicationBlackoutSimulator blackoutSimulator;
   
   public BlackoutPacketConsumer(PacketConsumer<P> packetConsumer, CommunicationBlackoutSimulator blackoutSimulator)
   {
      this.packetConsumer = packetConsumer;
      this.blackoutSimulator = blackoutSimulator;
   }
   
   @Override
   public void consumeObject(P packet)
   {
      if(blackoutSimulator != null && !blackoutSimulator.blackoutCommunication())
         packetConsumer.receivedPacket(packet);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandCollisionDetectedPacket.java<|end_filename|>
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;

/**
       * Message for the IHMC humanoid behavior module.
       */
public class HandCollisionDetectedPacket extends Packet<HandCollisionDetectedPacket> implements Settable<HandCollisionDetectedPacket>, EpsilonComparable<HandCollisionDetectedPacket>
{
   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_;
   public byte robot_side_ = (byte) 255;
   public int collision_severity_level_one_to_three_;

   public HandCollisionDetectedPacket()
   {
   }

   public HandCollisionDetectedPacket(HandCollisionDetectedPacket other)
   {
      this();
      set(other);
   }

   public void set(HandCollisionDetectedPacket other)
   {
      sequence_id_ = other.sequence_id_;

      robot_side_ = other.robot_side_;

      collision_severity_level_one_to_three_ = other.collision_severity_level_one_to_three_;

   }

   /**
            * 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 void setRobotSide(byte robot_side)
   {
      robot_side_ = robot_side;
   }
   public byte getRobotSide()
   {
      return robot_side_;
   }

   public void setCollisionSeverityLevelOneToThree(int collision_severity_level_one_to_three)
   {
      collision_severity_level_one_to_three_ = collision_severity_level_one_to_three;
   }
   public int getCollisionSeverityLevelOneToThree()
   {
      return collision_severity_level_one_to_three_;
   }


   public static Supplier<HandCollisionDetectedPacketPubSubType> getPubSubType()
   {
      return HandCollisionDetectedPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HandCollisionDetectedPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HandCollisionDetectedPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.collision_severity_level_one_to_three_, other.collision_severity_level_one_to_three_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HandCollisionDetectedPacket)) return false;

      HandCollisionDetectedPacket otherMyClass = (HandCollisionDetectedPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.robot_side_ != otherMyClass.robot_side_) return false;

      if(this.collision_severity_level_one_to_three_ != otherMyClass.collision_severity_level_one_to_three_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HandCollisionDetectedPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("robot_side=");
      builder.append(this.robot_side_);      builder.append(", ");
      builder.append("collision_severity_level_one_to_three=");
      builder.append(this.collision_severity_level_one_to_three_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/diagnostics/logging/DiagnosticLoggerConfiguration.java<|end_filename|>
package us.ihmc.wholeBodyController.diagnostics.logging;

import java.io.FileOutputStream;
import java.io.IOException;
import java.io.PrintStream;
import java.net.InetAddress;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.time.LocalDateTime;
import java.time.ZoneOffset;
import java.time.format.DateTimeFormatter;
import java.util.logging.FileHandler;
import java.util.logging.Filter;
import java.util.logging.Formatter;
import java.util.logging.Handler;
import java.util.logging.Level;
import java.util.logging.LogRecord;
import java.util.logging.Logger;

import org.apache.commons.io.output.TeeOutputStream;
import org.apache.commons.net.ntp.NTPUDPClient;
import org.apache.commons.net.ntp.TimeInfo;

import us.ihmc.yoVariables.variable.YoDouble;

public class DiagnosticLoggerConfiguration
{
   public static void setupLogging(YoDouble yoTime, Class<?> clazz, String robotName)
   {
      setupLogging(yoTime, clazz, robotName, false);
   }

   public static void setupLogging(YoDouble yoTime, Class<?> clazz, String robotName, boolean useInternetDate)
   {
      DateTimeFormatter dateTimeFormatter = DateTimeFormatter.ofPattern("yyyyMMdd_HHmmss");

      LocalDateTime now;

      if (!useInternetDate)
      {
         now = LocalDateTime.now();
      }
      else
      {
         now = getTimeFromServer();
         if (now == null)
            now = LocalDateTime.now();
      }

      String timestamp = now.format(dateTimeFormatter);

      Path diagnosticOutputDirectory = Paths.get(System.getProperty("user.home"), ".ihmc", "Diagnostic",
            timestamp + "_" + robotName + "_" + clazz.getSimpleName() + "_Outputs");
      try
      {
         Files.createDirectories(diagnosticOutputDirectory);
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }

      DiagnosticLoggerFormatter formatter = new DiagnosticLoggerFormatter(yoTime);

      setupSystemOut(diagnosticOutputDirectory);
      setupLogFiles(diagnosticOutputDirectory, formatter);
   }
   
   private static LocalDateTime getTimeFromServer()
   {
      InetAddress inetAddress;
      try
      {
         String TIME_SERVER = "192.168.3.11";
//         String TIME_SERVER = "nist1-macon.macon.ga.us"; // Does not work on Atlas :'(
         NTPUDPClient timeClient = new NTPUDPClient();
         inetAddress = InetAddress.getByName(TIME_SERVER);
         TimeInfo timeInfo = timeClient.getTime(inetAddress);
         timeInfo.computeDetails();
         long actualTime = timeInfo.getReturnTime() + timeInfo.getOffset();
         LocalDateTime time = LocalDateTime.ofEpochSecond(actualTime / 1000, (int) (actualTime % 1000) * 1000000, ZoneOffset.UTC);
         return time;
      }
      catch (IOException e)
      {
         e.printStackTrace();
         return null;
      }
   }

   private static void setupLogFiles(Path diagnosticOutputDirectory, DiagnosticLoggerFormatter formatter)
   {
      // Hack to get the root logger
      Logger rootLogger = Logger.getGlobal().getParent();

      Handler[] handlers = rootLogger.getHandlers();
      
      for (Handler handler : handlers)
         rootLogger.removeHandler(handler);

      rootLogger.addHandler(new DiagnosticLoggerSystemOutHandler(formatter, Level.INFO));
      rootLogger.addHandler(new DiagnosticLoggerSystemErrHandler(formatter, Level.INFO));
      
      Filter severeFilter = new Filter()
      {
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return record.getLevel() == Level.SEVERE;
         }
      };
      addFileHandler("severe", severeFilter, formatter, diagnosticOutputDirectory, rootLogger);

      Filter warningFilter = new Filter()
      {
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return record.getLevel() == Level.WARNING;
         }
      };
      addFileHandler("warning", warningFilter, formatter, diagnosticOutputDirectory, rootLogger);

      Filter noFilter = new Filter()
      {
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return true;
         }
      };
      addFileHandler("all", noFilter, formatter, diagnosticOutputDirectory, rootLogger);

      Filter processedJointPositionDelayFiter = new Filter()
      {
         
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return record instanceof ProcessedJointPositionDelayLogRecord;
         }
      };
      addFileHandler("processedJointPositionDelay", processedJointPositionDelayFiter, formatter, diagnosticOutputDirectory, rootLogger);

      Filter processedJointVelocityDelayFiter = new Filter()
      {
         
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return record instanceof ProcessedJointVelocityDelayLogRecord;
         }
      };
      addFileHandler("processedJointVelocityDelay", processedJointVelocityDelayFiter, formatter, diagnosticOutputDirectory, rootLogger);

      Filter jointForceTrackingDelayFiter = new Filter()
      {
         
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return record instanceof JointForceTrackingDelayLogRecord;
         }
      };
      addFileHandler("jointForceTrackingDelay", jointForceTrackingDelayFiter, formatter, diagnosticOutputDirectory, rootLogger);

      Filter rawJointVelocityDelayFilter = new Filter()
      {
         
         @Override
         public boolean isLoggable(LogRecord record)
         {
            return record instanceof RawJointVelocityDelayLogRecord;
         }
      };
      addFileHandler("rawJointVelocityDelay", rawJointVelocityDelayFilter, formatter, diagnosticOutputDirectory, rootLogger);
   }

   private static void addFileHandler(String fileName, Filter filter, Formatter formatter, Path diagnosticOutputDirectory, Logger rootLogger)
   {
      try
      {
         String fullFileName = Paths.get(diagnosticOutputDirectory.toString(), fileName + ".log").toString();
         Handler fileHandler = new FileHandler(fullFileName);
         fileHandler.setFormatter(formatter);
         fileHandler.setFilter(filter);
         rootLogger.addHandler(fileHandler);
      }
      catch (SecurityException | IOException e)
      {
         e.printStackTrace();
      }
      
   }

   private static void setupSystemOut(Path diagnosticOutputDirectory)
   {
      String consoleOutputPath = Paths.get(diagnosticOutputDirectory.toString(), "consoleOutput.log").toString();

      try
      {
         // we will want to print in standard "System.out" and in "consoleOutput.log"
         TeeOutputStream newOut = new TeeOutputStream(System.out, new FileOutputStream(consoleOutputPath));
         System.setOut(new PrintStream(newOut));
      }
      catch (Exception e)
      {
         e.printStackTrace();
      }
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/UIPositionCheckerPacket.java<|end_filename|>
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;

public class UIPositionCheckerPacket extends Packet<UIPositionCheckerPacket> implements Settable<UIPositionCheckerPacket>, EpsilonComparable<UIPositionCheckerPacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public us.ihmc.euclid.tuple3D.Point3D position_;
   public us.ihmc.euclid.tuple4D.Quaternion orientation_;

   public UIPositionCheckerPacket()
   {
      position_ = new us.ihmc.euclid.tuple3D.Point3D();
      orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
   }

   public UIPositionCheckerPacket(UIPositionCheckerPacket other)
   {
      this();
      set(other);
   }

   public void set(UIPositionCheckerPacket other)
   {
      sequence_id_ = other.sequence_id_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.position_, position_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.orientation_, orientation_);
   }

   /**
            * 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 us.ihmc.euclid.tuple3D.Point3D getPosition()
   {
      return position_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getOrientation()
   {
      return orientation_;
   }


   public static Supplier<UIPositionCheckerPacketPubSubType> getPubSubType()
   {
      return UIPositionCheckerPacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return UIPositionCheckerPacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(UIPositionCheckerPacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!this.position_.epsilonEquals(other.position_, epsilon)) return false;
      if (!this.orientation_.epsilonEquals(other.orientation_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof UIPositionCheckerPacket)) return false;

      UIPositionCheckerPacket otherMyClass = (UIPositionCheckerPacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if (!this.position_.equals(otherMyClass.position_)) return false;
      if (!this.orientation_.equals(otherMyClass.orientation_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("UIPositionCheckerPacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("position=");
      builder.append(this.position_);      builder.append(", ");
      builder.append("orientation=");
      builder.append(this.orientation_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robot-builder/src/main/java/us/ihmc/robotbuilder/util/Util.java<|end_filename|>
package us.ihmc.robotbuilder.util;

import javafx.application.Platform;
import javafx.geometry.Bounds;
import javafx.scene.Node;
import javafx.scene.PerspectiveCamera;
import javafx.scene.transform.Affine;
import javafx.scene.transform.MatrixType;
import javafx.scene.transform.Rotate;
import javafx.scene.transform.Translate;
import javaslang.concurrent.Future;
import javaslang.concurrent.Promise;

import us.ihmc.euclid.tuple3D.Vector3D;
import java.util.concurrent.Callable;

/**
 * Utility methods
 */
public class Util {
    /**
     * Wraps {@link Platform#runLater(Runnable)} to use a {@link Future} which allows chaining
     * and waiting for actions.
     * @param callable code to run in the UI thread
     * @param <T> return type of the UI code
     * @return {@link Future} instance for the given code that is fulfilled as soon as the given code executes in the UI thread
     */
    public static <T> Future<T> runLaterInUI(Callable<? extends T> callable) {
        Promise<T> result = Promise.make();
        Platform.runLater(() -> {
            try {
                result.success(callable.call());
            } catch (Throwable thr) {
                result.failure(thr);
            }
        });
        return result.future();
    }

    /**
     * Computes camera settings so that the camera shows the whole node (based on its bounding box) from the direction
     * given by the dir vector if corner == false or from the direction of a bounding box corner if corner == true
     * @param node node to look at
     * @param fovDegrees camera field of view in degrees
     * @param dir direction where to look at the node from
     * @param up up vector
     */
    public static PerspectiveCamera lookAtNodeFromDirection(final Node node, double fovDegrees, final Vector3D dir, final Vector3D up) {
        // We get a bounding sphere of the node and calculate how far the camera needs
        // to be to fit in the whole sphere in its view.
        final Bounds box = node.getBoundsInParent();
        if (box.isEmpty()) {
            return new PerspectiveCamera(true);
        }

        Vector3D dirVec = new Vector3D(dir);
        dirVec.normalize();
        Vector3D bboxMin = new Vector3D(box.getMinX(), box.getMinY(), box.getMinZ());
        Vector3D bboxMax = new Vector3D(box.getMaxX(), box.getMaxY(), box.getMaxZ());
        Vector3D sceneCenter = new Vector3D(bboxMin);
        sceneCenter.add(bboxMax);
        sceneCenter.scale(0.5f);

        Vector3D radiusVec = new Vector3D(sceneCenter);
        radiusVec.sub(bboxMin);
        double r = radiusVec.length(); // scene bounding sphere radius
        double near = r / 1000; // camera near, giving some space for zoom-in
        double h = Math.sin(Math.toRadians(fovDegrees) / 2) * near; // half of the height of the projection plane
        double d = r * near / h; // how far we need to be for the bounding sphere to fit in the projection rectangle

        double far = 10 * (2 * r + d); // (distance from the scene + scene bounding sphere diameter) * 10 (allows a 10x zoom-out)

        Vector3D cameraLocation = new Vector3D(dirVec);
        cameraLocation.scale(-d);
        cameraLocation.add(sceneCenter);

        PerspectiveCamera result = new PerspectiveCamera(true);
        result.setFieldOfView(fovDegrees);
        result.setNearClip(near);
        result.setFarClip(far);

        result.getTransforms().add(new Translate(cameraLocation.getX(), cameraLocation.getY(), cameraLocation.getZ()));
        result.getTransforms().add(lookAt(cameraLocation, sceneCenter, up));
        // Adjust to JavaFX world (z up, right handed)
        result.getTransforms().add(new Rotate(180, Rotate.X_AXIS));
        result.getTransforms().add(new Rotate(90, Rotate.Z_AXIS));
        return result;
    }

    /**
     * Get the rotation matrix from a camera location, target location and camera up vector.
     * Returns a 4x4 affine matrix for convenience.
     * @param cameraLocation camera location
     * @param target target location
     * @param up up vector
     * @return transform representing the rotation
     */
    private static Affine lookAt(Vector3D cameraLocation, Vector3D target, Vector3D up) {
        // Adjusted from JMonkey Engine code
        Vector3D newDirection = new Vector3D();
        Vector3D newUp = new Vector3D();
        Vector3D newLeft = new Vector3D();

        newDirection.set(target);
        newDirection.sub(cameraLocation);
        newDirection.normalize();

        newUp.set(up);
        newUp.normalize();
        if (newUp.lengthSquared() < 1e-8) {
            newUp.set(0, 1, 0);
        }

        newLeft.cross(newUp, newDirection);
        newLeft.normalize();
        if (newLeft.lengthSquared() < 1e-8) {
            if (newDirection.getX() != 0) {
                //noinspection SuspiciousNameCombination
                newLeft.set(newDirection.getY(), -newDirection.getX(), 0f);
            } else {
                newLeft.set(0f, newDirection.getZ(), -newDirection.getY());
            }
        }

        newUp.set(newDirection);
        newUp.cross(newDirection, newLeft);
        newUp.normalize();

        return new Affine(new double[] {
            newLeft.getX(), newLeft.getY(), newLeft.getZ(), 0,
            newUp.getX(), newUp.getY(), newUp.getZ(), 0,
            newDirection.getX(), newDirection.getY(), newDirection.getZ(), 0,
            0, 0, 0, 1
        }, MatrixType.MT_3D_4x4, 0);
    }
}


<|start_filename|>ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/ros/PeriodicRosHighLevelStatePublisher.java<|end_filename|>
package us.ihmc.avatar.ros;

import java.util.concurrent.TimeUnit;

import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage;
import us.ihmc.communication.net.PacketConsumer;
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
import us.ihmc.util.PeriodicNonRealtimeThreadScheduler;
import us.ihmc.utilities.ros.RosMainNode;

/**
 *
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
public class PeriodicRosHighLevelStatePublisher implements PacketConsumer<HighLevelStateChangeStatusMessage>, Runnable
{
   private final RosHighLevelStatePublisher statePublisher = new RosHighLevelStatePublisher(false);
   private final RosMainNode rosMainNode;

   private HighLevelControllerName currentState = HighLevelControllerName.DO_NOTHING_BEHAVIOR;
   private final PeriodicNonRealtimeThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler(getClass().getName());

   public PeriodicRosHighLevelStatePublisher(RosMainNode rosMainNode, String rosNameSpace)
   {
      this.rosMainNode = rosMainNode;
      initialize(rosNameSpace);
   }

   private void initialize(String rosNameSpace)
   {
      rosMainNode.attachPublisher(rosNameSpace + "/output/high_level_state", statePublisher);

      scheduler.schedule(this, 1, TimeUnit.MILLISECONDS);
   }

   @Override
   public void receivedPacket(HighLevelStateChangeStatusMessage packet)
   {
      if (packet.getEndHighLevelControllerName() != currentState.toByte())
      {
         currentState = HighLevelControllerName.fromByte(packet.getEndHighLevelControllerName());
      }
   }

   @Override
   public void run()
   {
      if (Thread.interrupted())
      {
         scheduler.shutdown();
      }
      else if (rosMainNode.isStarted())
      {
         statePublisher.publish(currentState);
      }
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/robotController/SensorProcessor.java<|end_filename|>
package us.ihmc.robotics.robotController;

import us.ihmc.simulationconstructionset.util.RobotControlElement;

public interface SensorProcessor extends RobotControlElement
{
   public abstract void update();
}


<|start_filename|>ihmc-path-planning/src/main/java/us/ihmc/pathPlanning/statistics/ListOfStatistics.java<|end_filename|>
package us.ihmc.pathPlanning.statistics;

import java.util.ArrayList;
import java.util.List;

public class ListOfStatistics implements PlannerStatistics<ListOfStatistics>
{
   private final List<PlannerStatistics<?>> statisticsList = new ArrayList<>();

   @Override
   public StatisticsType getStatisticsType()
   {
      return StatisticsType.LIST;
   }

   @Override
   public void set(ListOfStatistics listOfStatistics)
   {
      clear();
      addStatisticsList(listOfStatistics);
   }

   public void addStatistics(PlannerStatistics<?> statistics)
   {
      if (statistics != null)
      {
         if (statistics instanceof ListOfStatistics)
            addStatisticsList((ListOfStatistics) statistics);
         else
            statisticsList.add(statistics);
      }
   }

   public void addStatisticsList(ListOfStatistics listOfStatistics)
   {
      if (listOfStatistics == null)
         return;

      for (int i = 0; i < listOfStatistics.getNumberOfStatistics(); i++)
         addStatistics(listOfStatistics.getStatistics(i));
   }

   public PlannerStatistics<?> getStatistics(int statisticsIndex)
   {
      return statisticsList.get(statisticsIndex);
   }

   public void clear()
   {
      statisticsList.clear();
   }

   public int getNumberOfStatistics()
   {
      return statisticsList.size();
   }

   public PlannerStatistics<?> pollStatistics()
   {
      if (statisticsList.isEmpty())
         return null;
      else
         return statisticsList.remove(getNumberOfStatistics() - 1);
   }

}


<|start_filename|>ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/stateTransitionConditions/SingleSupportToTransferToCondition.java<|end_filename|>
package us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.stateTransitionConditions;

import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
import us.ihmc.robotics.stateMachine.core.StateTransitionCondition;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.states.SingleSupportState;
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.walkingController.states.TransferState;

public class SingleSupportToTransferToCondition implements StateTransitionCondition
{
   private final SingleSupportState singleSupportState;
   private final TransferState transferState;
   private final WalkingMessageHandler walkingMessageHandler;

   public SingleSupportToTransferToCondition(SingleSupportState singleSupportState, TransferState transferState, WalkingMessageHandler walkingMessageHandler)
   {
      this.singleSupportState = singleSupportState;
      this.transferState = transferState;
      this.walkingMessageHandler = walkingMessageHandler;
   }

   @Override
   public boolean testCondition(double timeInState)
   {
      if (!singleSupportState.isDone(timeInState))
         return false;
      return walkingMessageHandler.isNextFootstepFor(transferState.getTransferToSide().getOppositeSide());
   }
}

<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/joystick/GraspingPaneController.java<|end_filename|>
package us.ihmc.valkyrie.joystick;

import javafx.fxml.FXML;
import javafx.scene.Parent;
import us.ihmc.javaFXToolkit.messager.JavaFXMessager;

public class GraspingPaneController
{
   @FXML
   private Parent graspingObjectPane;

   @FXML
   private GraspingFingerPaneController graspingFingerPaneController;

   public GraspingPaneController()
   {

   }

   public void initialize(JavaFXMessager messager)
   {
      graspingFingerPaneController.initialize(messager);
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/QuadrupedBodyHeightMessage.java<|end_filename|>
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 quadruped controller API.
       * This message commands the controller to move in taskspace the body to the desired height while going through the specified trajectory points.
       * Sending this command will not affect the pelvis horizontal position.
       */
public class QuadrupedBodyHeightMessage extends Packet<QuadrupedBodyHeightMessage> implements Settable<QuadrupedBodyHeightMessage>, EpsilonComparable<QuadrupedBodyHeightMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean is_expressed_in_absolute_time_;
   /**
            * If true, the body height is controlled, rather than the center of mass height
            */
   public boolean control_body_height_;
   /**
            * The orientation trajectory information.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage euclidean_trajectory_;

   public QuadrupedBodyHeightMessage()
   {
      euclidean_trajectory_ = new controller_msgs.msg.dds.EuclideanTrajectoryMessage();
   }

   public QuadrupedBodyHeightMessage(QuadrupedBodyHeightMessage other)
   {
      this();
      set(other);
   }

   public void set(QuadrupedBodyHeightMessage other)
   {
      sequence_id_ = other.sequence_id_;

      is_expressed_in_absolute_time_ = other.is_expressed_in_absolute_time_;

      control_body_height_ = other.control_body_height_;

      controller_msgs.msg.dds.EuclideanTrajectoryMessagePubSubType.staticCopy(other.euclidean_trajectory_, euclidean_trajectory_);
   }

   /**
            * 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_;
   }

   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public void setIsExpressedInAbsoluteTime(boolean is_expressed_in_absolute_time)
   {
      is_expressed_in_absolute_time_ = is_expressed_in_absolute_time;
   }
   /**
            * If true, the times are expressed in absolute time relative to the robot start
            */
   public boolean getIsExpressedInAbsoluteTime()
   {
      return is_expressed_in_absolute_time_;
   }

   /**
            * If true, the body height is controlled, rather than the center of mass height
            */
   public void setControlBodyHeight(boolean control_body_height)
   {
      control_body_height_ = control_body_height;
   }
   /**
            * If true, the body height is controlled, rather than the center of mass height
            */
   public boolean getControlBodyHeight()
   {
      return control_body_height_;
   }


   /**
            * The orientation trajectory information.
            */
   public controller_msgs.msg.dds.EuclideanTrajectoryMessage getEuclideanTrajectory()
   {
      return euclidean_trajectory_;
   }


   public static Supplier<QuadrupedBodyHeightMessagePubSubType> getPubSubType()
   {
      return QuadrupedBodyHeightMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return QuadrupedBodyHeightMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(QuadrupedBodyHeightMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_expressed_in_absolute_time_, other.is_expressed_in_absolute_time_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.control_body_height_, other.control_body_height_, epsilon)) return false;

      if (!this.euclidean_trajectory_.epsilonEquals(other.euclidean_trajectory_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof QuadrupedBodyHeightMessage)) return false;

      QuadrupedBodyHeightMessage otherMyClass = (QuadrupedBodyHeightMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.is_expressed_in_absolute_time_ != otherMyClass.is_expressed_in_absolute_time_) return false;

      if(this.control_body_height_ != otherMyClass.control_body_height_) return false;

      if (!this.euclidean_trajectory_.equals(otherMyClass.euclidean_trajectory_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("QuadrupedBodyHeightMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("is_expressed_in_absolute_time=");
      builder.append(this.is_expressed_in_absolute_time_);      builder.append(", ");
      builder.append("control_body_height=");
      builder.append(this.control_body_height_);      builder.append(", ");
      builder.append("euclidean_trajectory=");
      builder.append(this.euclidean_trajectory_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/atlas/AtlasLowLevelControlMode.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.atlas;

public enum AtlasLowLevelControlMode
{
   STAND_PREP, FREEZE;

   public static final AtlasLowLevelControlMode[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static AtlasLowLevelControlMode fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>valkyrie/src/main/java/us/ihmc/valkyrie/fingers/ValkyrieFingerMotorName.java<|end_filename|>
package us.ihmc.valkyrie.fingers;

import us.ihmc.robotics.partNames.FingerName;
import us.ihmc.robotics.robotSide.RobotSide;

public enum ValkyrieFingerMotorName
{
   ThumbMotorRoll, ThumbMotorPitch1, ThumbMotorPitch2, IndexFingerMotorPitch1, MiddleFingerMotorPitch1, PinkyMotorPitch1;

   public static final ValkyrieFingerMotorName[] values = values();

   public String getJointName(RobotSide robotSide)
   {
      return getCamelCaseJointName(robotSide);
   }

   public String getCamelCaseJointName(RobotSide side)
   {
      return side.getCamelCaseName() + name();
   }

   public String getPascalCaseJointName(RobotSide side)
   {
      return side.getPascalCaseName() + name();
   }

   public FingerName getFingerName()
   {
      switch (this)
      {
      case ThumbMotorRoll:
      case ThumbMotorPitch1:
      case ThumbMotorPitch2:
         return FingerName.THUMB;
      case IndexFingerMotorPitch1:
         return FingerName.INDEX;
      case MiddleFingerMotorPitch1:
         return FingerName.MIDDLE;
      case PinkyMotorPitch1:
         return FingerName.PINKY;
      default:
         throw new RuntimeException("Unexpected " + getClass().getSimpleName() + " value: " + this);
      }
   }

   public ValkyrieHandJointName getCorrespondingJointName(int depth)
   {
      switch (this)
      {
      case ThumbMotorRoll:
         return ValkyrieHandJointName.ThumbRoll;
      case ThumbMotorPitch1:
      case ThumbMotorPitch2:
         if (depth == 1)
            return ValkyrieHandJointName.ThumbPitch1;
         else if (depth == 2)
            return ValkyrieHandJointName.ThumbPitch2;
         else
            return ValkyrieHandJointName.ThumbPitch3;

      case IndexFingerMotorPitch1:
         if (depth == 1)
            return ValkyrieHandJointName.IndexFingerPitch1;
         else if (depth == 2)
            return ValkyrieHandJointName.IndexFingerPitch2;
         else
            return ValkyrieHandJointName.IndexFingerPitch3;

      case MiddleFingerMotorPitch1:
         if (depth == 1)
            return ValkyrieHandJointName.MiddleFingerPitch1;
         else if (depth == 2)
            return ValkyrieHandJointName.MiddleFingerPitch2;
         else
            return ValkyrieHandJointName.MiddleFingerPitch3;

      case PinkyMotorPitch1:
         if (depth == 1)
            return ValkyrieHandJointName.PinkyPitch1;
         else if (depth == 2)
            return ValkyrieHandJointName.PinkyPitch2;
         else
            return ValkyrieHandJointName.PinkyPitch3;

      default:
         return null;
      }
   }
   
   public byte toByte()
   {
      return (byte) ordinal();
   }
   
   public static ValkyrieFingerMotorName fromByte(byte enumAsByte)
   {
      if (enumAsByte == -1)
         return null;
      return values[enumAsByte];
   }
}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/streamingData/PersistentTCPClient.java<|end_filename|>
package us.ihmc.communication.streamingData;

import java.io.IOException;
import java.io.InputStream;
import java.io.ObjectInputStream;
import java.net.Socket;
import java.net.UnknownHostException;
import java.util.concurrent.atomic.AtomicBoolean;

import us.ihmc.commons.thread.ThreadTools;

public class PersistentTCPClient
{
   private final String serverIPAddress;
   private final int serverPort;
   private final EstablishedAConnectionListener establishedAConnectionListener;

   private Socket socket;
   private InputStream inputStream;
   private ObjectInputStream objectInputStream;

   private AtomicBoolean connected = new AtomicBoolean(false);
   
   private boolean keepOnTrying = false;
   
   private static final boolean DEBUG = false;
   
   public PersistentTCPClient(String serverIPAddress, int serverPort, EstablishedAConnectionListener establishedAConnectionListener)
   {
      this.serverIPAddress = serverIPAddress;
      this.serverPort = serverPort;
      
      this.establishedAConnectionListener = establishedAConnectionListener;
   }

   public void connectToServer(boolean persistentlyStayConnected)
   {
      if (persistentlyStayConnected)
      {
         keepOnTrying = true;

         StreamingDataTCPClientPersistentConnector streamingDataTCPClientPersistentConnector = new StreamingDataTCPClientPersistentConnector();
         Thread thread = new Thread(streamingDataTCPClientPersistentConnector);
         thread.setDaemon(true);
         thread.start();
      }

      else
      {
         attemptToCreateSocketAndStreams();
         if (!connected.get())
            return;

         establishedAConnectionListener.establishedAConnection(objectInputStream, null);
      }
   }
   
   
   private class StreamingDataTCPClientPersistentConnector implements Runnable
   {
      public void run()
      {
         while (keepOnTrying)
         {
            if (!(isConnected()))
            {
               printIfDebug("!! Trying To Connect !!");

               attemptToCreateSocketAndStreams();
               if (socket != null)
               {
                  establishedAConnectionListener.establishedAConnection(objectInputStream, null);
               }
            }
            
            while(isConnected())
            { 
               ThreadTools.sleep(100L);
            }

            printIfDebug("!! Closing Streams and Sockets !!");
            
            closeStreamsAndSockets();
            // Sleep and try seeing if the server is available in a couple seconds.
            ThreadTools.sleep(100L);
         }
      }
   }
   
   
   private void attemptToCreateSocketAndStreams()
   {
      try
      {
         if (DEBUG)
            System.out.println("!! Creating Socket on " + serverIPAddress +":" + serverPort);
         
         socket = new Socket(serverIPAddress, serverPort); 
         inputStream = socket.getInputStream();
         objectInputStream = new ObjectInputStream(inputStream);  
         
         connected.set(true);
      }
      catch (UnknownHostException e)
      {
         System.err.println("Unknown Host: " + serverIPAddress);
      }
      catch (IOException e)
      {
          System.err.println("Failed to create socket" + e.getMessage());
          e.printStackTrace();
      }
   }
   
   
   public void notifyConnectionBroke()
   {
      closeStreamsAndSockets();
      connected.set(false);
   }
   
   private void closeStreamsAndSockets()
   {
      try
      {
         if (objectInputStream != null)
            objectInputStream.close();
      }
      catch (IOException e)
      {
      }

      try
      {
         if (inputStream != null)
            inputStream.close();
      }
      catch (IOException e)
      {
      }

      try
      {
         if (socket != null)
            socket.close();
      }
      catch (IOException e)
      {
      }

      socket = null;
   }
   
   
   public void close()
   {
      keepOnTrying = false;
      
      closeStreamsAndSockets();
   }

   public boolean isConnected()
   {
      return connected.get();
      
      // Note: It would be nice to just be able to check this way below, but it doesn't work.
      // The only way you can know is if you experienced an exception when trying to read or write.
      // So the EstablishedAConnectionListener has to let you know...
      
//      // These two checks one would think would be enough, but no. 
//      // socket.isConnected always returns true once there is a connection.
//      if (socket == null) return false;
//      if (!socket.isConnected()) return false; 
//      if (socket.isClosed()) return false;
//      
//      if (objectInputStream == null) return false;
//
//      // To fully see if it is connected, you need to do something with it.
//      // Checking to see if there are bytes available should do the trick.
//      try
//      {
//         objectInputStream.available();
//      } 
//      catch (Exception e)
//      {
//         return false;
//      }
//      
//      return true;
   }
   
   private static void printIfDebug(String message)
   {
      if (DEBUG)
      {
         System.out.println(message);
      }
   }
}


<|start_filename|>ihmc-manipulation-planning/src/main/java/us/ihmc/manipulation/planning/rrt/configurationAndTimeSpace/CTTaskNodeTreeExploringTest.java<|end_filename|>
package us.ihmc.manipulation.planning.rrt.configurationAndTimeSpace;

import us.ihmc.commons.PrintTools;

public class CTTaskNodeTreeExploringTest
{
   int numberOfTasks = 5;

   CTTaskNode rootNode;

   CTTaskNodeTree tree;

   TaskRegion taskRegion;

   CTTreeVisualizer treeVisualizer;

   public CTTaskNodeTreeExploringTest()
   {
      rootNode = new CTTaskNode(numberOfTasks + 1);
      rootNode.setNodeData(0, 0.0);

      for (int i = 1; i < numberOfTasks + 1; i++)
         rootNode.setNodeData(i, 0.5);
      
      
      rootNode.setNodeData(1, 0.1);
      rootNode.setNodeData(3, 0.9);
      rootNode.setNodeData(4, 0.9);

      taskRegion = new TaskRegion(numberOfTasks + 1);

      for (int i = 0; i < numberOfTasks + 1; i++)
         taskRegion.setRandomRegion(i, 0.0, 1.0);

      rootNode.convertDataToNormalizedData(taskRegion);

      tree = new CTTaskNodeTree(rootNode);

      tree.setTaskRegion(taskRegion);

      treeVisualizer = new CTTreeVisualizer(tree, true);
      treeVisualizer.initialize();

      treeVisualizer.update(rootNode);

      for (int i = 0; i < 1000; i++)
      {
         expandTree();
         if (tree.getNewNode().getTime() == taskRegion.getTrajectoryTime())
         {
            PrintTools.info("Finished " + i);
            break;
         }  
      }
   }

   private void expandTree()
   {
      tree.updateRandomConfiguration();

      tree.updateNearestNodeTaskTime();
      tree.updateNewConfiguration();
      tree.getNewNode().convertNormalizedDataToData(taskRegion);
      tree.getNewNode().setParentNode(tree.getNearNode());

      updateValidity(tree.getNewNode());
      if (tree.getNewNode().getValidity())
      {
         tree.connectNewNode(true);
         if (tree.getNewNode().getTime() == taskRegion.getTrajectoryTime())
         {
         }
      }
      else
      {         
         tree.connectNewNode(false);
                  
         tree.updateNearestNodeTaskOnly();         
         
         tree.updateNewConfiguration();
         tree.getNewNode().convertNormalizedDataToData(taskRegion);
         
         tree.getNewNode().setParentNode(tree.getNearNode());

         updateValidity(tree.getNewNode());
         if (tree.getNewNode().getValidity())
         {            
            tree.connectNewNode(true);
            if (tree.getNewNode().getTime() == taskRegion.getTrajectoryTime())
            {
            }
         }
      }

      treeVisualizer.update(tree.getNewNode());
   }

   private void updateValidity(CTTaskNode node)
   {
      if (0.4 < node.getNormalizedNodeData(0) && node.getNormalizedNodeData(0) < 0.6)
      {
         boolean eq1 = 0.3 < node.getNormalizedNodeData(1) && node.getNormalizedNodeData(1) < 0.7;
         boolean eq2 = 0.4 < node.getNormalizedNodeData(2) && node.getNormalizedNodeData(2) < 0.6;
         if (eq1 || eq2)
         {
            node.setValidity(false);
         }
      }

      if (0.8 < node.getNormalizedNodeData(0) && node.getNormalizedNodeData(0) < 0.95)
      {
         boolean eq1 = 0.1 < node.getNormalizedNodeData(1) && node.getNormalizedNodeData(1) < 0.4;
         if (eq1)
         {
            node.setValidity(false);
         }
      }
      
   }

   public static void main(String[] args)
   {
      System.out.println("Exploring test for CTNodeTree");

      CTTaskNodeTreeExploringTest test = new CTTaskNodeTreeExploringTest();

      System.out.println("Finished");
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/quadTree/QuadTreeForGround.java<|end_filename|>
package us.ihmc.robotics.quadTree;

import java.util.ArrayList;
import java.util.Collection;

import us.ihmc.euclid.tuple3D.Point3D;

/**
 * A quad tree for representing ground height maps.
 *
 * Uses a pretty straightforward implementation.
 * Does some filtering based on the parameters.
 * When a node gets to or smaller than the resolution, then the value of the node will be the average value of some of the points stored in the node.
 * As more than a certain amount of points get put into a small node, the points will start overwriting previous points randomly.
 *
 *
 * TODO: Later add functionality for clearing just a portion of the quad tree or for remembering the age of points and removing old points.
 * TODO: But for now, can only add to the quad tree, not take away.
 */
public class QuadTreeForGround
{
   private final ArrayList<QuadTreeForGroundListener> listeners = new ArrayList<QuadTreeForGroundListener>();
   private final QuadTreeForGroundNode root;
   private final Box bounds;

   private final QuadTreeForGroundParameters quadTreeParameters;
   private final Point3D nearestPointForHeightAt = new Point3D();
   private final QuadTreeForGroundPointLimiter pointLimiter;
   
   
   public QuadTreeForGround(double minX, double minY, double maxX, double maxY, double resolution, double heightThreshold,
                             double maxMultiLevelZChangeToFilterNoise, int maxSameHeightPointsPerNode,
                             double maxAllowableXYDistanceForAPointToBeConsideredClose)
   {
      this(new Box(minX, minY, maxX, maxY), new QuadTreeForGroundParameters(resolution, heightThreshold, maxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode,
            maxAllowableXYDistanceForAPointToBeConsideredClose, -1));
   }

   public QuadTreeForGround(Box bounds, QuadTreeForGroundParameters quadTreeParameters)
   {
      this.bounds = bounds;
      this.quadTreeParameters = quadTreeParameters;
      
      if(quadTreeParameters.getMaximumNumberOfPoints() > 0)
      {
         pointLimiter = new QuadTreeForGroundPointLimiter(quadTreeParameters.getMaximumNumberOfPoints());
      }
      else 
      {
         pointLimiter = null;
      }
      this.root = new QuadTreeForGroundNode("root", bounds, quadTreeParameters, pointLimiter, null, Double.NaN, listeners);
   }

   public int getNumberOfPoints()
   {
      return pointLimiter.size();
   }
   
   public QuadTreeForGroundParameters getQuadTreeParameters()
   {
      return quadTreeParameters;
   }

   public void setHeightThreshold(double heightThreshold)
   {
      this.quadTreeParameters.setHeightThreshold(heightThreshold);
   }

   public void addQuadTreeListener(QuadTreeForGroundListener quadTreeListener)
   {
      listeners.add(quadTreeListener);
      ArrayList<QuadTreeForGroundNode> listOfCurrentNodes = getAllVisibleNodes();

      for (QuadTreeForGroundNode node : listOfCurrentNodes)
      {
         double x = node.getBounds().centreX;
         double y = node.getBounds().centreY;
         double z = 0.0f;

         QuadTreeForGroundLeaf leaf = node.getLeaf();
         if (leaf != null)
         {
            Point3D averagePoint = leaf.getAveragePoint();
            x = averagePoint.getX();
            y = averagePoint.getY();
            z = averagePoint.getZ();
         }

         quadTreeListener.nodeAdded(node.getID(), node.getBounds(), (float) x, (float) y, (float) z);
      }
   }

   private ArrayList<QuadTreeForGroundNode> getAllVisibleNodes()
   {
      ArrayList<QuadTreeForGroundNode> list = new ArrayList<QuadTreeForGroundNode>();
      root.getAllDescendantNodes(list);

      return list;
   }

   public synchronized QuadTreeForGroundPutResult put(double x, double y, double z)
   {
      for (QuadTreeForGroundListener listener : listeners)
      {
         listener.RawPointAdded((float) x, (float) y, (float) z);
      }

      return root.put(x, y, z);
   }
   
   public boolean isEmpty()
   {
      return root.isEmpty();
   }

   public synchronized void clear()
   {
      root.clear();
   }


   public synchronized double getHeightAtPoint(double x, double y)
   {
      if (!bounds.containsOrEquals(x, y))
         return Double.NaN;

      nearestPointForHeightAt.set(Double.NaN, Double.NaN, Double.NaN);
      PointAndDistance pointAndDistance = new PointAndDistance(nearestPointForHeightAt,
                                             quadTreeParameters.getMaxAllowableXYDistanceForAPointToBeConsideredClose());
      root.getClosestPointAndDistance(x, y, pointAndDistance);

      double heightToReturn = nearestPointForHeightAt.getZ();
      
      if (Double.isNaN(heightToReturn))
      {
         heightToReturn = root.getDefaultHeightWhenNoPoints();
      }
      return heightToReturn;
      
   }

   public synchronized void getClosestPoint(double xQuery, double yQuery, Point3D pointToPack)
   {
      root.getClosestPoint(xQuery, yQuery, pointToPack);
   }

   public synchronized void getAllPointsWithinDistance(double x, double y, double distance, ArrayList<Point3D> pointsWithinDistanceToPack)
   {
      this.root.getAllPointsWithinDistance(x, y, distance, pointsWithinDistanceToPack);
   }

   public synchronized void getAllPointsWithinBounds(Box bounds, ArrayList<Point3D> pointsWithinBoundsToPack)
   {
      this.root.getAllPointsWithBounds(bounds, pointsWithinBoundsToPack);
   }

   public double getMinX()
   {
      return root.getBounds().minX;
   }

   public double getMaxX()
   {
      return root.getBounds().maxX;
   }

   public double getMinY()
   {
      return root.getBounds().minY;
   }

   public double getMaxY()
   {
      return root.getBounds().maxY;
   }

   public synchronized int getNumberOfQuads()
   {
      return root.getNumberOfChildren();
   }

   public QuadTreeForGroundNode getRootNode()
   {
      return root;
   }
   
   public void setDefaultHeightWhenNoPoints(double defaultHeightWhenNoPonts)
   {
      root.setDefaultHeightWhenNoPoints(defaultHeightWhenNoPonts);
   }
   
   protected double getDefaultHeightWhenNoPoints()
   {
      return root.getDefaultHeightWhenNoPoints();
   }

   public synchronized void getStoredPoints(Collection<Point3D> points)
   {
      root.getAllSubTreePoints(points);
   }

   public synchronized void getCellAverageStoredPoints(Collection<Point3D> points)
   {
      root.getCellAverageSubTreePoints(points);
   }

   public void checkRepInvarients()
   {
      root.checkRepInvarients();
   }


}


<|start_filename|>ihmc-communication/src/main/java/us/ihmc/communication/ros/generators/RosEnumValueDocumentation.java<|end_filename|>
package us.ihmc.communication.ros.generators;

import java.lang.annotation.Documented;
import java.lang.annotation.ElementType;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;
import java.lang.annotation.Target;

/**
 * @author <NAME> <a href="mailto:<EMAIL>">(<EMAIL>)</a>
 */
@Documented
@Retention(RetentionPolicy.RUNTIME)
@Target(ElementType.FIELD)
public @interface RosEnumValueDocumentation
{
   String documentation();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HeightQuadTreeLeafMessage.java<|end_filename|>
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 height quad tree module
       */
public class HeightQuadTreeLeafMessage extends Packet<HeightQuadTreeLeafMessage> implements Settable<HeightQuadTreeLeafMessage>, EpsilonComparable<HeightQuadTreeLeafMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public float center_x_;
   public float center_y_;
   public float height_;

   public HeightQuadTreeLeafMessage()
   {
   }

   public HeightQuadTreeLeafMessage(HeightQuadTreeLeafMessage other)
   {
      this();
      set(other);
   }

   public void set(HeightQuadTreeLeafMessage other)
   {
      sequence_id_ = other.sequence_id_;

      center_x_ = other.center_x_;

      center_y_ = other.center_y_;

      height_ = other.height_;

   }

   /**
            * 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 void setCenterX(float center_x)
   {
      center_x_ = center_x;
   }
   public float getCenterX()
   {
      return center_x_;
   }

   public void setCenterY(float center_y)
   {
      center_y_ = center_y;
   }
   public float getCenterY()
   {
      return center_y_;
   }

   public void setHeight(float height)
   {
      height_ = height;
   }
   public float getHeight()
   {
      return height_;
   }


   public static Supplier<HeightQuadTreeLeafMessagePubSubType> getPubSubType()
   {
      return HeightQuadTreeLeafMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HeightQuadTreeLeafMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HeightQuadTreeLeafMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.center_x_, other.center_x_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.center_y_, other.center_y_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.height_, other.height_, epsilon)) return false;


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HeightQuadTreeLeafMessage)) return false;

      HeightQuadTreeLeafMessage otherMyClass = (HeightQuadTreeLeafMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.center_x_ != otherMyClass.center_x_) return false;

      if(this.center_y_ != otherMyClass.center_y_) return false;

      if(this.height_ != otherMyClass.height_) return false;


      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HeightQuadTreeLeafMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("center_x=");
      builder.append(this.center_x_);      builder.append(", ");
      builder.append("center_y=");
      builder.append(this.center_y_);      builder.append(", ");
      builder.append("height=");
      builder.append(this.height_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/packets/sensing/StateEstimatorMode.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.packets.sensing;

public enum StateEstimatorMode
{
   NORMAL, FROZEN;
   
   public static StateEstimatorMode[] values = values();

   public byte toByte()
   {
      return (byte) ordinal();
   }

   public static StateEstimatorMode fromByte(byte enumAsByte)
   {
      return values[enumAsByte];
   }
}

<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/WallPosePacket.java<|end_filename|>
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 humanoid behavior module.
       */
public class WallPosePacket extends Packet<WallPosePacket> implements Settable<WallPosePacket>, EpsilonComparable<WallPosePacket>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public double cutting_radius_ = 0.2;
   public us.ihmc.euclid.tuple3D.Point3D center_position_;
   public us.ihmc.euclid.tuple4D.Quaternion center_orientation_;

   public WallPosePacket()
   {
      center_position_ = new us.ihmc.euclid.tuple3D.Point3D();
      center_orientation_ = new us.ihmc.euclid.tuple4D.Quaternion();
   }

   public WallPosePacket(WallPosePacket other)
   {
      this();
      set(other);
   }

   public void set(WallPosePacket other)
   {
      sequence_id_ = other.sequence_id_;

      cutting_radius_ = other.cutting_radius_;

      geometry_msgs.msg.dds.PointPubSubType.staticCopy(other.center_position_, center_position_);
      geometry_msgs.msg.dds.QuaternionPubSubType.staticCopy(other.center_orientation_, center_orientation_);
   }

   /**
            * 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 void setCuttingRadius(double cutting_radius)
   {
      cutting_radius_ = cutting_radius;
   }
   public double getCuttingRadius()
   {
      return cutting_radius_;
   }


   public us.ihmc.euclid.tuple3D.Point3D getCenterPosition()
   {
      return center_position_;
   }


   public us.ihmc.euclid.tuple4D.Quaternion getCenterOrientation()
   {
      return center_orientation_;
   }


   public static Supplier<WallPosePacketPubSubType> getPubSubType()
   {
      return WallPosePacketPubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return WallPosePacketPubSubType::new;
   }

   @Override
   public boolean epsilonEquals(WallPosePacket other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.cutting_radius_, other.cutting_radius_, epsilon)) return false;

      if (!this.center_position_.epsilonEquals(other.center_position_, epsilon)) return false;
      if (!this.center_orientation_.epsilonEquals(other.center_orientation_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof WallPosePacket)) return false;

      WallPosePacket otherMyClass = (WallPosePacket) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.cutting_radius_ != otherMyClass.cutting_radius_) return false;

      if (!this.center_position_.equals(otherMyClass.center_position_)) return false;
      if (!this.center_orientation_.equals(otherMyClass.center_orientation_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("WallPosePacket {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("cutting_radius=");
      builder.append(this.cutting_radius_);      builder.append(", ");
      builder.append("center_position=");
      builder.append(this.center_position_);      builder.append(", ");
      builder.append("center_orientation=");
      builder.append(this.center_orientation_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-robot-data-visualizer/src/main/java/us/ihmc/robotDataVisualizer/visualizer/SixDoFJointUpdater.java<|end_filename|>
package us.ihmc.robotDataVisualizer.visualizer;

import us.ihmc.euclid.matrix.RotationMatrix;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotDataLogger.jointState.SixDoFState;
import us.ihmc.simulationconstructionset.FloatingJoint;

public class SixDoFJointUpdater extends JointUpdater
{
   
   private final FloatingJoint joint;
   private final SixDoFState jointState;

   private final RotationMatrix rotationMatrix = new RotationMatrix();
   private final Vector3D tempVector = new Vector3D();

   public SixDoFJointUpdater(FloatingJoint joint, SixDoFState jointState)
   {
      this.joint = joint;
      this.jointState = jointState;
   }

   @Override
   public void update()
   {

      
      jointState.getRotation(rotationMatrix);
      jointState.getTranslation(tempVector);
      joint.setRotation(rotationMatrix);
      joint.setPosition(tempVector);

      
      jointState.getTwistAngularPart(tempVector);
      joint.setAngularVelocityInBody(tempVector);
      
      jointState.getTwistLinearPart(tempVector);
      joint.setVelocity(tempVector);
   }
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/LoadBearingMessage.java<|end_filename|>
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.
       * Message carrying the information needed to enable load bearing for a end-effector.
       * A contact point will be used to enable this feature. This point is attached to the end-effector.
       */
public class LoadBearingMessage extends Packet<LoadBearingMessage> implements Settable<LoadBearingMessage>, EpsilonComparable<LoadBearingMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   /**
            * If set to true this will load the contact point. Otherwise the rigid body will stop bearing load.
            */
   public boolean load_;
   /**
            * Sets the coefficient of friction that the controller will use for the contact point.
            */
   public double coefficient_of_friction_;
   /**
            * Sets the pose of the contact frame in the frame of the end effector body.
            */
   public us.ihmc.euclid.geometry.Pose3D body_frame_to_contact_frame_;
   /**
            * Sets the contact normal used by the controller to load the contact point.
            */
   public us.ihmc.euclid.tuple3D.Vector3D contact_normal_in_world_frame_;

   public LoadBearingMessage()
   {
      body_frame_to_contact_frame_ = new us.ihmc.euclid.geometry.Pose3D();
      contact_normal_in_world_frame_ = new us.ihmc.euclid.tuple3D.Vector3D();
   }

   public LoadBearingMessage(LoadBearingMessage other)
   {
      this();
      set(other);
   }

   public void set(LoadBearingMessage other)
   {
      sequence_id_ = other.sequence_id_;

      load_ = other.load_;

      coefficient_of_friction_ = other.coefficient_of_friction_;

      geometry_msgs.msg.dds.PosePubSubType.staticCopy(other.body_frame_to_contact_frame_, body_frame_to_contact_frame_);
      geometry_msgs.msg.dds.Vector3PubSubType.staticCopy(other.contact_normal_in_world_frame_, contact_normal_in_world_frame_);
   }

   /**
            * 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_;
   }

   /**
            * If set to true this will load the contact point. Otherwise the rigid body will stop bearing load.
            */
   public void setLoad(boolean load)
   {
      load_ = load;
   }
   /**
            * If set to true this will load the contact point. Otherwise the rigid body will stop bearing load.
            */
   public boolean getLoad()
   {
      return load_;
   }

   /**
            * Sets the coefficient of friction that the controller will use for the contact point.
            */
   public void setCoefficientOfFriction(double coefficient_of_friction)
   {
      coefficient_of_friction_ = coefficient_of_friction;
   }
   /**
            * Sets the coefficient of friction that the controller will use for the contact point.
            */
   public double getCoefficientOfFriction()
   {
      return coefficient_of_friction_;
   }


   /**
            * Sets the pose of the contact frame in the frame of the end effector body.
            */
   public us.ihmc.euclid.geometry.Pose3D getBodyFrameToContactFrame()
   {
      return body_frame_to_contact_frame_;
   }


   /**
            * Sets the contact normal used by the controller to load the contact point.
            */
   public us.ihmc.euclid.tuple3D.Vector3D getContactNormalInWorldFrame()
   {
      return contact_normal_in_world_frame_;
   }


   public static Supplier<LoadBearingMessagePubSubType> getPubSubType()
   {
      return LoadBearingMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return LoadBearingMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(LoadBearingMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.load_, other.load_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.coefficient_of_friction_, other.coefficient_of_friction_, epsilon)) return false;

      if (!this.body_frame_to_contact_frame_.epsilonEquals(other.body_frame_to_contact_frame_, epsilon)) return false;
      if (!this.contact_normal_in_world_frame_.epsilonEquals(other.contact_normal_in_world_frame_, epsilon)) return false;

      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof LoadBearingMessage)) return false;

      LoadBearingMessage otherMyClass = (LoadBearingMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.load_ != otherMyClass.load_) return false;

      if(this.coefficient_of_friction_ != otherMyClass.coefficient_of_friction_) return false;

      if (!this.body_frame_to_contact_frame_.equals(otherMyClass.body_frame_to_contact_frame_)) return false;
      if (!this.contact_normal_in_world_frame_.equals(otherMyClass.contact_normal_in_world_frame_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("LoadBearingMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("load=");
      builder.append(this.load_);      builder.append(", ");
      builder.append("coefficient_of_friction=");
      builder.append(this.coefficient_of_friction_);      builder.append(", ");
      builder.append("body_frame_to_contact_frame=");
      builder.append(this.body_frame_to_contact_frame_);      builder.append(", ");
      builder.append("contact_normal_in_world_frame=");
      builder.append(this.contact_normal_in_world_frame_);
      builder.append("}");
      return builder.toString();
   }
}


<|start_filename|>ihmc-java-toolkit/src/main/java/us/ihmc/tools/string/StringAndRegularExpressionMatcher.java<|end_filename|>
package us.ihmc.tools.string;

import java.util.ArrayList;
import java.util.regex.Pattern;

public class StringAndRegularExpressionMatcher
{
   private final ArrayList<String> exactStringsToMatch = new ArrayList<String>();
   private final ArrayList<Pattern> regularExpressionsToMatch = new ArrayList<Pattern>();
   
   private final boolean isCaseSensitive;
   
   public StringAndRegularExpressionMatcher()
   {
      this(true);
   }
   
   public StringAndRegularExpressionMatcher(boolean isCaseSensitive)
   {
      this.isCaseSensitive = isCaseSensitive;
   }
   
   public void addExactStringToMatch(String exactStringToMatch)
   {
      if (!isCaseSensitive)
      {
         exactStringToMatch = exactStringToMatch.toLowerCase();
      }
      
      this.exactStringsToMatch.add(exactStringToMatch);
   }
   
   public void addRegularExpression(String regularExpression)
   {
      Pattern pattern = Pattern.compile(regularExpression);
      this.regularExpressionsToMatch.add(pattern);
   }

   public boolean matches(String stringToTest)
   {
      if (!isCaseSensitive)
      {
         stringToTest = stringToTest.toLowerCase();
      }
      
      if (this.exactStringsToMatch.contains(stringToTest)) return true;

      for (Pattern pattern : regularExpressionsToMatch)
      {
         if (pattern.matcher(stringToTest).matches())
         {
            return true;
         }
      }
      
      return false;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/functionApproximation/NeuralNetwork/importing/NeuralNetworkYamlHelper.java<|end_filename|>
package us.ihmc.robotics.functionApproximation.NeuralNetwork.importing;

import java.io.FileInputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStream;

import org.yaml.snakeyaml.Yaml;

import us.ihmc.robotics.functionApproximation.NeuralNetwork.NeuralNetwork;

public class NeuralNetworkYamlHelper
{
   private NeuralNetworkYamlHelper()
   {
   }

   public static NeuralNetwork createNeuralNetworkFromYamlFile(InputStream inputStream)
   {
      Yaml yaml = new Yaml();
      NeuralNetworkConfiguration config = (NeuralNetworkConfiguration) yaml.load(inputStream);
      NeuralNetwork nn = new NeuralNetwork(config);

      return nn;
   }

   public static void saveNeuralNetworkConfigurationToYamlFile(NeuralNetworkConfiguration config, String fileName)
   {
      Yaml yaml = new Yaml();
      String result = yaml.dump(config);
      FileWriter fileWriter;

      try
      {
         fileWriter = new FileWriter(fileName);
         fileWriter.write(result);
         fileWriter.close();
      }
      catch (IOException e)
      {
         e.printStackTrace();
      }
   }

   /**
    * This is an example configuration created using TensorFlow.
    */
   public static void main(String[] args) throws IOException
   {
      //The neural network doesn't care about this, but the input data should match this ordering
      String[] inputVariableNames = new String[] {"LEFT_ANKLE_PITCH_joint_q", "LEFT_ANKLE_PITCH_joint_q_previous", "LEFT_ANKLE_PITCH_joint_qd",
            "LEFT_ANKLE_PITCH_joint_qd_previous", "LEFT_ANKLE_PITCH_joint_qdd", "LEFT_ANKLE_PITCH_joint_qdd_previous", "LEFT_ANKLE_PITCH_tauMeasured_previous",
            "LEFT_KNEE_PITCH_joint_q", "LEFT_KNEE_PITCH_joint_q_previous", "LEFT_KNEE_PITCH_joint_qd", "LEFT_KNEE_PITCH_joint_qd_previous",
            "LEFT_KNEE_PITCH_joint_qdd", "LEFT_KNEE_PITCH_joint_qdd_previous", "LEFT_KNEE_PITCH_tauMeasured_previous"};

      //This is hacky, but leaving something blank gets a passthrough activation function.
      String[] activationFunctionsPerLayer = new String[] {"", "RELU", ""};

      //The input layer has 16 neurons, one per input variable, the next has 4 neurons, and the output is a single neuron
      int[] numberOfNeuronsPerLayer = new int[] {16, 4, 1};

      //these are the constants added to each of the hidden layer neurons
      double[] hiddenLayerBias = new double[] {0.8374003, 0.03701664, -0.28729317, 0.01319704};

      //this is the constant added to the output neuron
      double[] outputBias = new double[] {-0.3590592};

      //the input layer bias's aren't used but need to be included to make things consistent. then we pass in the hidden layer bias and the output layer bias
      double[][] bias = new double[][] {new double[16], hiddenLayerBias, outputBias};

      //the input layer weights aren't used but need to be included to make things consistent. 
      double[][] inputLayerWeights = new double[16][1];

      //the weights between the input layer and the hidden layer
      double[][] hiddenLayerWeights = new double[4][];
      hiddenLayerWeights[0] = new double[] {-0.46510178, -0.05915822, 0.3717158, -0.45220408, 0.1210492, -0.12430278, -0.29257497, 0.3202149, 0.10641811,
            0.45007122, 0.24379735, -0.13266943, -0.1242077, 0.12984337, 0.62552196, 0.3651869};
      hiddenLayerWeights[1] = new double[] {-0.03552369, 0.7017831, -0.16389109, 0.2945423, -0.10112676, 0.10519658, 0.7274673, -0.2770513, -0.5848242,
            -0.08602782, 0.1309489, -0.2813644, 0.09275991, -0.099305, 1.612328, -0.42894414};
      hiddenLayerWeights[2] = new double[] {0.15122245, 0.5346764, 0.01194376, 0.33071777, -0.05709987, 0.05674522, -0.5670206, 0.0768759, -0.31645796,
            -0.13648617, 0.38817182, -0.31028637, 0.02570818, -0.03258159, 0.1956599, -0.17547709};
      hiddenLayerWeights[3] = new double[] {-0.50018334, -0.5318647, -0.24139157, 0.05813099, 0.15277725, -0.15938585, -0.71538013, 0.7469889, 0.17892408,
            0.76543343, 0.12041978, 0.10104298, -0.15151536, 0.160927, -2.4808238, 0.554581};

      //the weights between the hidden layer and the output layer
      double[][] ouputLayerWeights = new double[1][];
      ouputLayerWeights[0] = new double[] {0.46576336, 0.41093794, -0.01115055, -0.27883196};

      //all the weights together
      double[][][] weights = new double[][][] {inputLayerWeights, hiddenLayerWeights, ouputLayerWeights};

      //save them all in the configuration
      NeuralNetworkConfiguration neuralNetworkConfiguration = new NeuralNetworkConfiguration();
      neuralNetworkConfiguration.setInputVariableNames(inputVariableNames);
      neuralNetworkConfiguration.setActivationFunctionsPerLayer(activationFunctionsPerLayer);
      neuralNetworkConfiguration.setBias(bias);
      neuralNetworkConfiguration.setNumberOfNeuronsPerLayer(numberOfNeuronsPerLayer);
      neuralNetworkConfiguration.setWeights(weights);

      NeuralNetworkYamlHelper.saveNeuralNetworkConfigurationToYamlFile(neuralNetworkConfiguration, "testNNParam.yaml");

      // if you want to make the NN by hand you can simple call
      //      neuralNetwork.createInputLayer(16);
      //      neuralNetwork.createLayer(4, hiddenLayerBias, hiddenLayerWeights, new Relu());
      //      neuralNetwork.createLayer(1, outputBias, ouputLayerWeights, new PassThrough());

      FileInputStream fileInputStream = new FileInputStream("testNNParam.yaml");
      NeuralNetwork neuralNetwork = NeuralNetworkYamlHelper.createNeuralNetworkFromYamlFile(fileInputStream);

      neuralNetwork.setInput(new double[] {0.7041736, 0.7041736, -0.82175085, -0.7660606, -55.92842308, -55.55744086, -0.19960167, 0.20344828, 0.48069055,
            0.48069055, 0.07482435, 0.0718136, 3.15835607, 2.86869009, 1.14953754, -1.47241379});
      //ground truth 1.0482758620689654

      double[] output = new double[1];
      neuralNetwork.compute(output);
      System.out.println(output[0]);
   }
}


<|start_filename|>ihmc-parameter-tuner/src/main/java/us/ihmc/parameterTuner/guiElements/tuners/DoubleTuner.java<|end_filename|>
package us.ihmc.parameterTuner.guiElements.tuners;

import us.ihmc.parameterTuner.guiElements.GuiParameter;

public class DoubleTuner extends NumericTuner<Double>
{
   public DoubleTuner(GuiParameter parameter)
   {
      super(parameter);
   }

   @Override
   public NumericSpinner<Double> createSpinner()
   {
      return new DoubleSpinner();
   }

   @Override
   public NumericSlider<Double> createSlider()
   {
      return new DoubleSlider();
   }
}


<|start_filename|>robot-environment-awareness/src/visualizers/java/us/ihmc/robotEnvironmentAwareness/ui/JavaFXPlanarRegionsViewer.java<|end_filename|>
package us.ihmc.robotEnvironmentAwareness.ui;

import controller_msgs.msg.dds.PlanarRegionsListMessage;
import javafx.animation.AnimationTimer;
import javafx.scene.Node;
import javafx.scene.paint.Color;
import javafx.scene.paint.Material;
import javafx.scene.shape.Mesh;
import javafx.scene.shape.MeshView;
import javafx.util.Pair;
import us.ihmc.communication.packets.PlanarRegionMessageConverter;
import us.ihmc.euclid.geometry.ConvexPolygon2D;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.javaFXToolkit.shapes.JavaFXMultiColorMeshBuilder;
import us.ihmc.javaFXToolkit.shapes.TextureColorPalette2D;
import us.ihmc.robotics.geometry.PlanarRegion;
import us.ihmc.robotics.geometry.PlanarRegionsList;

import java.util.concurrent.atomic.AtomicReference;

public class JavaFXPlanarRegionsViewer
{
   private final JavaFXMultiColorMeshBuilder meshBuilder;

   private final AtomicReference<Pair<Mesh, Material>> meshAndMaterialToRender = new AtomicReference<>(null);

   private final MeshView planarRegionMeshView = new MeshView();

   private final AnimationTimer renderer;

   public JavaFXPlanarRegionsViewer()
   {
      TextureColorPalette2D colorPalette = new TextureColorPalette2D();
      colorPalette.setHueBrightnessBased(0.9);
      meshBuilder = new JavaFXMultiColorMeshBuilder(colorPalette);

      renderer = new AnimationTimer()
      {
         @Override
         public void handle(long now)
         {
            render();
         }
      };
   }

   public void start()
   {
      renderer.start();
   }

   public void stop()
   {
      renderer.stop();
   }

   public void clear()
   {
      meshAndMaterialToRender.set(new Pair<>(null, null));
   }

   public void submitPlanarRegions(PlanarRegionsListMessage planarRegionsListMessage)
   {
      if (planarRegionsListMessage != null)
         meshAndMaterialToRender.set(generateMeshAndMaterial(planarRegionsListMessage));
   }

   private void render()
   {
      Pair<Mesh, Material> meshAndMaterial = meshAndMaterialToRender.getAndSet(null);

      if (meshAndMaterial != null)
      {
         planarRegionMeshView.setMesh(meshAndMaterial.getKey());
         planarRegionMeshView.setMaterial(meshAndMaterial.getValue());
      }
   }

   private Pair<Mesh, Material> generateMeshAndMaterial(PlanarRegionsListMessage newMessage)
   {
      meshBuilder.clear();

      double lineWidth = 0.01;
      RigidBodyTransform transformToWorld = new RigidBodyTransform();
      PlanarRegionsList planarRegionsList = PlanarRegionMessageConverter.convertToPlanarRegionsList(newMessage);

      for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++)
      {
         PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex);

         int regionId = planarRegion.getRegionId();
         Color regionColor = getRegionColor(regionId);
         planarRegion.getTransformToWorld(transformToWorld);

         meshBuilder.addMultiLine(transformToWorld, planarRegion.getConcaveHull(), lineWidth, regionColor, true);

         for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++)
         {
            ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex);
            regionColor = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) polygonIndex / (double) planarRegion.getNumberOfConvexPolygons()));
            meshBuilder.addPolygon(transformToWorld, convexPolygon2d, regionColor);
         }
      }

      Material material = meshBuilder.generateMaterial();
      Mesh mesh = meshBuilder.generateMesh();

      return new Pair<>(mesh, material);
   }

   private Color getRegionColor(int regionId)
   {
      java.awt.Color awtColor = new java.awt.Color(regionId);
      return Color.rgb(awtColor.getRed(), awtColor.getGreen(), awtColor.getBlue());
   }

   public Node getRootNode()
   {
      return planarRegionMeshView;
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/stateMachine/old/eventBasedStateMachine/FiniteStateMachineState.java<|end_filename|>
package us.ihmc.robotics.stateMachine.old.eventBasedStateMachine;

@Deprecated
public interface FiniteStateMachineState<E extends Enum<E>>
{
   /**
    * Called when the state machine is transitioning into this state.
    */
   void onEntry();

   /**
    * Called regularly by the state machine to allow the state to do any time-dependent internal processing.
    *
    * @return a new event to be fired in the state machine, or <code>null</code> if no event is to be fired
    */
   E process();

   /**
    * Called when the state machine is transitioning out of this state.
    */
   void onExit();
}


<|start_filename|>simulation-construction-set-tools/src/main/java/us/ihmc/simulationConstructionSetTools/util/inputdevices/XTouchChannelMapper.java<|end_filename|>
package us.ihmc.simulationConstructionSetTools.util.inputdevices;

public class XTouchChannelMapper implements MidiChannelMapper
{

   @Override
   public int getKnobChannel(int knob)
   {
      if(knob > 32 || knob < 1)
         throw new RuntimeException("Invalid knob");
      
      if(knob <= 16)
      {
         return knob + 9;   
      }
      else
      {
         return knob + 20;
      }
      }

   @Override
   public int getSliderChannel(int slider)
   {
      if(slider > 18 || slider < 1)
         throw new RuntimeException("Invalid slider");
      
      if(slider <= 9)
      {
         return slider;         
      }
      else
      {
         return slider + 18;
      }
   }

   @Override
   public int getButtonChannel(int button)
   {
      if(button > 78 || button < 1)
         throw new RuntimeException("Invalid button");

      if(button <= 39)
      {
         return button + 15 + 127;
      }
      else
      {
         return button + 31 + 127;
      }
   }

   @Override
   public int getKnobButtonChannel(int knob)
   {
      if(knob > 32 || knob < 1)
         throw new RuntimeException("Invalid knob button");
      if(knob <= 16)
      {
         return knob - 1 + 127;
      }
      else
      {
         return knob + 38 + 127;
      }
   }

}


<|start_filename|>ihmc-humanoid-behaviors/src/main/java/us/ihmc/humanoidBehaviors/communication/CoactiveDataListenerInterface.java<|end_filename|>
package us.ihmc.humanoidBehaviors.communication;

import controller_msgs.msg.dds.SimpleCoactiveBehaviorDataPacket;

public interface CoactiveDataListenerInterface
{
   public void coactiveDataRecieved(SimpleCoactiveBehaviorDataPacket data);
}


<|start_filename|>ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/converter/ClearDelayQueueConverter.java<|end_filename|>
package us.ihmc.humanoidRobotics.communication.controllerAPI.converter;

import java.util.List;

import controller_msgs.msg.dds.ClearDelayQueueMessage;
import gnu.trove.map.hash.TIntObjectHashMap;
import us.ihmc.communication.controllerAPI.CommandConversionInterface;
import us.ihmc.communication.controllerAPI.command.Command;
import us.ihmc.euclid.interfaces.Settable;
import us.ihmc.humanoidRobotics.communication.controllerAPI.command.ClearDelayQueueCommand;

public class ClearDelayQueueConverter implements CommandConversionInterface
{
   private final TIntObjectHashMap<Class<? extends Command<?, ?>>> hashCodeToCommandClasses = new TIntObjectHashMap<>();
   private final TIntObjectHashMap<Class<? extends Settable<?>>> hashCodeToMessageClasses = new TIntObjectHashMap<>();

   public ClearDelayQueueConverter(List<Class<? extends Command<?, ?>>> commandsToRegister) throws InstantiationException, IllegalAccessException
   {
      for (Class<? extends Command<?, ?>> commandToRegister : commandsToRegister)
      {
         Class<? extends Settable<?>> messageClass = commandToRegister.newInstance().getMessageClass();
         int hashCode = messageClass.getSimpleName().hashCode();
         hashCodeToCommandClasses.put(hashCode, commandToRegister);
         hashCodeToMessageClasses.put(hashCode, messageClass);
      }
   }

   @Override
   public <C extends Command<?, M>, M extends Settable<M>> boolean isConvertible(C command, M message)
   {
      return command instanceof ClearDelayQueueCommand && message instanceof ClearDelayQueueMessage;
   }

   @Override
   public <C extends Command<?, M>, M extends Settable<M>> void process(C command, M message)
   {
      ClearDelayQueueCommand clearCommand = (ClearDelayQueueCommand) command;
      ClearDelayQueueMessage clearMessage = (ClearDelayQueueMessage) message;
      clearCommand.setCommandClassToClear(hashCodeToCommandClasses.get(clearMessage.getClassSimpleNameBasedHashCode()));
      clearCommand.setMessageClassToClear(hashCodeToMessageClasses.get(clearMessage.getClassSimpleNameBasedHashCode()));
      clearCommand.setClearAllDelayBuffers(clearMessage.getClearAllDelayBuffers());
   }
}


<|start_filename|>ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/robotController/RawOutputWriter.java<|end_filename|>
package us.ihmc.robotics.robotController;

import us.ihmc.simulationconstructionset.util.RobotControlElement;

public interface RawOutputWriter extends RobotControlElement
{
   public abstract void write();
}


<|start_filename|>ihmc-sensor-processing/src/main/java/us/ihmc/sensorProcessing/outputData/LowLevelStateReadOnly.java<|end_filename|>
package us.ihmc.sensorProcessing.outputData;

public interface LowLevelStateReadOnly
{
   double getPosition();
   double getVelocity();
   double getAcceleration();
   double getEffort();
   boolean isPositionValid();
   boolean isVelocityValid();
   boolean isAccelerationValid();
   boolean isEffortValid();
}


<|start_filename|>ihmc-whole-body-controller/src/main/java/us/ihmc/wholeBodyController/diagnostics/CalibrationState.java<|end_filename|>
package us.ihmc.wholeBodyController.diagnostics;

import us.ihmc.robotics.stateMachine.core.State;
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;

public interface CalibrationState extends State
{
   public abstract JointDesiredOutputListReadOnly getOutputForLowLevelController();
}


<|start_filename|>ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HeightQuadTreeMessage.java<|end_filename|>
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 height quad tree module
       */
public class HeightQuadTreeMessage extends Packet<HeightQuadTreeMessage> implements Settable<HeightQuadTreeMessage>, EpsilonComparable<HeightQuadTreeMessage>
{
   /**
            * Unique ID used to identify this message, should preferably be consecutively increasing.
            */
   public long sequence_id_;
   public float default_height_;
   public float resolution_;
   public float size_x_;
   public float size_y_;
   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.HeightQuadTreeLeafMessage>  leaves_;

   public HeightQuadTreeMessage()
   {
      leaves_ = new us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.HeightQuadTreeLeafMessage> (5000, new controller_msgs.msg.dds.HeightQuadTreeLeafMessagePubSubType());

   }

   public HeightQuadTreeMessage(HeightQuadTreeMessage other)
   {
      this();
      set(other);
   }

   public void set(HeightQuadTreeMessage other)
   {
      sequence_id_ = other.sequence_id_;

      default_height_ = other.default_height_;

      resolution_ = other.resolution_;

      size_x_ = other.size_x_;

      size_y_ = other.size_y_;

      leaves_.set(other.leaves_);
   }

   /**
            * 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 void setDefaultHeight(float default_height)
   {
      default_height_ = default_height;
   }
   public float getDefaultHeight()
   {
      return default_height_;
   }

   public void setResolution(float resolution)
   {
      resolution_ = resolution;
   }
   public float getResolution()
   {
      return resolution_;
   }

   public void setSizeX(float size_x)
   {
      size_x_ = size_x;
   }
   public float getSizeX()
   {
      return size_x_;
   }

   public void setSizeY(float size_y)
   {
      size_y_ = size_y;
   }
   public float getSizeY()
   {
      return size_y_;
   }


   public us.ihmc.idl.IDLSequence.Object<controller_msgs.msg.dds.HeightQuadTreeLeafMessage>  getLeaves()
   {
      return leaves_;
   }


   public static Supplier<HeightQuadTreeMessagePubSubType> getPubSubType()
   {
      return HeightQuadTreeMessagePubSubType::new;
   }

   @Override
   public Supplier<TopicDataType> getPubSubTypePacket()
   {
      return HeightQuadTreeMessagePubSubType::new;
   }

   @Override
   public boolean epsilonEquals(HeightQuadTreeMessage other, double epsilon)
   {
      if(other == null) return false;
      if(other == this) return true;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.sequence_id_, other.sequence_id_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.default_height_, other.default_height_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.resolution_, other.resolution_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.size_x_, other.size_x_, epsilon)) return false;

      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.size_y_, other.size_y_, epsilon)) return false;

      if (this.leaves_.size() != other.leaves_.size()) { return false; }
      else
      {
         for (int i = 0; i < this.leaves_.size(); i++)
         {  if (!this.leaves_.get(i).epsilonEquals(other.leaves_.get(i), epsilon)) return false; }
      }


      return true;
   }

   @Override
   public boolean equals(Object other)
   {
      if(other == null) return false;
      if(other == this) return true;
      if(!(other instanceof HeightQuadTreeMessage)) return false;

      HeightQuadTreeMessage otherMyClass = (HeightQuadTreeMessage) other;

      if(this.sequence_id_ != otherMyClass.sequence_id_) return false;

      if(this.default_height_ != otherMyClass.default_height_) return false;

      if(this.resolution_ != otherMyClass.resolution_) return false;

      if(this.size_x_ != otherMyClass.size_x_) return false;

      if(this.size_y_ != otherMyClass.size_y_) return false;

      if (!this.leaves_.equals(otherMyClass.leaves_)) return false;

      return true;
   }

   @Override
   public java.lang.String toString()
   {
      StringBuilder builder = new StringBuilder();

      builder.append("HeightQuadTreeMessage {");
      builder.append("sequence_id=");
      builder.append(this.sequence_id_);      builder.append(", ");
      builder.append("default_height=");
      builder.append(this.default_height_);      builder.append(", ");
      builder.append("resolution=");
      builder.append(this.resolution_);      builder.append(", ");
      builder.append("size_x=");
      builder.append(this.size_x_);      builder.append(", ");
      builder.append("size_y=");
      builder.append(this.size_y_);      builder.append(", ");
      builder.append("leaves=");
      builder.append(this.leaves_);
      builder.append("}");
      return builder.toString();
   }
}
