us.ihmc.humanoidRobotics.footstep.Footstep.getFootstepPose java code examples

Add the Codota plugin to your IDE and get smart completions

private void myMethod () {
 BufferedReader b =InputStream in;new BufferedReader(new InputStreamReader(in))
Reader in;new BufferedReader(in)
File file;new BufferedReader(new FileReader(file))
Smart code suggestions by Tabnine
}

private void updateNextFootsteps(int stepIndex)
{
 int numberOfStepToUpdate =
    (numberOfFootstepsForTest - stepIndex) < numberOfFootstepsToConsider ? (numberOfFootstepsForTest - stepIndex) : numberOfFootstepsToConsider;
 int nextStepIndex;
 for (nextStepIndex = 0; nextStepIndex < numberOfStepToUpdate; nextStepIndex++)
 {
   nextFootstepPoses.get(nextStepIndex).set(footstepList.get(stepIndex + nextStepIndex).getFootstepPose());
 }
 for (; nextStepIndex < numberOfFootstepsToConsider; nextStepIndex++)
 {
   nextFootstepPoses.get(nextStepIndex).setToNaN();
 }
}

public FootstepDataListMessage generateFootstepsFromLocationsAndOrientations(RobotSide[] robotSides, double[][][] footstepLocationsAndOrientations, double swingTime, double transferTime)
{
 FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime);
 for (int i = 0; i < robotSides.length; i++)
 {
   RobotSide robotSide = robotSides[i];
   double[][] footstepLocationAndOrientation = footstepLocationsAndOrientations[i];
   Footstep footstep = generateFootstepFromLocationAndOrientation(robotSide, footstepLocationAndOrientation);
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(robotSide, footstep.getFootstepPose().getPosition(), footstep.getFootstepPose().getOrientation());
   footstepDataList.getFootstepDataList().add().set(footstepData);
 }
 return footstepDataList;
}

private boolean areFoostepsEqual(Footstep sentFootstep, Footstep receivedFootstep, double epsilon)
{
 boolean arePosesEqual = sentFootstep.getFootstepPose().epsilonEquals(receivedFootstep.getFootstepPose(), epsilon);
 boolean sameRobotSide = sentFootstep.getRobotSide() == receivedFootstep.getRobotSide();
 boolean isTrustHeightTheSame = sentFootstep.getTrustHeight() == receivedFootstep.getTrustHeight();
 // FIXME the field is set in FootstepDataListMessage only, which makes impossible to properly convert messages to footsteps.
 boolean isAdjustableTheSame = true; //sentFootstep.getIsAdjustable() == receivedFootstep.getIsAdjustable();
 boolean sameWaypoints = sentFootstep.getCustomPositionWaypoints().size() == receivedFootstep.getCustomPositionWaypoints().size();
 if (sameWaypoints)
 {
   for (int i = 0; i < sentFootstep.getCustomPositionWaypoints().size(); i++)
   {
    FramePoint3D waypoint = sentFootstep.getCustomPositionWaypoints().get(i);
    FramePoint3D otherWaypoint = receivedFootstep.getCustomPositionWaypoints().get(i);
    sameWaypoints = sameWaypoints && waypoint.epsilonEquals(otherWaypoint, epsilon);
   }
 }
 boolean sameBlendDuration = MathTools.epsilonEquals(sentFootstep.getSwingTrajectoryBlendDuration(), receivedFootstep.getSwingTrajectoryBlendDuration(), epsilon);
 return arePosesEqual && sameRobotSide && isTrustHeightTheSame && isAdjustableTheSame && sameWaypoints && sameBlendDuration;
}

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;
}

@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);
}

private void updateContactState(int currentStepCount, double percentageOfPhase)
{
 if (inDoubleSupport.getBooleanValue())
 {
   for (RobotSide side : RobotSide.values)
    contactStates.get(side).setFullyConstrained();
 }
 else
 {
   RobotSide swingSide = footstepList.get(currentStepCount).getRobotSide();
   contactStates.get(swingSide).clear();
   contactStates.get(swingSide.getOppositeSide()).setFullyConstrained();
   if (currentStepCount > 0)
   {
    swingFootPose.set(footstepList.get(currentStepCount - 1).getFootstepPose());
   }
   else
   {
    swingFootPose.set(initialPose);
    swingFootPose.appendTranslation(0.0, swingSide.negateIfRightSide(stepWidth / 2.0), 0.0);
   }
   FramePose3D endOfSwing = footstepList.get(currentStepCount).getFootstepPose();
   swingFootPose.interpolate(endOfSwing, percentageOfPhase);
   FootSpoof foot = feet.get(swingSide);
   foot.setSoleFrame(swingFootPose);
 }
 bipedSupportPolygons.updateUsingContactStates(contactStates);
}

@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;
}

@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;
}

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;
}

for (int numberOfStepsFromTarget = 0; numberOfStepsFromTarget <= numberOfFootstepsBetweenStartAndTarget; numberOfStepsFromTarget++)
  FramePose2D currentFootstepPose = new FramePose2D(footsteps.get(numberOfFootsteps - numberOfStepsFromTarget - 1).getFootstepPose());
  assertEquals("Current footstep orientation does not match end orientation.", 0.0, currentFootstepPose.getOrientationDistance(targetMidFeetPose2d),
        ORIENTATION_THRESHOLD);

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;
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testConstructor()
{
 FootstepData testObject = new FootstepData();
 assertTrue(testObject != null);
 Footstep testFootstep = new Footstep(RobotSide.LEFT);
 FootstepTiming testFootstepTiming = new FootstepTiming(0.2, 0.1);
 testFootstepTiming.setAbsoluteTime(0.1, 0.5);
 testObject.set(testFootstep, testFootstepTiming);
 assertTrue(testObject.getFootstep() == testFootstep);
 assertTrue(testObject.getStepTime() == testFootstepTiming.getStepTime());
 assertTrue(testObject.getSwingSide() == testFootstep.getRobotSide());
 assertTrue(testObject.getSupportSide() == testFootstep.getRobotSide().getOppositeSide());
 assertTrue(testObject.getSwingTime() == testFootstepTiming.getSwingTime());
 assertTrue(testObject.getTransferTime() == testFootstepTiming.getTransferTime());
 assertTrue(MathTools.epsilonEquals(testObject.getTransferStartTime(), testFootstepTiming.getExecutionStartTime(), Epsilons.ONE_BILLIONTH));
 assertTrue(testObject.getSwingStartTime() == testFootstepTiming.getSwingStartTime());
 assertTrue(testObject.getFootstepPoseReferenceFrame() == testFootstep.getFootstepPose().getReferenceFrame());
 assertTrue(testObject.getFramePose() == testFootstep.getFootstepPose());
 testObject.setSwingTime(0.8);
 assertTrue(testObject.getSwingTime() == 0.8);
 Footstep newFootstep = new Footstep(RobotSide.LEFT);
 testObject.setFootstep(newFootstep);
 assertTrue(testObject.getFootstep() == newFootstep);
}

for (Footstep footstep : footsteps)
  FramePose2D currentFootstepPose = new FramePose2D(footstep.getFootstepPose());
  if (currentFootstepPose.getOrientationDistance(startTargetMidPose2dMean) < ORIENTATION_THRESHOLD)
   numberOfStepsAlignedWithMeanOrientation++;

@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;
}

FramePoint2D transferFromFootPosition = new FramePoint2D(transferFromFootstep.getFootstepPose().getPosition());
FramePoint2D transferToFootPosition = new FramePoint2D(transferToFootstep.getFootstepPose().getPosition());

for (Footstep footstep : walkToLocationBehavior.getFootSteps())
  FramePose2D currentFootstepPose = new FramePose2D(footstep.getFootstepPose());
  assertEquals("Current footstep orientation does not match start orientation.", 0.0, currentFootstepPose.getOrientationDistance(startMidFeetPose2d),
        ORIENTATION_THRESHOLD);

Popular methods of Footstep

Popular in Java

Reading from database using SQL prepared statement
findViewById (Activity)
notifyDataSetChanged (ArrayAdapter)
getSystemService (Context)
ServerSocket (java.net)
This class represents a server-side socket that waits for incoming client connections. A ServerSocke
SimpleDateFormat (java.text)
Formats and parses dates in a locale-sensitive manner. Formatting turns a Date into a String, and pa
ArrayList (java.util)
ArrayList is an implementation of List, backed by an array. All optional operations including adding
HashMap (java.util)
HashMap is an implementation of Map. All optional operations are supported.All elements are permitte
Modifier (javassist)
The Modifier class provides static methods and constants to decode class and member access modifiers
Annotation (javassist.bytecode.annotation)
The annotation structure.An instance of this class is returned bygetAnnotations() in AnnotationsAttr
Top 17 PhpStorm Plugins

How to use getFootstepPosemethodin us.ihmc.humanoidRobotics.footstep.Footstep

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

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