Tabnine Logo
Footstep.getPose
Code IndexAdd Tabnine to your IDE (free)

How to use
getPose
method
in
us.ihmc.humanoidRobotics.footstep.Footstep

Best Java code snippets using us.ihmc.humanoidRobotics.footstep.Footstep.getPose (Showing top 20 results out of 315)

origin: us.ihmc/ihmc-humanoid-robotics-test

private void checkFootMeetsAngleRequirement(Footstep stance, Footstep swingEnd)
{
 FramePose3D stancePose = new FramePose3D();
 stance.getPose(stancePose);
 FramePose3D swingEndPose = new FramePose3D();
 swingEnd.getPose(swingEndPose);
 FrameOrientation2D stanceOrientation = new FrameOrientation2D(stancePose.getOrientation());
 FrameOrientation2D swingOrientation = new FrameOrientation2D(swingEndPose.getOrientation());
 double yawDifference = swingOrientation.difference(stanceOrientation);
 double allowedDifference = (Math.abs(yawDifference) - footTwistLimitInRadians) - 3e-16;
 if (allowedDifference > 0)
   valid = false;
}
origin: us.ihmc/ihmc-simulation-toolkit-test

private void assertFootstepInAngleRange(String testDescription, Footstep footstep, double startYaw, double endYaw)
{
 FramePose3D footPose = new FramePose3D();
 footstep.getPose(footPose);
 FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation());
 footOrientation.changeFrame(WORLD_FRAME);
 double footYaw = footOrientation.getYaw();
 double angleBetweenStartAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, startYaw));
 double angleBetweenStartAndFoot = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(footYaw, startYaw));
 double angleBetweenFootAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, footYaw));
 //if start->foot + foot-> end > start->end, then the foot angle is outside the range between the two
 boolean outsideFan = (angleBetweenStartAndFoot + angleBetweenFootAndEnd) > angleBetweenStartAndEnd + 1e-14;
}
origin: us.ihmc/ihmc-simulation-toolkit-test

private void assertNoRepeatedSquareUpSteps(String message, List<Footstep> footsteps)
{
 int numFootsteps = footsteps.size();
 if (numFootsteps < 4) // True step in place should only have two footsteps and that should be OK.
   return;
 // Assuming last steps are squared up: shouldn't match steps two before.
 for (int i = numFootsteps - 1; i > numFootsteps - 3; i--)
 {
   Footstep lastFootstep = footsteps.get(i);
   Footstep compareToLastFootstep = footsteps.get(i - 2);
   FramePose3D lastFootstepPose = new FramePose3D();
   lastFootstep.getPose(lastFootstepPose);
   FramePose3D compareToLastFootstepPose = new FramePose3D();
   compareToLastFootstep.getPose(compareToLastFootstepPose);
   FramePoint2D lastFootstepPosition2d = new FramePoint2D(lastFootstepPose.getPosition());
   FramePoint2D compareToLastFootstepPosition2d = new FramePoint2D(compareToLastFootstepPose.getPosition());
   double eps = 1e-14;
   assertTrue(
      message + " step " + i + " and " + (i - 2),
      (compareToLastFootstepPosition2d.distance(lastFootstepPosition2d) > eps)
         || (Math.abs(lastFootstepPose.getYaw() - compareToLastFootstepPose.getYaw()) > eps));
 }
}
origin: us.ihmc/CommonWalkingControlModules

@Override public boolean getUpcomingFootstepSolution(Footstep footstepToPack)
{
 if (icpOptimizationController.getNumberOfFootstepsToConsider() > 0)
 {
   footstepToPack.getPose(footstepPose);
   icpOptimizationController.getFootstepSolution(0, footstepPositionSolution);
   footstepPose.setXYFromPosition2d(footstepPositionSolution);
   footstepToPack.setPose(footstepPose);
 }
 return icpOptimizationController.wasFootstepAdjusted();
}
origin: us.ihmc/ihmc-avatar-interfaces-test

private FootstepDataListMessage createFootstepDataList(ArrayList<Footstep> desiredFootsteps)
{
 FootstepDataListMessage ret = new FootstepDataListMessage();
 for (int i = 0; i < desiredFootsteps.size(); i++)
 {
   Footstep footstep = desiredFootsteps.get(i);
   FramePoint3D position = new FramePoint3D();
   FrameQuaternion orientation = new FrameQuaternion();
   footstep.getPose(position, orientation);
   RobotSide footstepSide = footstep.getRobotSide();
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation);
   ret.getFootstepDataList().add().set(footstepData);
 }
 return ret;
}
origin: us.ihmc/ihmc-simulation-toolkit-test

private void assertNoCourseRepeatedSquareUpSteps(String message, ArrayList<Footstep> footstepsOriginal, double startX, double startY, double startYaw,
   double eps, int shortSteps)
{
 SideDependentList<Footstep> startFeet = updatedStartFeet(startX, startY, startYaw);
 ArrayList<Footstep> footsteps = prependStanceToFootstepQueue(footstepsOriginal, startFeet);
 int numFootsteps = footsteps.size();
 for (int i = 0; i + 2 < numFootsteps; i++)
 {
   Footstep firstFootstep = footsteps.get(i);
   Footstep nextFootstep = footsteps.get(i + 2);
   FramePose3D firstFootstepPose = new FramePose3D();
   firstFootstep.getPose(firstFootstepPose);
   FramePose3D nextFootstepPose = new FramePose3D();
   nextFootstep.getPose(nextFootstepPose);
   FramePoint2D firstFootstepPosition2d = new FramePoint2D(firstFootstepPose.getPosition());
   FramePoint2D nextFootstepPosition2d = new FramePoint2D(nextFootstepPose.getPosition());
   double testeps = eps;
   String message2 = message + "course restep test " + i + "to" + (i + 2);
   boolean condition = (nextFootstepPosition2d.distance(firstFootstepPosition2d) > testeps)
      || (Math.abs(firstFootstepPose.getYaw() - nextFootstepPose.getYaw()) > testeps);
   Visualization visualize = (condition == true) ? Visualization.NO_VISUALIZATION : Visualization.VISUALIZE;
   showFootstepsIfVisualize(message2, footstepsOriginal, startFeet, visualize);
   assertTrue(message2, condition);
 }
}
origin: us.ihmc/ihmc-humanoid-robotics-test

private FrameVector3D offCenterVectorToSwingEnd(Footstep stance, Footstep swingEnd, FrameVector3D translationFromFootCenterToCircleCenter)
{
 FramePoint3D circleCenter;
 FramePose3D stanceSole = new FramePose3D();
 FramePose3D swingEndSole = new FramePose3D();
 stance.getPose(stanceSole);
 swingEnd.getPose(swingEndSole);
 circleCenter = new FramePoint3D(stanceSole.getPosition());
 circleCenter.add(translationFromFootCenterToCircleCenter);//change frame of translation first?
 FrameVector3D vectorToSwingEnd = new FrameVector3D(ReferenceFrame.getWorldFrame());
 vectorToSwingEnd.set(swingEndSole.getPosition());
 vectorToSwingEnd.sub(circleCenter);
 return vectorToSwingEnd;
}
origin: us.ihmc/ihmc-humanoid-behaviors

public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime)
{
 FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime);
 for (int i = 0; i < footsteps.size(); i++)
 {
   Footstep footstep = footsteps.get(i);
   FramePoint3D position = new FramePoint3D();
   FrameQuaternion orientation = new FrameQuaternion();
   footstep.getPose(position, orientation);
   RobotSide footstepSide = footstep.getRobotSide();
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation);
   footstepDataList.getFootstepDataList().add().set(footstepData);
 }
 set(footstepDataList);
}
origin: us.ihmc/ihmc-humanoid-robotics

public static AdjustFootstepMessage createAdjustFootstepMessage(Footstep footstep)
{
 AdjustFootstepMessage message = new AdjustFootstepMessage();
 message.setRobotSide(footstep.getRobotSide().toByte());
 FramePoint3D location = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(location, orientation);
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 message.getLocation().set(location);
 message.getOrientation().set(orientation);
 MessageTools.copyData(footstep.getPredictedContactPoints().stream().map(Point3D::new).collect(Collectors.toList()),
            message.getPredictedContactPoints2d());
 return message;
}
origin: us.ihmc/DarpaRoboticsChallenge

footstep.getPose(footstepPose);
origin: us.ihmc/ihmc-humanoid-robotics-test

FramePose3D stanceFramePose = new FramePose3D();
FramePose3D endingFramePose = new FramePose3D();
swingStart.getPose(initialFramePose);
stance.getPose(stanceFramePose);
swingEnd.getPose(endingFramePose);
origin: us.ihmc/ihmc-simulation-toolkit-test

private static void assertStepIsPointingCorrectly(String message, Footstep footstep, FrameOrientation2D endOrientation)
{
 FramePose3D footPose = new FramePose3D();
 footstep.getPose(footPose);
 // System.out.println(Math.toDegrees(endOrientation.getYaw()) + "?=" + Math.toDegrees(footPose.getYaw()));
 // System.out.println(Math.toDegrees(footPose.getRoll()) + "?=" + Math.toDegrees(footPose.getPitch()) + "?=" + 0.0);
 assertEquals(message + " Foot roll should be 0.", 0.0, footPose.getRoll(), 1e-10);
 assertEquals(message + " Foot pitch should be 0.", 0.0, footPose.getPitch(), 1e-10);
 FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation());
 double yawDiff = endOrientation.difference(footOrientation);
 assertEquals(message + " Foot yaw and desired yaw difference should be 0.", 0.0, yawDiff, 1e-10);
}
origin: us.ihmc/CommonWalkingControlModules

private int checkForEndingOfAdjustment(double omega0)
{
 int numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue());
 if (timeRemainingInState.getDoubleValue() < remainingTimeToStopAdjusting.getDoubleValue() && localUseStepAdjustment && !isStanding.getBooleanValue() && !isInTransfer.getBooleanValue())
 {
   //record current locations
   for (int i = 0; i < numberOfFootstepsToConsider; i++)
   {
    upcomingFootstepLocations.get(i).set(footstepSolutions.get(i));
    Footstep footstep = upcomingFootsteps.get(i);
    footstep.getPose(footstepPose);
    footstepPose.setXYFromPosition2d(footstepSolutions.get(i).getFrameTuple2d());
    footstep.setPose(footstepPose);
    inputHandler.addFootstepToPlan(footstep);
   }
   localUseStepAdjustment = false;
   numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue());
   footstepRecursionMultiplierCalculator.resetTimes();
   footstepRecursionMultiplierCalculator.submitTimes(0, 0.0, singleSupportDuration.getDoubleValue());
   for (int i = 1; i < numberOfFootstepsToConsider + 1; i++)
    footstepRecursionMultiplierCalculator.submitTimes(i, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
   footstepRecursionMultiplierCalculator.submitTimes(numberOfFootstepsToConsider + 1, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue());
   footstepRecursionMultiplierCalculator.computeRecursionMultipliers(numberOfFootstepsToConsider, isInTransfer.getBooleanValue(), localUseTwoCMPs, omega0);
 }
 return numberOfFootstepsToConsider;
}
origin: us.ihmc/ihmc-humanoid-robotics

private void calculateInitialFootPoseAndOffsets()
{
 SideDependentList<Footstep> currentFootsteps;
 if (priorStanceFeetSpecified)
   currentFootsteps = priorStanceFeet;
 else
   currentFootsteps = createFootstepsFromBipedFeet();
 Footstep left = currentFootsteps.get(RobotSide.LEFT);
 Footstep right = currentFootsteps.get(RobotSide.RIGHT);
 FramePose3D leftPose = new FramePose3D();
 FramePose3D rightPose = new FramePose3D();
 left.getPose(leftPose);
 right.getPose(rightPose);
 FramePose2D leftPose2d = new FramePose2D(leftPose);
 FramePose2D rightPose2d = new FramePose2D(rightPose);
 startPose.interpolate(leftPose2d, rightPose2d, 0.5);
 Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose);
 leftPose.changeFrame(startFramePose);
 rightPose.changeFrame(startFramePose);
 FrameOrientation2D leftOrientation = new FrameOrientation2D(leftPose.getOrientation());
 FrameOrientation2D rightOrientation = new FrameOrientation2D(rightPose.getOrientation());
 initialDeltaFeetYaw = leftOrientation.difference(rightOrientation);
 initialDeltaFeetY = leftPose.getY() - rightPose.getY();
 initialDeltaFeetX = leftPose.getX() - rightPose.getX();
}
origin: us.ihmc/ihmc-humanoid-robotics

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0){
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 footstep.setFootstepType(type);
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}
origin: us.ihmc/ihmc-humanoid-robotics

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0 )
 {
   throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points");
 }
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 footstep.setFootstepType(type);
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}
origin: us.ihmc/ihmc-humanoid-robotics

@Override
public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FramePose3D footstepPose = new FramePose3D();
 footstep.getPose(footstepPose);
 Point3D position = new Point3D(footstepPose.getPosition());
 double yaw = footstep.getFootstepPose().getYaw();
 Quaternion orientation = new Quaternion();
 RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation);
 position.setZ(height);
 footstepPose.set(new Point3D(position), orientation);
 footstep.setPose(footstepPose);
}
origin: us.ihmc/ihmc-humanoid-robotics

public static FootstepDataMessage createFootstepDataMessage(Footstep footstep)
{
 FootstepDataMessage message = new FootstepDataMessage();
 message.setRobotSide(footstep.getRobotSide().toByte());
 FramePoint3D location = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(location, orientation);
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 message.getLocation().set(location);
 message.getOrientation().set(orientation);
 packPredictedContactPoints(footstep.getPredictedContactPoints(), message);
 message.setTrajectoryType(footstep.getTrajectoryType().toByte());
 message.setSwingHeight(footstep.getSwingHeight());
 message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration());
 if (footstep.getCustomPositionWaypoints().size() != 0)
 {
   for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++)
   {
    FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i);
    framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
    message.getCustomPositionWaypoints().add().set(framePoint);
   }
 }
 return message;
}
origin: us.ihmc/ihmc-humanoid-robotics

@Override
public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap);
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 footstep.setFootstepType(type);
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}
origin: us.ihmc/ihmc-humanoid-robotics

public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3D> pointList, double defaultHeight)
{
 // can only snap footsteps in world frame
 footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
 FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep);
 //set to the sole pose
 FramePoint3D position = new FramePoint3D();
 FrameQuaternion orientation = new FrameQuaternion();
 footstep.getPose(position, orientation);
 originalFootstep.getLocation().set(position);
 originalFootstep.getOrientation().set(orientation);
 //get the footstep
 Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight);
 footstep.setFootstepType(type);
 footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep));
 FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation());
 footstep.setPose(solePoseInWorld);
 footstep.setSwingHeight(originalFootstep.getSwingHeight());
 footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType()));
 return type;
}
us.ihmc.humanoidRobotics.footstepFootstepgetPose

Popular methods of Footstep

  • getRobotSide
  • <init>
  • getSoleReferenceFrame
  • getPredictedContactPoints
  • getZ
  • getPosition
  • getSolePose
  • setSwingHeight
  • setTrajectoryType
  • setZ
  • getFootstepPose
  • getFootstepType
  • getFootstepPose,
  • getFootstepType,
  • getSwingHeight,
  • getTrajectoryType,
  • setPose,
  • getBody,
  • getCustomPositionWaypoints,
  • getOrientation,
  • getOrientationInWorldFrame

Popular in Java

  • Updating database using SQL prepared statement
  • setRequestProperty (URLConnection)
  • getExternalFilesDir (Context)
  • addToBackStack (FragmentTransaction)
  • Kernel (java.awt.image)
  • HttpURLConnection (java.net)
    An URLConnection for HTTP (RFC 2616 [http://tools.ietf.org/html/rfc2616]) used to send and receive d
  • Socket (java.net)
    Provides a client-side TCP socket.
  • PriorityQueue (java.util)
    A PriorityQueue holds elements on a priority heap, which orders the elements according to their natu
  • Vector (java.util)
    Vector is an implementation of List, backed by an array and synchronized. All optional operations in
  • JarFile (java.util.jar)
    JarFile is used to read jar entries and their associated data from jar files.
  • Top Vim plugins
Tabnine Logo
  • Products

    Search for Java codeSearch for JavaScript code
  • IDE Plugins

    IntelliJ IDEAWebStormVisual StudioAndroid StudioEclipseVisual Studio CodePyCharmSublime TextPhpStormVimGoLandRubyMineEmacsJupyter NotebookJupyter LabRiderDataGripAppCode
  • Company

    About UsContact UsCareers
  • Resources

    FAQBlogTabnine AcademyTerms of usePrivacy policyJava Code IndexJavascript Code Index
Get Tabnine for your IDE now