us.ihmc.humanoidRobotics.footstep.Footstep.getPredictedContactPoints 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
}

public void updateContactPointsForUpcomingFootstep(Footstep nextFootstep)
{
 RobotSide robotSide = nextFootstep.getRobotSide();
 List<Point2d> predictedContactPoints = nextFootstep.getPredictedContactPoints();
 if ((predictedContactPoints != null) && (!predictedContactPoints.isEmpty()))
 {
   setFootPlaneContactPoints(robotSide, predictedContactPoints);
 }
 else
 {
   resetFootPlaneContactPoint(robotSide);
 }
}

public void update(Footstep footstep)
{
 footstep.getSolePose(footstepPose);
 yoFootstepPose.setAndMatchFrame(footstepPose);
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 List<Point2d> contactPointsToVisualize;
 if (predictedContactPoints == null || predictedContactPoints.isEmpty())
   contactPointsToVisualize = defaultContactPointsInSoleFrame;
 else
   contactPointsToVisualize = predictedContactPoints;
 foothold.setAndUpdate(contactPointsToVisualize, contactPointsToVisualize.size());
 yoFoothold.setConvexPolygon2d(foothold);
 poseViz.update();
 footholdViz.update();
}

public AdjustFootstepMessage(Footstep footstep)
{
 uniqueId = VALID_MESSAGE_DEFAULT_ID;
 origin = FootstepOrigin.AT_ANKLE_FRAME;
 robotSide = footstep.getRobotSide();
 location = new Point3d();
 orientation = new Quat4d();
 footstep.getPositionInWorldFrame(location);
 footstep.getOrientationInWorldFrame(orientation);
 List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints();
 if (footstepContactPoints != null)
 {
   if (predictedContactPoints == null)
   {
    predictedContactPoints = new ArrayList<>();
   }
   else
   {
    predictedContactPoints.clear();
   }
   for (Point2d contactPoint : footstepContactPoints)
   {
    predictedContactPoints.add((Point2d) contactPoint.clone());
   }
 }
 else
 {
   predictedContactPoints = null;
 }
}

private void computeEntryCMPForFootstep(FramePoint entryCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot,
   YoFramePoint previousExitCMP)
{
 ReferenceFrame soleFrame = footstep.getSoleReferenceFrame();
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 RobotSide robotSide = footstep.getRobotSide();
 if (predictedContactPoints != null)
   tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints);
 else
   tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide));
 computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousExitCMP);
}

private void addPredictedContactPointsToPolygon(Footstep footstep, FrameConvexPolygon2d convexPolygonToExtend)
{
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null && !predictedContactPoints.isEmpty())
 {
   int numberOfContactPoints = predictedContactPoints.size();
   for (int i = 0; i < numberOfContactPoints; i++)
   {
    tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), predictedContactPoints.get(i));
    convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint);
   }
 }
 else
 {
   ConvexPolygon2d defaultPolygon = defaultFootPolygons.get(footstep.getRobotSide());
   for (int i = 0; i < defaultPolygon.getNumberOfVertices(); i++)
   {
    tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), defaultPolygon.getVertex(i));
    convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint);
   }
 }
}

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

for (int i = 0; i < footstepToAdjust.getPredictedContactPoints().size(); i++)
 contactPoints.add(footstepToAdjust.getPredictedContactPoints().get(i));
footstepToAdjust.setPredictedContactPointsFromPoint2ds(contactPoints);

footstep.getOrientationInWorldFrame(orientation);
List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints();
if (footstepContactPoints != null)

public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot)
 List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints();

public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot)
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();

private void computeExitCMPForFootstep(FramePoint exitCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfUpcomingSupportFoot,
   boolean isUpcomingFootstepLast)
{
 if (useTwoCMPsPerSupport)
 {
   ReferenceFrame soleFrame = footstep.getSoleReferenceFrame();
   List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
   RobotSide robotSide = footstep.getRobotSide();
   if (predictedContactPoints != null)
    tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints);
   else
    tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide));
   computeExitCMP(exitCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfUpcomingSupportFoot, isUpcomingFootstepLast);
 }
 else
 {
   exitCMPToPack.setToNaN(worldFrame);
 }
}

partialFootholdPolygon.addVertices(footstep.getPredictedContactPoints(), footstep.getPredictedContactPoints().size());
partialFootholdPolygon.update();

public static FramePoint getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 Point2d footstepCenter;
 
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null)
 {
   ConvexPolygon2d contactPolygon = new ConvexPolygon2d(predictedContactPoints);
   footstepCenter = contactPolygon.getCentroid();
 }
 else
 {
   footstepCenter = new Point2d();
 }
 
 footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter);
 footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 
 FramePoint footstepCenter3d = new FramePoint(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0);
 footstepCenter3d.changeFrame(worldFrame);
 return footstepCenter3d;
}

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

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

partialFootholdPolygon.addVertices(Vertex2DSupplier.asVertex2DSupplier(footstep.getPredictedContactPoints()));
partialFootholdPolygon.update();

assertTrue((generatedSnappedFootstep.getPredictedContactPoints() == null) || (generatedSnappedFootstep.getPredictedContactPoints().size() > 2));
boolean pointsBelowPlane = pointsBelowPlane(footstepSnapper.getPointList(), solePlane, tolerance);
assertTrue("queryPoint = " + queryPoint + " yaw = " + soleYaw.getDoubleValue() + " planeNormal = " + planeNormal + ", pointsBelowPlane = "

List<Point2d> predictedContactPoints = nextFootstep.getPredictedContactPoints();
if (predictedContactPoints != null && !predictedContactPoints.isEmpty())

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

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

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