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; } }
location = new Point3d(); orientation = new Quat4d(); footstep.getPositionInWorldFrame(location); footstep.getOrientationInWorldFrame(orientation);
private void printFootstepConstructor(Footstep footstep) { RobotSide robotSide = footstep.getRobotSide(); Point3d position = new Point3d(); footstep.getPositionInWorldFrame(position); Quat4d orientation = new Quat4d(); footstep.getOrientationInWorldFrame(orientation); System.out.println("footsteps.add(footstepProviderTestHelper.createFootstep(RobotSide." + robotSide + ", new Point3d(" + position.getX() + ", " + position.getY() + ", " + position.getZ() + "), new Quat4d(" + orientation.getW() + ", " + orientation.getX() + ", " + orientation.getY() + ", " + orientation.getZ() + ")));"); } }