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

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

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

origin: us.ihmc/CommonWalkingControlModules

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);
 }
}
origin: us.ihmc/CommonWalkingControlModules

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();
}
origin: us.ihmc/IHMCHumanoidRobotics

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;
 }
}
origin: us.ihmc/CommonWalkingControlModules

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);
}
origin: us.ihmc/CommonWalkingControlModules

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);
   }
 }
}
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/CommonWalkingControlModules

for (int i = 0; i < footstepToAdjust.getPredictedContactPoints().size(); i++)
 contactPoints.add(footstepToAdjust.getPredictedContactPoints().get(i));
footstepToAdjust.setPredictedContactPointsFromPoint2ds(contactPoints);
origin: us.ihmc/IHMCHumanoidRobotics

footstep.getOrientationInWorldFrame(orientation);
List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints();
if (footstepContactPoints != null)
origin: us.ihmc/ihmc-humanoid-robotics

public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot)
 List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints();
origin: us.ihmc/IHMCHumanoidRobotics

public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot)
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
origin: us.ihmc/CommonWalkingControlModules

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);
 }
}
origin: us.ihmc/IHMCAvatarInterfaces

partialFootholdPolygon.addVertices(footstep.getPredictedContactPoints(), footstep.getPredictedContactPoints().size());
partialFootholdPolygon.update();
origin: us.ihmc/IHMCHumanoidRobotics

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

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;
}
origin: us.ihmc/ihmc-avatar-interfaces

partialFootholdPolygon.addVertices(Vertex2DSupplier.asVertex2DSupplier(footstep.getPredictedContactPoints()));
partialFootholdPolygon.update();
origin: us.ihmc/ihmc-simulation-toolkit-test

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

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

Popular methods of Footstep

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

Popular in Java

  • Parsing JSON documents to java classes using gson
  • getSupportFragmentManager (FragmentActivity)
  • requestLocationUpdates (LocationManager)
  • runOnUiThread (Activity)
  • FileInputStream (java.io)
    An input stream that reads bytes from a file. File file = ...finally if (in != null) in.clos
  • UnknownHostException (java.net)
    Thrown when a hostname can not be resolved.
  • Connection (java.sql)
    A connection represents a link from a Java application to a database. All SQL statements and results
  • ConcurrentHashMap (java.util.concurrent)
    A plug-in replacement for JDK1.5 java.util.concurrent.ConcurrentHashMap. This version is based on or
  • SSLHandshakeException (javax.net.ssl)
    The exception that is thrown when a handshake could not be completed successfully.
  • DataSource (javax.sql)
    An interface for the creation of Connection objects which represent a connection to a database. This
  • 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