public Footstep generateFootstepUsingHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, List<Point3D> pointList, double defaultHeight) { Footstep toReturn = generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, 0.0, new Vector3D(0.0, 0.0, 1.0)); snapFootstep(toReturn, pointList, defaultHeight); return toReturn; }
@Override public Footstep generateFootstepUsingHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, 0.0, new Vector3D(0.0, 0.0, 1.0)); snapFootstep(toReturn, heightMap); return toReturn; }
@Override public Footstep generateFootstepUsingHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, 0.0, new Vector3d(0.0, 0.0, 1.0)); snapFootstep(toReturn, heightMap); return toReturn; }
public Footstep generateFootstepUsingHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, List<Point3d> pointList, double defaultHeight) { Footstep toReturn = generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, 0.0, new Vector3d(0.0, 0.0, 1.0)); snapFootstep(toReturn, pointList, defaultHeight); return toReturn; }
@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) Footstep.FootstepType footstepFound = convexHullFootstepSnapper.snapFootstep(footstep, heightMap); footstepFound = convexHullFootstepSnapper.snapFootstep(footstep, heightMap);
@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) Footstep.FootstepType footstepFound = convexHullFootstepSnapper.snapFootstep(footstep, heightMap); footstepFound = convexHullFootstepSnapper.snapFootstep(footstep, heightMap);
@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 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(FootstepDataMessage footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstepFound = new FootstepDataMessage(footstep); Point3D position = originalFootstepFound.getLocation(); double yaw = originalFootstepFound.getOrientation().getYaw(); if (!useMask) { pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength()); } else { footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength(), footstepMask); } double height = heightMap.getHeightAtPoint(position.getX(), position.getY()); return snapFootstep(footstep, pointList, height); }
@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstepFound = new FootstepDataMessage(footstep); Point3d position = originalFootstepFound.getLocation(); double yaw = RotationTools.computeYaw(originalFootstepFound.getOrientation()); if (!useMask) { pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength()); } else { footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength(), footstepMask); } double height = heightMap.getHeightAtPoint(position.getX(), position.getY()); return snapFootstep(footstep, pointList, height); }
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; }
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; }