public double getSwingHeight(Footstep initialFootstep, Footstep stanceFootstep, Footstep endFootstep, HeightMapWithPoints heightMap) { FramePose startPose = new FramePose(); initialFootstep.getSolePose(startPose); FramePose endPose = new FramePose(); endFootstep.getSolePose(endPose); Point3d position = new Point3d(); stanceFootstep.getPosition(position); return getSwingHeight(startPose, endPose, position.getZ(), heightMap); }
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(); }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){ FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints() != null){ throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3d planeNormal) { Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); double yaw = RotationTools.computeYaw(orientation); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), new Point3d(position), orientation)); }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints() != null) { throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
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); FramePose leftPose = new FramePose(); FramePose rightPose = new FramePose(); left.getSolePose(leftPose); right.getSolePose(rightPose); FramePose2d leftPose2d = new FramePose2d(); FramePose2d rightPose2d = new FramePose2d(); leftPose.getPose2dIncludingFrame(leftPose2d); rightPose.getPose2dIncludingFrame(rightPose2d); startPose.interpolate(leftPose2d, rightPose2d, 0.5); Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose); leftPose.changeFrame(startFramePose); rightPose.changeFrame(startFramePose); FrameOrientation2d leftOrientation = new FrameOrientation2d(); FrameOrientation2d rightOrientation = new FrameOrientation2d(); leftPose.getOrientation2dIncludingFrame(leftOrientation); rightPose.getOrientation2dIncludingFrame(rightOrientation); initialDeltaFeetYaw = leftOrientation.sub(rightOrientation); initialDeltaFeetY = leftPose.getY() - rightPose.getY(); initialDeltaFeetX = leftPose.getX() - rightPose.getX(); }
public void setFootstep(Footstep footstep, double swingTime) footstep.getSolePose(footstepSolePose);
footstep.getSolePose(swingEndPose);
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){ FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3d> pointList, double defaultHeight) { FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight); footstep.setFootstepType(type); footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }