public Footstep(String id, RigidBody endEffector, RobotSide robotSide, ReferenceFrame soleFrame, PoseReferenceFrame poseReferenceFrame, boolean trustHeight, List<Point2d> predictedContactPoints, TrajectoryType trajectoryType, double swingHeight) { poseReferenceFrame.getParent().checkIsWorldFrame(); this.id = id; this.endEffector = endEffector; this.robotSide = robotSide; this.ankleReferenceFrame = poseReferenceFrame; endEffector.getParentJoint().getFrameAfterJoint().getTransformToDesiredFrame(transformFromAnkleToSoleFrame, soleFrame); this.soleReferenceFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("soleFrame", poseReferenceFrame, transformFromAnkleToSoleFrame); this.trustHeight = trustHeight; setPredictedContactPointsFromPoint2ds(predictedContactPoints); this.trajectoryType = trajectoryType; this.swingHeight = swingHeight; }
for (int i = 0; i < footstepToAdjust.getPredictedContactPoints().size(); i++) contactPoints.add(footstepToAdjust.getPredictedContactPoints().get(i)); footstepToAdjust.setPredictedContactPointsFromPoint2ds(contactPoints);
@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) { 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){ 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; }