@Override public Footstep generateFootstepUsingHeightMap(FramePose2D desiredSolePosition, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = convexHullFootstepSnapper.generateFootstepUsingHeightMap(desiredSolePosition, foot, soleFrame, robotSide, heightMap); snapFootstep(toReturn, heightMap); return toReturn; }
private AdjustingFootstepSnapper createAdjustingFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); AdjustingFootstepSnapper footstepSnapper = new AdjustingFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); return footstepSnapper; }
@Override public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { FramePose2D footPose2d = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(soleX, soleY), yaw); return generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap); }
@Override public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { FramePose2d footPose2d = new FramePose2d(ReferenceFrame.getWorldFrame(), new Point2d(soleX, soleY), yaw); return generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap); }
@Override public Footstep generateFootstepUsingHeightMap(FramePose2d desiredSolePosition, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { Footstep toReturn = convexHullFootstepSnapper.generateFootstepUsingHeightMap(desiredSolePosition, foot, soleFrame, robotSide, heightMap); snapFootstep(toReturn, heightMap); return toReturn; }
@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; }